text:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_Prosix_C3_A601C(robot, T)
%   Solves the inverse kinematic problem for the EPSON Prosix_C3_A601C robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC__Prosix_C3_A601C returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   epson=load_robot('EPSON', 'Prosix_C3_A601C');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(epson, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(epson, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(epson, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function [q] = inversekinematic_Prosix_C3_A601C(robot, T)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry at the reference for this robot
L6=d(6);

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);

q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);

q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%Arrange solutions, there are 8 possible solutions so far.
% if q1 is a solution, q1* = q1 + pi is also a solution.
% For each (q1, q1*) there are two possible solutions
% for q2 and q3 (namely, elbow up and elbow up solutions)
% So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3),
% there exist two more possible solutions for the last three joints, generally
% called wrist up and wrist down solutions. For this reason,
%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    % use solve_spherical_wrist2 for the particular orientation
    % of the systems in this ABB robot
    % use either the geometric or algebraic method.
    % the function solve_spherical_wrist2 is used due to the relative
    % orientation of the last three DH reference systems.

    %This robot uses a different function to compute the last three angles,
    %since the relative orientation of the systems S4, S5 and S6 differs
    %from that of the rest of the robots
    qtemp = solve_spherical_wrist_Prosix(robot, q(:,i), T, 1); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist_Prosix(robot, q(:,i), T, -1); %wrist down

    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=a(2);
L3=d(4);

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = (acos((L2^2+r^2-L3^2)/(2*r*L2)));

if ~isreal(gamma)
    disp('WARNING:inversekinematic_Prosix_C3_A601C: the point is not reachable for this configuration, imaginary solutions');
    gamma = real(gamma);
end

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=a(2);
L3=d(4);

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

eta = (acos((L2^2 + L3^2 - r^2)/(2*L2*L3)));

if ~isreal(eta)
   disp('WARNING:inversekinematic_Prosix_C3_A601C: the point is not reachable for this configuration, imaginary solutions');
   eta = real(eta);
end

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = -(pi/2 - eta);
q3(2) = -(eta - 3*pi/2);

% Solve the special case of this spherical wrist
% For wrists that whose reference systems have been placed as in the
% ABB IRB 140--> use solve_spherical_wrist2
% For wrists with the same orientation as in the KUKA KR30_jet
%--> use solve_spherical_wrist
function q = solve_spherical_wrist_Prosix(robot, q, T, wrist)

% T is the noa matrix defining the position/orientation of the end
% effector's reference system
vx6=T(1:3,1);
vz5=T(1:3,3); % The vector a z6=T(1:3,3) is coincident with z5

% Obtain the position and orientation of the system 3
% using the already computed joints q1, q2 and q3
T01=dh(robot, q, 1);
T12=dh(robot, q, 2);
T23=dh(robot, q, 3);
T03=T01*T12*T23;

vx3=T03(1:3,1);
vy3=T03(1:3,2);
vz3=T03(1:3,3);

% find z4 normal to the plane formed by z3 and a
vz4=cross(vz3, vz5);	% end effector's vector a: T(1:3,3)

% in case of degenerate solution,
% when vz3 and vz6 are parallel--> then z4=0 0 0, choose q(4)=0 as solution
if norm(vz4) <= 0.00000001
    if wrist == 1 %wrist up
        q(4)=0;
    else
        q(4)=-pi; %wrist down
    end
else
    %this is the normal and most frequent solution
    cosq4=wrist*dot(vy3,vz4);
    sinq4=wrist*dot(-vx3,vz4);
    q(4)=atan2(sinq4, cosq4);
end
%propagate the value of q(4) to compute the system 4
T34=dh(robot, q, 4);
T04=T03*T34;
vx4=T04(1:3,1);
vy4=T04(1:3,2);

% solve for q5
cosq5=dot(-vy4,vz5);
sinq5=dot(vx4,vz5);
q(5)=atan2(sinq5, cosq5);

%propagate now q(5) to compute T05
T45=dh(robot, q, 5);
T05=T04*T45;
vx5=T05(1:3,1);
vy5=T05(1:3,2);

% solve for q6
cosq6=dot(vx6,vx5);
sinq6=dot(vx6,vy5);
q(6)=atan2(sinq6, cosq6);

repo: ARTE


text:
function [ know, x ] = p11_sol ( )

%*****************************************************************************80
%
%% P11_SOL returns the solution for problem 11.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, integer KNOW.
%    If KNOW is 0, then the solution is not known.
%    If KNOW is positive, then the solution is known, and is returned in X.
%
%    Output, real X, the solution, if known.
%
  know = 1;

  x = 1.0 / 3.0;

  return
end

author: johannesgerer

repo: jburkardt-m


text:
function [listRxns] = findRxnsPerTypeInMap(map, rxnType)
% Finds reaction names based on the type of reactions in the map. Useful
% to look for transport, catalysis or simple state_transition.
%
% USAGE:
%
%    [listRxns] = findTransRxns(map, rxnType)
%
% INPUTS:
%    map:           Map from CellDesigner parsed to MATLAB format
%    rxnType:       Reaction type as a String
%
% OUTPUT:
%    listRxns:      List of reactions indexes (1st column) and
%                   reaction names (2nd column)
%
% .. Author: - N.Sompairac - Institut Curie, Paris, 20/10/2017

    index = find(strcmp(map.rxnType, rxnType));
    listRxns(:,1) = num2cell(index);
    listRxns(:,2) = map.rxnName(index,1);

end

author: opencobra

repo: cobratoolbox


text:
function outsig=comp_frsyn_fusion(F,insig)

W=size(insig,2);
L=size(insig,1)/framered(F);

outsig=zeros(L,W);

idx=0;
for ii=1:F.Nframes
    coeflen=L*framered(F.frames{ii});
    outsig=outsig+frsyn(F.frames{ii},insig(idx+1:idx+coeflen,:))*F.w(ii);
    idx=idx+coeflen;
end;

author: ltfat

repo: ltfat

save_path: github-repos/MATLAB/ltfat-ltfat


text:
% Code that clears an empty variable, or a NaN vsriable.
% Does not clear structures, or cells.

if exist('var2fix')==1,
    if   eval(['exist(''' var2fix ''') == 1']),
        if eval(['isempty(' var2fix ')']),
            eval(['clear ' var2fix ]);
        else
            if eval(['~isstruct(' var2fix ')']),
                if eval(['~iscell(' var2fix ')']),
                    if eval(['isnan(' var2fix '(1))']),
                        eval(['clear ' var2fix ]);
                    end;
                end;
            end;
        end;
    end;
end;

repo: ekfmonoslam


text:
function [p] = registerjacobian(p)
%REGISTERJACOBIAN Register jacobians to an expression

if ~isfield(p.extra, 'jacobian')
    x = recover(depends(p));
    p.extra.jacobian = jacobian(p,x);
end

author: yalmip

repo: YALMIP

save_path: github-repos/MATLAB/yalmip-YALMIP


text:
function out = isreal(f)
%ISREAL   True for real TRIGTECH.
%   ISREAL(F) returns TRUE if F does not have an imaginary part and FALSE
%   otherwise.
%
% See also REAL, IMAG.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

out = all(f.isReal);

end

author: chebfun

repo: chebfun

save_path: github-repos/MATLAB/chebfun-chebfun


text:
function varargout = interp1(varargin)
% bsarray/interp1: 1-D interpolation (table lookup)
% usage: YI = interp1(B,XI);
%    or: YI = interp1(B,XI,EXTRAPVAL);
%
% arguments:
%   B - bsarray object having tensorOrder 1. The positions of the
%       underlying data array are assumed to be X = s.*(1:N), where N is
%       the number of data points (i.e., max(get(B,'dataSize'))), and s
%       is the element spacing (i.e., get(B,'elementSpacing')).
%   XI - points at which to interpolate B.
%   EXTRAPVAL - value to return for points in XI that are outside the range
%       of X. Default EXTRAPVAL = NaN.
%
%   YI - the values of the underlying bsarray B evaluated at the points in
%       the array XI.
%

% author: Nathan D. Cahill
% email: ndcahill@gmail.com
% date: 18 April 2008

% parse input arguments
[b,xi,extrapval] = parseInputs(varargin{:});

% get flag to determine if basis functions are centred or shifted
m = double(get(b,'centred'));

% get number of data elements and coefficients, determine amount of padding
% that has been done to create coefficients
nData = max(get(b,'dataSize'));
n = max(get(b,'coeffsSize'));
padNum = (n-nData-1)/2;

% get the spacing between elements, and then construct vectors of the
% locations of the data and of the BSpline coefficients
h = get(b,'elementSpacing');
xCol = h.*((1-padNum):(nData+padNum+1))';
xDataCol = xCol((1+padNum):(end-padNum));

% turn evaluation points into a column vector, but retain original size so
% output can be returned in same size as input
siz_xi = size(xi);
xiCol = xi(:);
siz_yi = siz_xi;

% grab the BSpline coefficients
cMat = get(b,'coeffs');

% initialize some variables for use in interpolation
numelXi = length(xiCol);
yiMat = zeros(numelXi,1);
p = 1:numelXi;

% Find indices of subintervals, x(k) <= u < x(k+1),
% or u < x(1) or u >= x(m-1).
k = min(max(1+floor((xiCol-xCol(1))/h),1+padNum),n-padNum) + 1-m;
s = (xiCol - xCol(k))/h;

% perform interpolation
d = get(b,'degree');
for i=1:ceil((d+1)/2)
    yiMat(:) = yiMat(:) + cMat(k-i+m).*evalBSpline(s+i-(1+m)/2,d);
    yiMat(:) = yiMat(:) + cMat(k+i-1+m).*evalBSpline(s-i+(1-m)/2,d);
end
if ~m && mod(d,2)
    yiMat(:) = yiMat(:) + cMat(k-i-1).*evalBSpline(s+i+1/2,d);
end

% perform extrapolation
outOfBounds = xiCol<xDataCol(1) | xiCol>xDataCol(nData);
yiMat(p(outOfBounds)) = extrapval;

% reshape result to have same size as input xi
yi = reshape(yiMat,siz_yi);
varargout{1} = yi;

%% subfunction parseInputs
function [b,xi,extrapval] = parseInputs(varargin)

nargs = length(varargin);
error(nargchk(2,3,nargs));

% Process B
b = varargin{1};
if ~isequal(b.tensorOrder,1)
    error([mfilename,'parseInputs:WrongOrder'], ...
        'bsarray/interp1 can only be used with bsarray objects having tensor order 1.');
end

% Process XI
xi = varargin{2};
if ~isreal(xi)
    error([mfilename,'parseInputs:ComplexInterpPts'], ...
        'The interpolation points XI should be real.')
end

% Process EXTRAPVAL
if nargs > 2
    extrapval = varargin{3};
else
    extrapval = [];
end
if isempty(extrapval)
    extrapval = NaN;
end
if ~isscalar(extrapval)
    error([mfilename,':NonScalarExtrapValue'],...
        'EXTRAP option must be a scalar.')
end

author: Sable

repo: mcbench-benchmarks


text:
classdef TankToTankConnState < matlab.mixin.SetGet & matlab.mixin.Copyable
    %TankToTankConnState Summary of this class goes here
    %   Detailed explanation goes here

    properties
        conn(1,1) TankToTankConnection
        active(1,1) logical = true;
        flowRate(1,1) double = 0;
    end

    methods
        function obj = TankToTankConnState(conn)
            obj.conn = conn;
        end

        function newT2TState = deepCopy(obj)
            newT2TState = obj.copy();
            newT2TState.conn = obj.conn;
        end
    end
end

author: Arrowstar

repo: ksptot

save_path: github-repos/MATLAB/Arrowstar-ksptot


text:
function [sData] = computeROI(sData)
% -------------------------------------------------------------------------
% function [sData] = computeROI(sData)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function computes the region of interest (ROI) defined by a given
% RTstruct in a 'sData' file.
% -------------------------------------------------------------------------
% INPUTS: 'sData' file
% -------------------------------------------------------------------------
% OUTPUTS: 'sData' file with ROI definition
% -------------------------------------------------------------------------
% AUTHOR(S): Martin Vallieres <mart.vallieres@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: May 2015
%--------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
%
%    This package is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    This package is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this package.  If not, see <http://www.gnu.org/licenses/>.
% -------------------------------------------------------------------------

% INITIALIZATION
sizeScan = size(sData{2}.scan.volume);
sData{2}.scan.contour.name     = [];
sData{2}.scan.contour.boxBound = [];
sData{2}.scan.contour.boxMask  = [];
if strcmp(sData{2}.scan.orientation,'Sagittal')
    a = 2;
    b = 3;
elseif strcmp(sData{2}.scan.orientation,'Coronal')
    a = 1;
    b = 3;
else
    a = 1;
    b = 2;
end

% ROI COMPUTATION
nContours = length(fieldnames(sData{4}.ROIContourSequence));
for contourNumber = 1:nContours

    % Initialization
    mask = zeros(sizeScan);
    itemContour = ['Item_',num2str(contourNumber)];
    sData{2}.scan.contour(contourNumber).name = sData{4}.StructureSetROISequence.(itemContour).ROIName;
    nSlices = length(fieldnames(sData{4}.ROIContourSequence.(itemContour).ContourSequence));

    for sliceNumber = 1:nSlices

        % Find slice correspondence between volume and RTstruct
        itemSlice = ['Item_',num2str(sliceNumber)];
        UIDrt = sData{4}.ROIContourSequence.(itemContour).ContourSequence.(itemSlice).ContourImageSequence.Item_1.ReferencedSOPInstanceUID;
        for i = 1:sizeScan(3)
            UIDslice = sData{3}(i).SOPInstanceUID;
            if strcmp(UIDrt,UIDslice)
                sliceOK = i;
                break
            end
        end

        pts_temp = sData{4}.ROIContourSequence.(itemContour).ContourSequence.(itemSlice).ContourData; % points stored in the RTstruct file
        if ~isempty(pts_temp)

            % Get XYZ points in the reference frame coordinates
            ind = 1:numel(pts_temp)/3;
            pts = zeros([numel(pts_temp)/3,3]);
            pts(:,1) = pts_temp(ind*3-2); pts(:,2) = pts_temp(ind*3-1); pts(:,3) = pts_temp(ind*3);
            pts(:,1) = pts(:,1) - sData{3}(sliceOK).ImagePositionPatient(1);
            pts(:,2) = pts(:,2) - sData{3}(sliceOK).ImagePositionPatient(2);
            pts(:,3) = pts(:,3) - sData{3}(sliceOK).ImagePositionPatient(3);

            % Get transformation matrix
            p1 = sData{3}(sliceOK).PixelSpacing(1);
            p2 = sData{3}(sliceOK).PixelSpacing(2);
            m = [sData{3}(sliceOK).ImageOrientationPatient(a)*p1 sData{3}(sliceOK).ImageOrientationPatient(a+3)*p2; ...
            sData{3}(sliceOK).ImageOrientationPatient(b)*p1 sData{3}(sliceOK).ImageOrientationPatient(b+3)*p2];

            % Transform points from reference frame to image coordinates
            pts = ((m^-1)*pts(:,[a,b])')' + 1; % +1 for MATLAB image coordinates

            % Obtain mask using set of image points
            mask(:,:,sliceOK) = or(mask(:,:,sliceOK),poly2mask(pts(:,1),pts(:,2),sizeScan(1),sizeScan(2)));
        else
            mask(:,:,sliceOK) = zeros(sizeScan(1),sizeScan(2));
        end
    end

    % Compute the smallest box containing the whole tumor (and its associated bounds)
    [boxBound] = computeBoundingBox(mask);
    mask = mask(boxBound(1,1):boxBound(1,2),boxBound(2,1):boxBound(2,2),boxBound(3,1):boxBound(3,2));
    sData{2}.scan.contour(contourNumber).boxBound = boxBound;
    sData{2}.scan.contour(contourNumber).boxMask = mask;
end

end

author: mvallieres

repo: radiomics


text:
function varargout=rsfft(varargin)
%
% For calling details please see v_rsfft.m
%
% This dummy routine is included for backward compatibility only
% and will be removed in a future release of voicebox. Please use
% v_rsfft.m in future and/or update with v_voicebox_update.m
%
%      Copyright (C) Mike Brookes 2018
%      Version: $Id: rsfft.m 10863 2018-09-21 15:39:23Z dmb $
%
if nargout
    varargout=cell(1,nargout);
    [varargout{:}]=v_rsfft(varargin{:});
else
    v_rsfft(varargin{:});
end

author: ImperialCollegeLondon

repo: sap-voicebox


text:
% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

function MU_funcAxis(Temp,Event,handles)
handles = guidata(handles.MU_matrix_display);

AType={'on','off','auto','manual','tight','fill','ij','xy','equal','image','square','vis3d','normal'};
[Type,ok] = listdlg('ListString',AType, ...
                         'SelectionMode','single',...
                         'PromptString','Axis Appearance',...
                         'Name','Axis');
if ok==0
    warndlg('Changing axis is cancelled.');
    return;
end
handles.V.Axis=AType(Type);

handles=MU_update_image(handles.Matrix_display_axes,{handles.TMatrix,handles.Mask},handles,0);
guidata(handles.MU_matrix_display, handles);

end

author: leoliuf

repo: MRiLab

save_path: github-repos/MATLAB/leoliuf-MRiLab


text:
function [EEG com] = pop_selectmodel(EEG, model, type);

if nargin < 1
    help pop_selectmodel;
    return;
end;
com = '';

if ~isfield(EEG.etc,'amica') || isempty(EEG.etc.amica)
    error('No AMICA solution found. You should first load AMICA components');
end

guititle = 'Select/change data using AMICA model probabilities -- pop_selectmodel()';
data_options = 'Discard data for other models|Weight data by model probability';
if nargin < 2

    %newEEG = EEG;

    if EEG.etc.amica.num_models == 1
        error('Only one model exists');
    end

    % create user interface that allows choosing the model, weighting
    % method, selecting/discarding data.
    allmodels= 1:EEG.etc.amica.num_models;

    if ~isfield(EEG.etc.amica,'modnames')
        EEG.etc.amica.modnames{length(allmodels)} = '';
    end

    str = [num2str(allmodels(1)) fastif(~isempty(EEG.etc.amica.modnames{1}) ,[' - ' EEG.etc.amica.modnames{1}],'')];
    for i = 2:EEG.etc.amica.num_models
        str = [str '|' num2str(allmodels(i)) fastif(~isempty(EEG.etc.amica.modnames{i}) ,[' - ' EEG.etc.amica.modnames{i}],'')];
    end
    editwhat2select = str;

    %smooth_options = 'None (default)|Hann';
    cb_enable = ['set(findobj(''parent'', gcbf, ''tag'', ''labelstring'')    , ''enable'', ''on'');'];
    cb_disable = ['set(findobj(''parent'', gcbf, ''tag'', ''labelstring'')    , ''enable'', ''off'');'];
    cb_label = ['if get(gcbo,''value''),' cb_enable ...
        'else,' cb_disable ...
        'end;'];

    uilist = { {'style' 'text' 'string' 'Select model'} ...
        {'style' 'popupmenu' 'string' editwhat2select 'value' 1} ...
        {'style' 'text' 'string' 'Data selection/weighting'} ...
        {'style' 'popupmenu' 'string' data_options 'value' 1} ...
        {'style' 'checkbox' 'tag' 'labelcheck' 'string' 'Label the chosen model as:' 'value' 0 'callback' cb_label} ...
        {'style' 'edit' 'string' '' 'enable' 'off' 'tag' 'labelstring'}};

    uigeom = {[1 2] [1 2] [1 1]};

    result = inputgui(uigeom, uilist, 'pophelp(''pop_selectmodel'')', guititle, [], 'normal');

    if isempty(result)

        return
    end

    model = result{1};

    if result{3}
        if ~isfield(EEG.etc.amica,'modnames')
            for i = 1:EEG.etc.amica.num_models
                EEG.etc.amica.modnames{i} = '';
            end

        end
        EEG.etc.amica.modnames{model} = result{4};
    end

    type = result{2};
    EEG= selectmodel(EEG,model,type);

else
    if ischar(model) && isfield(EEG.etc.amica,'mod_names')
        for i = 1:EEG.etc.amica.num_models
            if strcmpi(model,EEG.etc.amica.mod_names{i})
                model = i;
                break;
            end
            model = 0;
        end
    end
    if model < 1
        error('Model not found');
    end
    if model>=EEG.etc.amica.num_models
        error('Model number exceeds the total number of models');
    end

    if nargin<3
        uilist = {{'style' 'text' 'string' 'Data selection/weighting'}...
                 {'style' 'popupmenu' 'string' data_options 'value' 1}};
        uigeom = {[1 1.5]};
        result = inputgui(uigeom, uilist, 'pophelp(''pop_selectmodel'')', guititle, [], 'normal');
        if isempty(result)
            return;
        end
        type = result{1};
    end
    EEG = selectmodel(EEG,model,type);
end

com = sprintf('%s = pop_selectmodel(%s,%d,%d)', inputname(1), inputname(1), model, type);

author: PatternRecognition

repo: OpenBMI


text:
function unique_index = i4vec_unique_index ( n, a )

%*****************************************************************************80
%
%% I4VEC_UNIQUE_INDEX indexes the first occurrence of values in an I4VEC.
%
%  Discussion:
%
%    For element A(I) of the vector, FIRST_UNIQUE(I) is the uniqueness index
%    of A(I).  That is, if A_UNIQUE contains the unique elements of A,
%    gathered in order, then
%
%      A_UNIQUE ( UNIQUE_INDEX(I) ) = A(I)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 August 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of elements of A.
%
%    Input, integer A(N), the array.
%
%    Output, integer UNIQUE_INDEX(N), the unique index.
%
  unique_index(1:n) = -1;
  unique_num = 0;

  for i = 1 : n

    if ( unique_index(i) == -1 )

      unique_num = unique_num + 1;
      unique_index(i) = unique_num;

      for j = i + 1 : n
        if ( a(i) == a(j) )
          unique_index(j) = unique_num;
        end
      end

    end

  end

  return
end


text:
function seed_rand()
% Initialize random number generator to a fixed seed
% This ensures that results are reproducible

% AUTORIGHTS
% -------------------------------------------------------
% Copyright (C) 2011-2012 Ross Girshick
% Copyright (C) 2008, 2009, 2010 Pedro Felzenszwalb, Ross Girshick
%
% This file is part of the voc-releaseX code
% (http://people.cs.uchicago.edu/~rbg/latent/)
% and is available under the terms of an MIT-like license
% provided in COPYING. Please retain this notice and
% COPYING if you use this file (or a portion of it) in
% your project.
% -------------------------------------------------------

% Try to make different versions of matlab happy
try
  rng(3, 'twister')
catch
  rand('twister',3);
end

author: rbgirshick

repo: voc-dpm

save_path: github-repos/MATLAB/rbgirshick-voc-dpm


text:
% Author: Ricardo Baptista and Matthias Poloczek
% Date:   June 2018
%
% See LICENSE.md for copyright information
%

function out = bayes_obj(objective, x)
	out = objective(double(table2array(x)));
end

author: baptistar

repo: BOCS

save_path: github-repos/MATLAB/baptistar-BOCS


text:
function [bdNode,bdFacet,isBdNode] = findpolyboundary(varargin)
%% FINDPOLYBOUNDARY finds the boundary facet(s) of a polytopal mesh (dim=2 or 3)
%  2D: facet is given as an NEx2 edge array.
%  3D: facet is given as an NFx4 face array.
%  allFacet contains all facets retrieved from elem{:} or elem
%  If the input is elem, elem has be a cell array
%  Current restriction: face has to be triangular or quadrilateral
%  when f-th face is triangular, the face(f,4) = 0.
%
%  Examples:
%
%  totalEdge = uint32([elem(:,[1 2]); elem(:,[2 3]); ...
%                    elem(:,[3 4]); elem(:,[4 1])]);
%  [bdNode,bdFacet,isBdNode] = findpolyboundary(allEdge)
%
%  allFace = [elem(:,[2, 3, 4]);elem(:,[1, 4,3]);elem(:,[1,2,4]);elem(:,[1,3,2])];
%  allFace(:,4) = 0;
%  [bdNode,bdFacet,isBdNode] = findpolyboundary(allFace)
%
%  the following example only works in 2D elem is a cell array:
%  elem = {[10,9,11,14,15];[15,14,16,17];[3,1,6,5];[5,6,7,12,11];...
%          [4,3,5,11,9];[2,4,9,10];[12,7,8,13];[14,11,12,13,16]};
%  node = [1 0; 0 0; 0.63591 0; 0.36409 0; 0.68734 0.31329;...
%          1  0.36233; 1 0.63767; 1 1; 0.31266 0.31329;...
%          0 0.36233; 0.49815 0.5; 0.68734 0.68671; 0.63591 1;...
%          0.31266 0.68671;  0 0.63767; 0.36409 1; 0 1];
%  [bdNode,bdFacet,isBdNode] = findpolyboundary(elem);
%  showmeshpoly(node,elem);
%  findnode(node,bdNode);
%
%  To-do: add bdFlag for more streamlined workflow

%%
if iscell(varargin{1})
    elem = varargin{1};
    elemVertexNumber = cellfun('length',elem);
    maxNv = max(elemVertexNumber);
    minNv = min(elemVertexNumber);
    allFacet = cell(maxNv,1);
    for Nv = minNv:maxNv
        idx = (elemVertexNumber == Nv);
        elemNv = cell2mat(elem(idx));
        NT = sum(idx);
        % # of edge = # of vertices in 2D
        locEdge = [1:Nv; circshift(1:Nv,-1)]';
        allFacet{Nv} = zeros(NT*Nv,2);
        for j = 1:Nv
           allFacet{Nv}((j-1)*NT+1:j*NT,:) = elemNv(:,locEdge(j,:));
        end
    end
    if isrow(allFacet); allFacet = allFacet'; end
    allFacet = cell2mat(allFacet);
elseif ismatrix(varargin{1})
    allFacet = varargin{1};
end

[NF, dim] = size(allFacet);
allFacet = sort(allFacet, 2);
[uface, i2, j] = myunique(allFacet); %#ok<*ASGLU>
i1(j(NF:-1:1)) = NF:-1:1;
i1 = i1';
bdFacet = allFacet(i1(i1 == i2), :);
switch dim
    case 2
        isBdNode(bdFacet(:)) = true;
    case 4
        isTriFace = bdFacet(:, 4) == 0;
        isBdNode(bdFacet(isTriFace,1:3)) = true;
        isBdNode(bdFacet(~isTriFace, :)) = true;
end

bdNode = find(isBdNode);

repo: ifem


text:
%GENDATH Generation of Highleyman classes
%
%   A = GENDATH(N,LABTYPE)
%
% INPUT
%  N        Number of objects (optional; default: [50,50])
%  LABTYPE  Label type (optional; default: 'crisp')
%
% OUTPUT
%  A        Generated dataset
%
% DESCRIPTION
% Generation of a 2-dimensional 2-class dataset A of N objects
% according to Highleyman.
%
% The two Highleyman classes are defined by
% 1: Gauss([1 1],[1 0; 0 0.25]).
% 2: Gauss([2 0],[0.01 0; 0 4]).
% Class priors are P(1) = P(2) = 0.5
%
% If N is a vector of sizes, exactly N(I) objects are generated
% for class I, I = 1,2.
%
% LABTYPE defines the desired label type: 'crisp' or 'soft'. In the
% latter case true posterior probabilities are set for the labels.
%
% Defaults: N = [50,50], LABTYPE = 'crisp'.
%
% EXAMPLES
% PREX_PLOTC, PREX_CLEVAL
%
% SEE ALSO (PRTools Guide)
% DATASETS, GAUSS, PRDATASETS

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Sciences, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

% $Id: gendath.m,v 1.5 2009/01/27 13:01:42 duin Exp $

function A = gendath(N,labtype)

	if nargin < 1, N = [50, 50]; end
	if nargin < 2, labtype = 'crisp'; end

	GA = [1 0; 0 0.25];
	GB = [0.01 0; 0 4];
	G = cat(3,GA,GB);
	p = [0.5 0.5];
	N = genclass(N,p);
	U = prdataset([1 1; 2 0],[1 2]','prior',p);
	U = setprior(U,p);
	A = gendatgauss(N,U,G);
	A = setname(A,'Highleyman Dataset');

	switch labtype
	 case 'crisp'
	  ;
	 case 'soft'
	  W = nbayesc(U,cat(3,GA,GB));
	  targets = A*W*classc;
	  A = setlabtype(A,'soft',targets);
	 otherwise
	  error(['Label type ' labtype ' not supported'])
	end

	return

author: marianux

repo: ecg-kit

save_path: github-repos/MATLAB/marianux-ecg-kit


text:
%{
Copyright (c) 2015, Tom Mertens
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%}

% This procedure loads a sequence of images
%
% Arguments:
%   'path', refers to a directory which contains a sequence of JPEG or PPM
%   images
%   'reduce' is an optional parameter that controls downsampling, e.g., reduce = .5
%   downsamples all images by a factor of 2.
%
% tom.mertens@gmail.com, August 2007
%

function I = load_images(path, reduce)

if ~exist('reduce')
    reduce = 1;
end

if (reduce > 1 || reduce <= 0)
    error('reduce must fulfill: 0 < reduce <= 1');
end

% find all JPEG or PPM files in directory
files = dir([path '/*.jpg']);
N = length(files);
if (N == 0)
    files = dir([path '/*.ppm']);
    N = length(files);
    if (N == 0)
        error('no files found');
    end
end

% allocate memory
sz = size(imread([path '/' files(1).name]));
r = floor(sz(1)*reduce);
c = floor(sz(2)*reduce);
I = zeros(r,c,3,N);

% read all files
for i = 1:N

    % load image
    filename = [path '/' files(i).name];
    im = double(imread(filename)) / 255;
    if (size(im,1) ~= sz(1) || size(im,2) ~= sz(2))
        error('images must all have the same size');
    end

    % optional downsampling step
    if (reduce < 1)
        im = imresize(im,[r c],'bicubic');
    end

    I(:,:,:,i) = im;
end

author: Mericam

repo: exposure-fusion


text:
function c = double(b)
% bsarray/double: convert bsarray object to coefficient array
% usage: c = double(b)
%
% arguments:
%   b - bsarray object
%
%   c - array of bspline coefficients

% author: Nathan D. Cahill
% email: ndcahill@gmail.com
% date: 18 April 2008

c = b.coeffs;


text:
function D = partsof(D,name,parts,thresh)
% Finds the parts of an object and modifies the database structure D so
% that parts point to the object to which it belongs.  To find the parts
% of a "car", do the following:
%
% D = partsof(D,'car');
%
% If object j in image i is a part of a car, then
% D(i).annotation.object(j).partof will be set to the ID of the car to
% which it belongs in the image.

if nargin < 4
    thresh = 0.5;
end

[~,jj] = LMquery(D,'object.name',[name ',' parts]);

for ii = 1:length(jj)
    jo = LMobjectindex(D(jj(ii)).annotation,name);
    jp = LMobjectindex(D(jj(ii)).annotation,parts);
    M = zeros(length(jo),length(jp));
    for kk = 1:length(jo)
        for ll = 1:length(jp)
            M(kk,ll) = 0;
            [XXo,YYo] = getLMpolygon(D(jj(ii)).annotation.object(jo(kk)).polygon);
            [XXp,YYp] = getLMpolygon(D(jj(ii)).annotation.object(jp(ll)).polygon);

            ro = [min(XXo) min(YYo) max(XXo) max(YYo)];
            rp = [min(XXp) min(YYp) max(XXp) max(YYp)];

            if rect_intersect(ro,rp) >= thresh

                Ao = polyarea(XXo,YYo);
                Ap = polyarea(XXp,YYp);
                if Ao > Ap
                    M(kk,ll) = int_area(XXo,YYo,XXp,YYp)/Ap;
                end
            end
        end
    end
    [vals,inds] = max(M,[],1);

    %
    %
    for kk = 1:length(inds)
        if vals(kk) >= thresh
            [ids,jj_id] = getID(D(jj(ii)).annotation);
            id_ind = find(jj_id==jo(inds(kk)));

            object_id = ids(id_ind);
            part_id =
            D(jj(ii)).annotation = addpart_i_to_object_j(D(jj(ii)).annotation, ids(id_ind), j);

            if ~isempty(id_ind)
                D(jj(ii)).annotation.object(jp(kk)).partof = num2str(ids(id_ind));
            elseif ~isempty(ids)
                D(jj(ii)).annotation.object(jo(inds(kk))).id = num2str(max(ids)+1);
                D(jj(ii)).annotation.object(jp(kk)).partof = num2str(max(ids)+1);
            else
                D(jj(ii)).annotation.object(jo(inds(kk))).id = '0';
                D(jj(ii)).annotation.object(jp(kk)).partof = '0';
            end
            D(jj(ii)).annotation.object(jp(kk)).partofobject = ...
                D(jj(ii)).annotation.object(jo(inds(kk))).name;
        end
    end
end

function annotation = addpart_i_to_object_j(annotation,i,j)

annotation.object(j).parts.hasparts = i;
annotation.object(i).parts.ispartof = j;

function [v,j] = getID(annotation)

if isfield(annotation,'object') && isfield(annotation.object(1),'id')
    v = str2double({annotation.object(:).id});
    j = find(~isnan(v));
    v = v(j);
    if length(v) ~= length(unique(v))
        disp('WARNING: getID(): There are duplicate IDs!');
    end
else
    v = [];
    j = [];
end

function tt = rect_intersect(ro,rp)
% min_x,min_y,max_x,max_y

tt = 0;
wo = ro(3)-ro(1);
ho = ro(4)-ro(2);
wp = rp(3)-rp(1);
hp = rp(4)-rp(2);
if (wp*hp) > 0
    tt = rectint([ro(1) ro(2) wo ho],[rp(1) rp(2) wp hp]) / (wp*hp);
end

author: CSAILVision

repo: LabelMeToolbox


text:
function Phi = dipole (z,z0,S,Beta)
Phi = S*exp(i*Beta)./(2*pi*(z-z0));


text:
classdef StressVoigt2TensorConverterPS < SecondOrderVoigt2TensorConverterPS

    properties
    end

    methods (Access = public)

        function obj = StressVoigt2TensorConverterPS(tensor)
           obj.computeConversion(tensor)
        end

    end

    methods (Access = protected)

        function factor = computeVoigtFactor(obj)
            factor = 1;
        end

        function selectTensorClass(obj)
            obj.tensor = StressPlaneStressTensor();
        end
    end

end

author: SwanLab

repo: Swan

save_path: github-repos/MATLAB/SwanLab-Swan


text:
function result = avoidcopies_functions
% AVOIDCOPIES_FUNCTIONS
% Compares the time of code segment 3, which does not use in-place function calling, to
% the time for code segment 4, which does. It returns a structure with the
% fields:
%
%   timeSegment3= time for code segment 3
%   timeSegment4= time for code segment 4
%   speedUp = timeSegment3/timeSegment4;
%   percentLessTime = 100*(timeSegment3-timeSegment4)/timeSegment3;
%
% The speedUp value is good for characterizing large improvements, whereas
% the percentLessTime is good at characterizing small improvements. Run a
% few times to remove first time costs.
%
%   Example:
%   >> avoidcopies_functions

%% Call code segments
result.timeSegment3=segment3;
result.timeSegment4=segment4;

%% Calculate performance improvement
% In terms of speed up and percentage less time
result.speedUp=result.timeSegment3/result.timeSegment4;
result.percentLessTime=100*(result.timeSegment3-result.timeSegment4)/result.timeSegment3;

%% Code segment 3
function time=segment3
N=3e3;
x=rand(N);
tic;
y=myfun(x);
time=toc;

function y=myfun(x)
y=1.2*x;

%% Code segment 4
function time=segment4
N=3e3;
x=rand(N);
tic
x=myfun_ip(x);
time=toc;

function x=myfun_ip(x)
x=1.2*x; % In-place


text:
function [coef]=ref_dwiltii_1(f,g,a,M)
%COMP_DWILT  Compute Discrete Wilson transform.
%
%   Do not call this function directly, use DWILT instead.

%   Author : Peter L. Søndergaard.

L=size(g,1);
N=L/a;
W=size(f,2);

c=ref_gdgt(f,g,a,2*M,.5,0,0);

coef2=reshape(c,2*M,N,W); % ----- Type II ------
%coef2=dgt(f,g,a,2*M);

coef=zeros(2*M,N/2,W);

if 0
  % --- Loop version ---
  for n=0:N/2-1

    % ---- m is zero ---------
    coef(1,n+1,:)=coef2(1,2*n+1,:);

    for m=1:2:M-1
      % --- m is odd ----------
      coef(m+1,n+1,:)=   i/sqrt(2)*(coef2(m+1,2*n+1,:)+coef2(2*M-m+1,2*n+1,:));
      coef(M+m+1,n+1,:)= 1/sqrt(2)*(coef2(m+1,2*n+2,:)-coef2(2*M-m+1,2*n+2,:));
    end;
    for m=2:2:M-1
      % --- m is even ---------
      coef(m+1,n+1,:)=   1/sqrt(2)*(coef2(m+1,2*n+1,:)-coef2(2*M-m+1,2*n+1,:));
      coef(M+m+1,n+1,:)= i/sqrt(2)*(coef2(m+1,2*n+2,:)+coef2(2*M-m+1,2*n+2,:));
    end;

    % --- m is nyquest ------
    if mod(M,2)==0
      coef(M+1,n+1,:) = i*coef2(M+1,2*n+2,:);
    else
      coef(M+1,n+1,:) = i*coef2(M+1,2*n+1,:);
    end;

  end;

else
  % --- Vector version---

  % ---- m is zero ---------
  coef(1,:,:)=coef2(1,1:2:N,:);

  % --- m is odd ----------
  coef(2:2:M,:,:)    = i/sqrt(2)*(coef2(2:2:M,1:2:N,:)+coef2(2*M:-2:M+2,1:2:N,:));
  coef(M+2:2:2*M,:,:)= 1/sqrt(2)*(coef2(2:2:M,2:2:N,:)-coef2(2*M:-2:M+2,2:2:N,:));

  % --- m is even ---------
  coef(3:2:M,:,:)=     1/sqrt(2)*(coef2(3:2:M,1:2:N,:)-coef2(2*M-1:-2:M+2,1:2:N,:));
  coef(M+3:2:2*M,:,:)= i/sqrt(2)*(coef2(3:2:M,2:2:N,:)+coef2(2*M-1:-2:M+2,2:2:N,:));

  % --- m is nyquest ------
  if mod(M,2)==0
    coef(M+1,:,:) = i*coef2(M+1,2:2:N,:);
  else
    coef(M+1,:,:) = i*coef2(M+1,1:2:N,:);
  end;

end;

coef=reshape(coef,M*N,W);

repo: ltfat


text:
%============================================================================
% Copyright (C) 2015, Heikki Hyyti
%
% Permission is hereby granted, free of charge, to any person obtaining a
% copy of this software and associated documentation files (the "Software"),
% to deal in the Software without restriction, including without limitation
% the rights to use, copy, modify, merge, publish, distribute, sublicense,
% and/or sell copies of the Software, and to permit persons to whom the
% Software is furnished to do so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
% FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
% DEALINGS IN THE SOFTWARE.
%============================================================================

disp(' ');
disp('If you use the algorithm in any scientific context, please cite:');
disp('Heikki Hyyti and Arto Visala, "A DCM Based Attitude Estimation Algorithm');
disp('for Low-Cost MEMS IMUs," International Journal of Navigation and Observation,');
disp('vol. 2015, Article ID 503814, 18 pages, 2015. http://dx.doi.org/10.1155/2015/503814');
disp(' ');

clear all;
close all;
pause(0.1);

%% Configuration!
% select to test biases using Acceleration test (true) or Rotation test (false)
ACCtest = true;

% use c/compile.m before setting this to true (tested only with linux)
use_C_versions = false;

% fetch GPL licensed algortihms in matlab and C from here before use:
% http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
% Add the C code to c folder and matlab functions in this folder
% (please add also the quaternion library used by Madgwick).
% I apologize this mess. The Madgwick's GPS licenced code is separated from
% this MIT licenced product to avoid infecting this code with GPL licence.
use_comparisonAlgorithms = false;

if (ACCtest)
    disp('Calculating and plotting IMU bias test for Acceleration data');
else
    disp('Calculating and plotting IMU bias test for Rotation data');
end
data = load('allData.mat');

if (~exist('figures', 'dir')); mkdir('figures'); end;

ILtime = data.InertiaLink.tv_sec + (data.InertiaLink.tv_msec/1000);
SFtime = data.SparkFun6DOF.tv_sec + (data.SparkFun6DOF.tv_usec / 1000000);
Ktime = data.Kuka.tv_sec + (data.Kuka.tv_usec/1000000);

firstCommonTime = max([ILtime(1), SFtime(1), Ktime(1)]);
lastCommonTime = min([ILtime(end), SFtime(end), Ktime(end)]);
commonDuration = lastCommonTime - firstCommonTime;

ILtime = ILtime - firstCommonTime;
SFtime = SFtime - firstCommonTime;
Ktime = Ktime - firstCommonTime;

if (ACCtest)
    %ACC-test sequence: Linear motion with different accelerations
    StartIdx = find(ILtime > 468, 1, 'first');
    StopIdx = find(ILtime > 559, 1, 'first');
else
    %IMU-test sequence: free 6D motion
    StartIdx = find(ILtime > 564, 1, 'first');
    StopIdx = find(ILtime > 770, 1, 'first');
end

testName = 'Rotation test';
if (ACCtest)
    testName = 'Acceleration test';
end

markerSize = 8;

figPos = [105 105 645 660];

plotPos = [0.1 0.15 0.70 0.70];

plot21Pos = [0.1 0.53 0.86 0.40];
plot22Pos = plot21Pos + [0 -0.47 0 0];

plot1Pos = [0.1 0.69 0.86 0.25];
plot2Pos = plot1Pos + [0 -0.31 0 0];
plot3Pos = plot2Pos + [0 -0.31 0 0];

plot41Pos = [0.1 0.75 0.86 0.19];
plot42Pos = plot41Pos + [0 -0.23 0 0];
plot43Pos = plot42Pos + [0 -0.23 0 0];
plot44Pos = plot43Pos + [0 -0.23 0 0];

%% getting reference trajectory
R11 = data.Kuka.data_msrCartPos01;
R12 = data.Kuka.data_msrCartPos02;
R13 = data.Kuka.data_msrCartPos03;
x = data.Kuka.data_msrCartPos04;
R21 = data.Kuka.data_msrCartPos05;
R22 = data.Kuka.data_msrCartPos06;
R23 = data.Kuka.data_msrCartPos07;
y = data.Kuka.data_msrCartPos08;
R31 = data.Kuka.data_msrCartPos09;
R32 = data.Kuka.data_msrCartPos10;
R33 = data.Kuka.data_msrCartPos11;
z = data.Kuka.data_msrCartPos12;

[yaw, pitch, roll] = yawpitchroll(R11, R21, R31, R32, R33);
yaw = un_modulo(yaw, 2*pi); %remove 2pi jumps and make yaw continuous
yaw = yaw - yaw(1); %starting yaw from zero (same as IMU estimates)

% compute reference angular velocities using the KUKA robot
% (derivate between positions)
gyro = zeros(length(R11),3);
dt = median(Ktime(2:end)-Ktime(1:end-1)); % the most frequently used discretation time
for i = 2:length(R11)
    R_last = [R11(i-1), R12(i-1), R13(i-1); ...
              R21(i-1), R22(i-1), R23(i-1); ...
              R31(i-1), R32(i-1), R33(i-1)];
    R_cur = [R11(i), R12(i), R13(i); ...
             R21(i), R22(i), R23(i); ...
             R31(i), R32(i), R33(i)];

    Wh = (R_last'*R_cur - eye(3));
    W = 1/dt * Wh;
    gyro(i,1) = (W(3,2) - W(2,3))/2;
    gyro(i,2) = (W(1,3) - W(3,1))/2;
    gyro(i,3) = (W(2,1) - W(1,2))/2;
end

%% calibrate IMU data
imusWithKukaCalibration;

if (~use_comparisonAlgorithms)
    disp('To get reference algorithms, download their implementations from here:');
    disp('http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/');
    disp(' ');
end

%% start bias test computation (compute everything with test region using different biases)
% test and computation is done only for InertiaLink data
testBiases = ((0:0.5:7)*pi/180)';
%testBiases = ((0:0.5:1)*pi/180)';

yaw_DCM_RMSE = zeros(size(testBiases));
yaw_e1_RMSE = zeros(size(testBiases));
yaw_e2_RMSE = zeros(size(testBiases));

pitch_DCM_RMSE = zeros(size(testBiases));
pitch_e1_RMSE = zeros(size(testBiases));
pitch_e2_RMSE = zeros(size(testBiases));

roll_DCM_RMSE = zeros(size(testBiases));
roll_e1_RMSE = zeros(size(testBiases));
roll_e2_RMSE = zeros(size(testBiases));

acc2 = [acc2_x(StartIdx:StopIdx), acc2_y(StartIdx:StopIdx), acc2_z(StartIdx:StopIdx)];
gyro2_noBias = [w2_x(StartIdx:StopIdx), w2_y(StartIdx:StopIdx), w2_z(StartIdx:StopIdx)];
time = ILtime(StartIdx:StopIdx);
deltaT = [0; time(2:end) - time(1:end-1)];

%resample Kuka reference data to InertiaLink time
warning('off','interpolation:interpolation:noextrap');
yaw_in_IL = resample(timeseries(yaw, Ktime),time);
yaw_in_IL = yaw_in_IL.Data;
yaw_in_IL = yaw_in_IL - yaw_in_IL(1); %starting yaw from zero (same as IMU estimates)
pitch_in_IL = resample(timeseries(pitch, Ktime),time);
pitch_in_IL = pitch_in_IL.Data;
roll_in_IL = resample(timeseries(roll, Ktime),time);
roll_in_IL = roll_in_IL.Data;
warning('on','interpolation:interpolation:noextrap');

disp('Starting to compute RMSEs using different biases');

%check if matlabpool is opened
scheduler = findResource();
if (matlabpool('size') < scheduler.ClusterSize)
    if (matlabpool('size') > 0)
        matlabpool close;
    end

    matlabpool(scheduler.ClusterSize);
end

warning('off','MATLAB:mir_warning_maybe_uninitialized_temporary');
parfor i = 2:(length(testBiases))
    disp(['iteration ' int2str(i) ', bias ' num2str(testBiases(i)*180/pi) ' deg/s']);

    % add constant bias for testing purposes
    addedGyroBias = testBiases(i);
    gyro2 = gyro2_noBias + addedGyroBias*ones(size(gyro2_noBias));

    if (use_C_versions)
        [x_hist2, ypr_hist2, a_hist2, P_diag_hist2] = ...
            DCM_IMU_C(gyro2, acc2, deltaT);

        if (use_comparisonAlgorithms)
            quaternion1 = Madgwick_IMU_C(gyro2, acc2);
            quaternion2 = Mahony_IMU_C(gyro2, acc2);
        else
            quaternion1 = nan(size(gyro2,1),4);
            quaternion2 = nan(size(gyro2,1),4);
        end
    else
        DCM = DCM_IMU();

        if (use_comparisonAlgorithms)
            addpath('quaternion_library');      % include quaternion library

            IMU1t = MadgwickAHRS('SamplePeriod', 1/150, 'Beta', 0.1);
            IMU2t = MahonyAHRS('SamplePeriod', 1/150, 'Kp', 0.5);

            quaternion1 = zeros(length(time), 4);
            quaternion2 = zeros(length(time), 4);
        else
            quaternion1 = nan(length(time), 4);
            quaternion2 = nan(length(time), 4);
        end

        x_hist2 = zeros(length(time), 6);
        ypr_hist2 = zeros(length(time), 3);
        P_diag_hist2 = zeros(length(time), 6);
        for t = 1:length(time)
            DCM.UpdateIMU(gyro2(t,:), acc2(t,:), deltaT(t));	% gyroscope units must be radians
            x_hist2(t, :) = DCM.state';
            ypr_hist2(t, :) = [DCM.yaw, DCM.pitch, DCM.roll];
            P_diag_hist2(t, :) = diag(DCM.P)';

            if (use_comparisonAlgorithms)
                IMU1t.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
                quaternion1(t, :) = IMU1t.Quaternion;

                IMU2t.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
                quaternion2(t, :) = IMU2t.Quaternion;
            end
        end
    end

    % Plot algorithm output as Euler angles
    % The first and third Euler angles in the sequence (phi and psi) become
    % unreliable when the middle angles of the sequence (theta) approaches ~90
    % degrees. This problem commonly referred to as Gimbal Lock.
    % See: http://en.wikipedia.org/wiki/Gimbal_lock

    % use conjugate for sensor frame relative to Earth and convert to degrees.
    if (use_comparisonAlgorithms)
        euler1 = quatern2euler(quaternConj(quaternion1)) * (180/pi);
        euler2 = quatern2euler(quaternConj(quaternion2)) * (180/pi);
    else
        euler1 = nan(size(quaternion1,1),3);
        euler2 = nan(size(quaternion2,1),3);
    end

    % compute errors

    %yaw pitch roll errors (only InertiaLink data)
    ypr_hist2(:,1) = un_modulo(ypr_hist2(:,1), 2*pi); %remove 2pi jumps and make continuous
    ypr_hist2(:,1) = ypr_hist2(:,1) - ypr_hist2(1,1); %DCM

    euler1(:,3) = un_modulo(euler1(:,3), 360); %remove 2pi jumps and make continuous
    euler1(:,3) = euler1(:,3) - euler1(1,3); %Madgwick

    euler2(:,3) = un_modulo(euler2(:,3), 360); %remove 2pi jumps and make continuous
    euler2(:,3) = euler2(:,3) - euler2(1,3); %Mahony

    %yaw
    yaw_DCM_error = (ypr_hist2(:,1)-yaw_in_IL)*180/pi;
    yaw_DCM_RMSE(i) = sqrt(mean(yaw_DCM_error.^2));
    yaw_e1_error = euler1(:,3) - yaw_in_IL*180/pi;
    yaw_e1_RMSE(i) = sqrt(mean(yaw_e1_error.^2));
    yaw_e2_error = euler2(:,3) - yaw_in_IL*180/pi;
    yaw_e2_RMSE(i) = sqrt(mean(yaw_e2_error.^2));

    %pitch
    pitch_DCM_error = (ypr_hist2(:,2)-pitch_in_IL)*180/pi;
    pitch_DCM_RMSE(i) = sqrt(mean(pitch_DCM_error.^2));
    pitch_e1_error = euler1(:,2) - pitch_in_IL*180/pi;
    pitch_e1_RMSE(i) = sqrt(mean(pitch_e1_error.^2));
    pitch_e2_error = euler2(:,2) - pitch_in_IL*180/pi;
    pitch_e2_RMSE(i) = sqrt(mean(pitch_e2_error.^2));

    %roll
    roll_DCM_error = (ypr_hist2(:,3)-roll_in_IL)*180/pi;
    roll_DCM_RMSE(i) = sqrt(mean(roll_DCM_error.^2));
    roll_e1_error = euler1(:,1) - roll_in_IL*180/pi;
    roll_e1_RMSE(i) = sqrt(mean(roll_e1_error.^2));
    roll_e2_error = euler2(:,1) - roll_in_IL*180/pi;
    roll_e2_RMSE(i) = sqrt(mean(roll_e2_error.^2));
end
warning('on','MATLAB:mir_warning_maybe_uninitialized_temporary');
%% plotting

% compute same for smallest and some other index (for separate plotting)
figIdx = 1;
j_idx = [1 3];
for j = 1:2
    i = j_idx(j);

    disp(['iteration ' int2str(i) ', bias ' num2str(testBiases(i)*180/pi) ' deg/s']);

    % add constant bias for testing purposes
    addedGyroBias = testBiases(i);
    gyro2 = gyro2_noBias + addedGyroBias*ones(size(gyro2_noBias));

    if (use_C_versions)
        [x_hist2, ypr_hist2, a_hist2, P_diag_hist2] = ...
            DCM_IMU_C(gyro2, acc2, deltaT);

        if (use_comparisonAlgorithms)
            quaternion1 = Madgwick_IMU_C(gyro2, acc2);
            quaternion2 = Mahony_IMU_C(gyro2, acc2);
        else
            quaternion1 = nan(size(gyro2,1),4);
            quaternion2 = nan(size(gyro2,1),4);
        end
    else
        DCM = DCM_IMU();

        if (use_comparisonAlgorithms)
            addpath('quaternion_library');      % include quaternion library

            IMU1 = MadgwickAHRS('SamplePeriod', 1/150, 'Beta', 0.1);
            IMU2 = MahonyAHRS('SamplePeriod', 1/150, 'Kp', 0.5);

            quaternion1 = zeros(length(time), 4);
            quaternion2 = zeros(length(time), 4);
        else
            quaternion1 = nan(length(time), 4);
            quaternion2 = nan(length(time), 4);
        end

        x_hist2 = zeros(length(time), 6);
        ypr_hist2 = zeros(length(time), 3);
        P_diag_hist2 = zeros(length(time), 6);
        for t = 1:length(time)
            DCM.UpdateIMU(gyro2(t,:), acc2(t,:), deltaT(t));	% gyroscope units must be radians
            x_hist2(t, :) = DCM.state';
            ypr_hist2(t, :) = [DCM.yaw, DCM.pitch, DCM.roll];
            P_diag_hist2(t, :) = diag(DCM.P)';

            if (use_comparisonAlgorithms)
                IMU1.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
                quaternion1(t, :) = IMU1.Quaternion;

                IMU2.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
                quaternion2(t, :) = IMU2.Quaternion;
            end
        end
    end

    % Plot algorithm output as Euler angles
    % The first and third Euler angles in the sequence (phi and psi) become
    % unreliable when the middle angles of the sequence (theta) approaches ~90
    % degrees. This problem commonly referred to as Gimbal Lock.
    % See: http://en.wikipedia.org/wiki/Gimbal_lock

    % use conjugate for sensor frame relative to Earth and convert to degrees.
    if (use_comparisonAlgorithms)
        euler1 = quatern2euler(quaternConj(quaternion1)) * (180/pi);
        euler2 = quatern2euler(quaternConj(quaternion2)) * (180/pi);
    else
        euler1 = nan(size(quaternion1,1),3);
        euler2 = nan(size(quaternion2,1),3);
    end

    % compute errors

    % yaw pitch roll errors (only InertiaLink data)
    ypr_hist2(:,1) = un_modulo(ypr_hist2(:,1), 2*pi); %remove 2pi jumps and make continuous
    ypr_hist2(:,1) = ypr_hist2(:,1) - ypr_hist2(1,1); %DCM

    euler1(:,3) = un_modulo(euler1(:,3), 360); %remove 2pi jumps and make continuous
    euler1(:,3) = euler1(:,3) - euler1(1,3); %Madgwick

    euler2(:,3) = un_modulo(euler2(:,3), 360); %remove 2pi jumps and make continuous
    euler2(:,3) = euler2(:,3) - euler2(1,3); %Mahony

    %yaw
    yaw_DCM_error = (ypr_hist2(:,1)-yaw_in_IL)*180/pi;
    yaw_DCM_RMSE(i) = sqrt(mean(yaw_DCM_error.^2));
    yaw_e1_error = euler1(:,3) - yaw_in_IL*180/pi;
    yaw_e1_RMSE(i) = sqrt(mean(yaw_e1_error.^2));
    yaw_e2_error = euler2(:,3) - yaw_in_IL*180/pi;
    yaw_e2_RMSE(i) = sqrt(mean(yaw_e2_error.^2));

    %pitch
    pitch_DCM_error = (ypr_hist2(:,2)-pitch_in_IL)*180/pi;
    pitch_DCM_RMSE(i) = sqrt(mean(pitch_DCM_error.^2));
    pitch_e1_error = euler1(:,2) - pitch_in_IL*180/pi;
    pitch_e1_RMSE(i) = sqrt(mean(pitch_e1_error.^2));
    pitch_e2_error = euler2(:,2) - pitch_in_IL*180/pi;
    pitch_e2_RMSE(i) = sqrt(mean(pitch_e2_error.^2));

    %roll
    roll_DCM_error = (ypr_hist2(:,3)-roll_in_IL)*180/pi;
    roll_DCM_RMSE(i) = sqrt(mean(roll_DCM_error.^2));
    roll_e1_error = euler1(:,1) - roll_in_IL*180/pi;
    roll_e1_RMSE(i) = sqrt(mean(roll_e1_error.^2));
    roll_e2_error = euler2(:,1) - roll_in_IL*180/pi;
    roll_e2_RMSE(i) = sqrt(mean(roll_e2_error.^2));

    % plotting
    % Yaw, pitch and roll plot
    figure(figIdx); clf;
    subplot(3,1,1);
    yaw_dcm = ypr_hist2(:,1)*180/pi;
    pitch_dcm = ypr_hist2(:,2)*180/pi;
    roll_dcm = ypr_hist2(:,3)*180/pi;

    plot(time, yaw_dcm, 'b',  time, euler1(:,3), 'g--', time, euler2(:,3), 'r:', time, yaw_in_IL*180/pi, 'k--', 'LineWidth', 1);
    set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');
    legend('DCM', 'Madgwick', 'Mahony', 'Reference', 'Location', legendLocation([yaw_dcm euler1(:,3) euler2(:,3)]));

    ylabel('yaw (deg)', 'FontSize', 12, 'FontName', 'Times');
    title(['Euler angles with a bias of ' num2str(addedGyroBias*180/pi, '%.0f') ' deg/s'], 'FontSize', 14, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    subplot(3,1,2);
    plot(time, pitch_dcm, 'b',  time, euler1(:,2), 'g--', time, euler2(:,2), 'r:', time, pitch_in_IL*180/pi, 'k--', 'LineWidth', 1);
    set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');
    ylabel('pitch (deg)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    subplot(3,1,3);
    plot(time, roll_dcm, 'b',  time, euler1(:,1), 'g--', time, euler2(:,1), 'r:', time, roll_in_IL*180/pi, 'k--', 'LineWidth', 1);
    set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');
    xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
    ylabel('roll (deg)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
    set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
    set(gcf,'PaperPositionMode','auto');
    saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']);
    print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps'])
    figIdx = figIdx + 1;

    % error plot
    figure(figIdx); clf;

    subplot(3,1,1);
    plot(time, yaw_DCM_error, 'b', time, yaw_e1_error, 'g--', time, yaw_e2_error, 'r:', 'LineWidth', 1);
    set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');
    legend('DCM', 'Madgwick', 'Mahony', 'Location', legendLocation([yaw_DCM_error yaw_e1_error yaw_e2_error]));
    ylabel('yaw error (deg)', 'FontSize', 12, 'FontName', 'Times');
    title(['Errors to the reference measurement with a bias of ' num2str(addedGyroBias*180/pi, '%.0f') ' deg/s'], 'FontSize', 14, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    subplot(3,1,2);
    plot(time, pitch_DCM_error, 'b', time, pitch_e1_error, 'g--', time, pitch_e2_error, 'r:', 'LineWidth', 1);
    set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');
    ylabel('pitch error (deg)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    subplot(3,1,3);
    plot(time, roll_DCM_error, 'b',  time, roll_e1_error, 'g--', time, roll_e2_error, 'r:', 'LineWidth', 1);
    set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');
    xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
    ylabel('roll error (deg)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
    set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
    set(gcf,'PaperPositionMode','auto');
    saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']);
    print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps'])
    figIdx = figIdx + 1;

    % bias plot
    figure(figIdx); clf;

    x_sigma = sqrt(P_diag_hist2(:,4));
    y_sigma = sqrt(P_diag_hist2(:,5));
    z_sigma = sqrt(P_diag_hist2(:,6));

    bias_x = x_hist2(:,4)*180/pi;
    bias_y = x_hist2(:,5)*180/pi;
    bias_z = x_hist2(:,6)*180/pi;

    plotLims = 1.25*[mean(x_sigma(1:floor(end/3))), mean(y_sigma(1:floor(end/3))), mean(z_sigma(1:floor(end/3)))];

    subplot(3,1,1);
    plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ...
        time, bias_x, 'b', time, (addedGyroBias-x_sigma)*180/pi, 'b-.', ...
        time, (addedGyroBias+x_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize);
    h = legend('Reference', 'Estimate', '1-sigma');
    set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');

    ylabel('x_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times');
    title('Bias estimates and 1-sigma distances of standard deviations', 'FontSize', 14, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(1) addedGyroBias+plotLims(1)]*180/pi);

    subplot(3,1,2);
    plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ...
        time, bias_y, 'b', time, (addedGyroBias-y_sigma)*180/pi, 'b-.', ...
        time, (addedGyroBias+y_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize);
    set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');

    ylabel('y_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(2) addedGyroBias+plotLims(2)]*180/pi);

    subplot(3,1,3);
    plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ...
        time, bias_z, 'b', time, (addedGyroBias-z_sigma)*180/pi, 'b-.', ...
        time, (addedGyroBias+z_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize);
    set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');

    xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
    ylabel('z_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(3) addedGyroBias+plotLims(3)]*180/pi);

    set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
    set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
    set(gcf,'PaperPositionMode','auto');
    saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']);
    print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps'])
    figIdx = figIdx + 1;

    pause(0.2);
end

%% statistics plot
figure(figIdx); clf;

subplot(3,1,1);
semilogy(testBiases*180/pi, yaw_DCM_RMSE, 'b*-', testBiases*180/pi, yaw_e1_RMSE, 'g^-', testBiases*180/pi, yaw_e2_RMSE, 'ro-', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');
legend('DCM', 'Madgwick', 'Mahony', 'Location', 'SouthEast');
ylabel('yaw RMSE (deg)', 'FontSize', 12, 'FontName', 'Times');
title(['RMSEs as a function of gyroscope bias (' testName ')'], 'FontSize', 14, 'FontName', 'Times');

subplot(3,1,2);
semilogy(testBiases*180/pi, pitch_DCM_RMSE, 'b*-', testBiases*180/pi, pitch_e1_RMSE, 'g^-', testBiases*180/pi, pitch_e2_RMSE, 'ro-', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('pitch RMSE (deg)', 'FontSize', 12, 'FontName', 'Times');

subplot(3,1,3);
semilogy(testBiases*180/pi, roll_DCM_RMSE, 'b*-', testBiases*180/pi, roll_e1_RMSE, 'g^-', testBiases*180/pi, roll_e2_RMSE, 'ro-', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');
xlabel('Constant gyroscope bias (deg/s)', 'FontSize', 12, 'FontName', 'Times');
ylabel('roll RMSE (deg)', 'FontSize', 12, 'FontName', 'Times');

set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
set(gcf,'PaperPositionMode','auto');
saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps'])

author: hhyyti

repo: dcm-imu

save_path: github-repos/MATLAB/hhyyti-dcm-imu


text:
function [model,output] = normalizeExponentialCone(model)

% N.B: YALMIP Definition % x2*exp(x1/x2)  <= x3

% Temporarily extract the data in a more standard name min c'*y, b + Ay >= 0
output.problem = 0;
if ~isempty(model.evalMap)

    % Temporarily extract the data in a more standard name min c'*y, b + Ay >= 0
    data.A = model.F_struc(:,2:end);
    data.b = full(model.F_struc(:,1));
    data.c = full(model.c);
    cones = model.K;
    % Clear away the Power and SDP cone to easier add new rows. Will be put back in
    % place later
    data.A(end-sum(model.K.p)-sum(model.K.s.^2)+1:end,:)=[];
    data.b(end-sum(model.K.p)-sum(model.K.s.^2)+1:end) = [];
    sdpData = model.F_struc(end-sum(model.K.p)-sum(model.K.s.^2)+1:end,:);

    % First check that we have only exponentials
    convexFunctions = [];
    concaveFunctions = [];
    vectorentropy = [];
    vectorkullback = [];
    vectorlogsumexp = [];
    for i = 1:length(model.evalMap)
        switch model.evalMap{i}.fcn
            case {'exp','pexp'}
                convexFunctions = [convexFunctions model.evalMap{i}.computes];
            case {'log','plog','slog'}
                concaveFunctions = [concaveFunctions model.evalMap{i}.computes];
            case 'entropy'
                concaveFunctions = [concaveFunctions model.evalMap{i}.computes];
                if length(model.evalMap{i}.variableIndex) > 1
                    vectorentropy = [vectorentropy i];
                end
            case 'kullbackleibler'
                convexFunctions = [convexFunctions model.evalMap{i}.computes];
                if length(model.evalMap{i}.variableIndex) > 2
                    vectorkullback = [vectorkullback i];
                end
            case 'logsumexp'
                convexFunctions = [convexFunctions model.evalMap{i}.computes];
                vectorlogsumexp = [vectorlogsumexp i];
            otherwise
                % Standard interface, return solver not applicable
                % This should not be able to happen as we check this
                % earlier
                output = createOutputStructure([],[],[],-4,model.solver.tag,[],[],0);
                return
        end
    end
    % Check that all exp/log enter in a convex fashion
    if any(model.K.f)
        if nnz(data.A(1:model.K.f,convexFunctions))>0 || nnz(data.A(1:model.K.f,concaveFunctions))>0
            output = createOutputStructure([],[],[],19,model.solver.tag,[],[],0);
            return
        end
    end
    % Check sign in objective on exp/log terms
    if any(data.c(convexFunctions) < 0) || any(data.c(concaveFunctions) > 0)
        output = createOutputStructure([],[],[],19,model.solver.tag,[],[],0);
        return
    end
    % Check sign in elementwise inequalities on exp/log terms
    if any(model.K.l)
        if nnz(data.A(1+model.K.f:model.K.f+model.K.l,convexFunctions)>0) || nnz(data.A(1+model.K.f:model.K.f+model.K.l,concaveFunctions)<0)
            output = createOutputStructure([],[],[],19,model.solver.tag,[],[],0);
            return
        end
    end
    % Check so there are no exp/log-terms inside cones
    if sum(model.K.q) + model.K.e + sum(model.K.p) + sum(model.K.s) > 0
        if nnz(model.F_struc(1+model.K.f+model.K.l:end,convexFunctions+1))+nnz(model.F_struc(1+model.K.f+model.K.l:end,concaveFunctions+1))>0
            output = createOutputStructure([],[],[],23,model.solver.tag,[],[],0);
            return
        end
    end
    % Scalarize entropy operator
    if ~isempty(vectorentropy)
        for i = vectorentropy(:)'
            involved = model.evalMap{i}.variableIndex;
            computes = model.evalMap{i}.computes;
            n = length(data.c);
            m = length(involved);
            data.A = [sparse(ones(1,m+1),[computes n+(1:m)],[-1 ones(1,m)],1,n+m);
                      data.A spalloc(size(data.A,1),m,0)];
            data.b = [0;data.b];
            data.c = [data.c;zeros(m,1)];
            cones.f = cones.f + 1;
            % The original strucuture now just relates to the first new term
            model.evalMap{i}.computes = n+1;
            model.evalMap{i}.variableIndex = involved(1);
            % and then we add some
            for j = 2:length(involved)
                model.evalMap{end+1} = model.evalMap{i};
                model.evalMap{end}.variableIndex = involved(j);
                model.evalMap{end}.computes = n+j;
            end
        end
    end

    % Scalarize kullbackleibler operator
    if ~isempty(vectorkullback)
        for i = vectorkullback(:)'
            involved = model.evalMap{i}.variableIndex;
            computes = model.evalMap{i}.computes;
            n = length(data.c);
            m = length(involved)/2;
            data.A = [sparse(ones(1,m+1),[computes n+(1:m)],[-1 ones(1,m)],1,n+m);
                      data.A spalloc(size(data.A,1),m,0)];
            data.b = [0;data.b];
            data.c = [data.c;zeros(m,1)];
            cones.f = cones.f + 1;
            % The original strucuture now just relates to the first new term
            model.evalMap{i}.computes = n+1;
            model.evalMap{i}.variableIndex = involved([1 m+1]);
            % and then we add some
            for j = 2:length(involved)/2
                model.evalMap{end+1} = model.evalMap{i};
                model.evalMap{end}.variableIndex = involved([j j+m]);
                model.evalMap{end}.computes = n+j;
            end
        end
    end

    % Scalarize logsumexp operator
    % write log(expx1+expx2..)<= z as exp(x1-z)+exp(x2-z)+...<=1
    if ~isempty(vectorlogsumexp)
        for i = vectorlogsumexp(:)'
            involved = model.evalMap{i}.variableIndex;
            computes = model.evalMap{i}.computes;
            % Orignal #variables
            n = length(data.c);
            % Number of terms in logsumexp
            m = length(involved);

            % exp(x1-z)+exp(x2-z)+...<=1
            data.A = [data.A(1:cones.f,:) spalloc(cones.f,m,0);
                      spalloc(1,n,0) -ones(1,m);
                      data.A(cones.f+1:end,:) spalloc(cones.l+sum(cones.q),m,0)];
            data.b = [data.b(1:cones.f);
                      1;
                      data.b(cones.f+1:end)];
            data.c = [data.c;zeros(m,1)];
            cones.l = cones.l + 1;

            % The original strucuture now just relates to the first new term
            model.evalMap{i}.computes = n+1;
            model.evalMap{i}.variableIndex = [involved(1) computes];
            model.evalMap{i}.fcn = 'expdiff';
            % and then we add some new
            for j = 2:length(involved)
                model.evalMap{end+1} = model.evalMap{i};
                model.evalMap{end}.variableIndex = [involved(j) computes];
                model.evalMap{end}.computes = n+j;
            end
        end
    end

    % Describe all exponential cones
    m = length(model.evalMap);
    cones.e = model.K.e + m;
    for i = 1:m
        switch model.evalMap{i}.fcn
            case 'exp'
                % 1*exp(xv/1) <= xc
                % y*exp(x/y)  <= z.  y new variable, xv the original variable, and xc the "computed"
                x = model.evalMap{i}.variableIndex;
                z = model.evalMap{i}.computes;
                data.A = [data.A;
                          -sparse([1;3],[x z],-1,3,size(data.A,2))];
                data.b = [data.b;[0;1;0]];
            case 'pexp'
                % xv(1)*exp(xv(2)/xv(1)) <= xc
                x = model.evalMap{i}.variableIndex(2);
                y = model.evalMap{i}.variableIndex(1);
                z = model.evalMap{i}.computes;
                data.A = [data.A;
                          -sparse([1;2;3],[x y z],[-1 -1 -1],3,size(data.A,2))];
                data.b = [data.b;[0;0;0]];
            case 'log'
                % log(xv) >= xc i.e. xv >= exp(xc/1)*1
                z = model.evalMap{i}.variableIndex;
                x = model.evalMap{i}.computes;
                data.A = [data.A;
                          -sparse([1;3],[x z],-1,3,size(data.A,2))];
                data.b = [data.b;[0;1;0]];
            case 'plog'
                % xv(1)log(xv(2)/xv(1))>=xc i.e.
                % -xc >= -xv(1)log(xv(2)/xv(1))
                z = model.evalMap{i}.computes;
                y = model.evalMap{i}.variableIndex(1);
                x = model.evalMap{i}.variableIndex(2);
                data.A = [data.A;
                          -sparse([1;2;3],[x y z],[1 -1 1],3,size(data.A,2))];
                data.b = [data.b;[0;0;0]];
            case 'slog'
                % log(1+xv) >= xc i.e. (1+xv) >= exp(xc/1)*1
                z = model.evalMap{i}.variableIndex;
                x = model.evalMap{i}.computes;
                data.A = [data.A;
                          -sparse([1;3],[x z],-1,3,size(data.A,2))];
                data.b = [data.b;0;1;1];
            case 'entropy'
                % -xv*log(xv)>=xc i.e. 1 >= exp(xc/xv)*xv
                x = model.evalMap{i}.computes;
                y = model.evalMap{i}.variableIndex;
                data.A = [data.A;
                          -sparse([1;2],[x y],[-1 -1],3,size(data.A,2))];
                data.b = [data.b;[0;0;1]];
            case 'kullbackleibler'
                % -xv1*log(xv1/xv2)>=-xc i.e. xv2 >= exp(-xc/xv1)*xv1
                x = model.evalMap{i}.computes;
                y = model.evalMap{i}.variableIndex(1);
                z = model.evalMap{i}.variableIndex(2);
                data.A = [data.A;
                          -sparse([1;2;3],[x y z],[1 -1 -1],3,size(data.A,2))];
                data.b = [data.b;[0;0;0]];
            case 'expdiff'
                % exp(xv(1)-xv(2)) <= xc
                x = model.evalMap{i}.variableIndex;
                z = model.evalMap{i}.computes;
                data.A = [data.A;
                          -sparse([1;1;3],[x(1) x(2) z],[-1 1 -1],3,size(data.A,2))];
                data.b = [data.b;[0;1;0]];
            otherwise
        end
    end
    model.F_struc = [data.b data.A];
    model.c = data.c;
    model.K = cones;
    % Put back the POWER+SDP cone in place
    if any(model.K.s) || any(model.K.p)
        if size(sdpData,2) < size(model.F_struc,2)
            sdpData(end,size(model.F_struc,2)) = 0;
        end
        model.F_struc = [model.F_struc;sdpData];
    end
end
n = size(model.F_struc,2)-1;
if n > length(model.lb)
    missing = n - length(model.lb);
    model.lb = [model.lb;-inf(missing,1)];
end
if size(model.F_struc,2)-1 > length(model.ub)
    missing = n - length(model.ub);
    model.ub = [model.ub;inf(missing,1)];
end

repo: YALMIP


text:
function ind = id2ind(ebsd,id)
% find ind such that ebsd.id(ind) == id
%
% Syntax
%   ind = id2ind(ebsd,id)
%
% Input
%  ebsd - @EBSD
%  id - a list of id's as stored in ebsd.id
%
% Output
%  ind - a list indeces such that ebsd.id(ind) == id

[~,ind] = ismember(id,ebsd.id);

author: mtex-toolbox

repo: mtex

save_path: github-repos/MATLAB/mtex-toolbox-mtex


text:
function model = dnetUpdateBeta(model)

% DNETUPDATEBETA Do an M-step (update parameters) on an Density Network model.
% FORMAT
% DESC updates the parameters of a Density Network model.
% ARG model : the model which is to be updated.
% RETURN model : the model with updated parameters.
%
% SEEALSO : dnetCreate, dnetUpdateOutputWeights
%
% COPYRIGHT : Neil D. Lawrence, 2008

% MLTOOLS

  Ypred = dnetOut(model);
  tot = 0;
  if model.N > model.M
    for i = 1:model.M
      diffY = model.y - repmat(Ypred(i, :), model.N, 1);
      diffY =diffY.*diffY.*repmat(model.w(:,i), 1, model.d);
      tot = tot + sum(sum(diffY));
    end
  else
    for i = 1:model.N
      diffY = repmat(model.y(i, :), model.M, 1) - Ypred;
      diffY = diffY.*diffY.*repmat(model.w(i,:)', 1, model.d);
      tot = tot + sum(sum(diffY));
    end
  end
  model.beta = (model.N*model.d)/tot;

author: SheffieldML

repo: GPmat

save_path: github-repos/MATLAB/SheffieldML-GPmat


text:
% This file is part of the following project:
% J. Zico Kolter. Tommi Jaakkola.
% Approximate Inference in Additive Factorial HMMs with Application to Energy Disaggregation.
% In: International Conference on Artificial Intelligence and Statistics (AISTATS). 2012.
% Code provided as supplementary material
% Copyright: J. Zico Kolter, MIT CSAIL, 2014.

% Modified by Romano Cicchetti, ETH Zurich, in the context of the NILM-Eval project

function [data] = kolter(days, setup, fid)

    % load parameters
    dataset = setup.dataset;
    interval = setup.granularity;
    household = setup.household;
    lratio = setup.filtLRatio;

    % set variables
    edgeThreshold = 5;
    plevelMinLength = 5;

    % get total power consumption
    evaluation_days = days{1};
    true_power = read_smartmeter_data(dataset, num2str(household, '%02d'), evaluation_days, interval, 'powerallphases');

    % total variation denoising
    lmax = tvdiplmax(true_power);
    [data, ~, ~] = tvdip(true_power, lmax*lratio, 1, 1e-3, 100);
    data = data';

    % generate power levels
    plevel = generatePlevels(edgeThreshold, plevelMinLength, data, true_power);

    % generate snippets from power levels
    snippets = generateSnippetsFromPlevels(plevel);

    % generate HMMs from snippets
    HMMs = generateHMMsFromSnippets(snippets);

    % calculate probability of one snippet generating another
    numOfSnippets = length(snippets.mean);
    probsBetweenAllSnippets = zeros(numOfSnippets, numOfSnippets);
    for i = 1:numOfSnippets
        log_trans = log(cell2mat(HMMs.transition(i)));
        mu = cell2mat(HMMs.mean(i));
        sigma = cell2mat(HMMs.std(i));
        numOfStates = length(cell2mat(snippets.mean(i))) + 1;
        for j = 1:numOfSnippets
            if numOfStates ~= length(cell2mat(snippets.mean(j))) + 1
                continue;
            end
            firstPlevelOfSnippet = snippets.start(j);
            lastPlevelOfSnippet = snippets.end(j);
            observation = data(plevel.startidx(firstPlevelOfSnippet):plevel.endidx(lastPlevelOfSnippet)) - ...
                plevel.mean(firstPlevelOfSnippet - 1);
            emit = zeros(numOfStates, length(observation));
            for state = 1:numOfStates
                emit(state, :) = normpdf(observation, mu(state), sigma(state));
            end
            log_emit = log(emit);

            maxProb = zeros(numOfStates, length(observation));
            maxProb(:,1) = log(0);
            maxProb(1,1) = log(1);
            for t = 2:length(observation)
                for k = 1:numOfStates
                    sum = maxProb(:,t-1) + log_trans(:,k) + log_emit(k,t);
                    maxProb(k,t) = max(sum);
                end
                if all(isinf(maxProb(:,t)))
                    break;
                end
            end
            probsBetweenAllSnippets(i,j) = max(maxProb(:,length(observation)));
        end
    end

    % build adjacency Matrix from k-nearest neighbor graph
    numOfStatesOfSnippets = cellfun(@length, HMMs.mean);
    mu = {};
    P = {};
    for numOfStates = 2:5
        idxOfHMMs = find(numOfStatesOfSnippets == numOfStates);
        meansOfHMMs = HMMs.mean(idxOfHMMs);
        transitionOfHMMs = HMMs.transition(idxOfHMMs);
        probsBetweenSelectedSnippets = probsBetweenAllSnippets(idxOfHMMs, idxOfHMMs);
        adjacencyMatrix = zeros(size(probsBetweenSelectedSnippets));
        for i = 1:length(idxOfHMMs)
            idx_nonzero = find(probsBetweenSelectedSnippets(:,i) ~= 0);
            numSnippets = length(idx_nonzero);
            if numSnippets == 0
                continue;
            end
             [~,sortedIdx] = sort(probsBetweenSelectedSnippets(idx_nonzero,i), 'descend');
            sortedIdx = sortedIdx(1:1+floor(numSnippets/4));
            adjacencyMatrix(idx_nonzero(sortedIdx),i) = 1;
        end

        numOfClusters = floor(sqrt(length(idxOfHMMs)/2));
        if numOfClusters == 0
            continue;
        end
        clusters = SpectralClustering(adjacencyMatrix, numOfClusters, 1);

        for cl = 1:numOfClusters
            idx = clusters(:,cl) == 1;
            selectedMeans = cell2mat(meansOfHMMs(idx)');
            mu{end+1} = mean(selectedMeans,2)';
            selectedTrans = transitionOfHMMs(idx);
            Trans = cat(3,selectedTrans{:});
            P{end+1} = mean(Trans,3);
        end
    end

    % write cluster centorids (HMMs) to text file
    fprintf(fid,'%20s\n', 'MEANS:');
    for i = 1:length(mu)
        fprintf(fid, '%f\t', cell2mat(mu(i)));
        fprintf(fid, '\n');
    end

    % run AFAMAP algorithm
    n = 1;
    paramsAfamap = struct;
    paramsAfamap.max_iter = 1;
    paramsAfamap.lambda = Inf;
    paramsAfamap.dlambda = Inf;
    paramsAfamap.dSig = var(diff(data));
    paramsAfamap.Sig = var(data);
    [X0,Z,G] = myAfamap(plevel.mean', mu, plevel.duration', P, paramsAfamap);

end

author: beckel

repo: nilm-eval

save_path: github-repos/MATLAB/beckel-nilm-eval


text:
function quad_mesh_test02 ( )

%*****************************************************************************80
%
%% QUAD_MESH_TEST02 tests AREA_Q4_MESH.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'QUAD_MESH_TEST02\n' );
  fprintf ( 1, '  AREA_Q4_MESH computes the area of each element\n' );
  fprintf ( 1, '  in a Q4 mesh.\n' );

  [ node_num, element_num, hole_num ] = example2_q4_mesh_size ( );

  [ node_xy, element_node, element_neighbor ] = example2_q4_mesh ( ...
    node_num, element_num );

  [ element_area, mesh_area ] = area_q4_mesh ( node_num, element_num, ...
    node_xy, element_node );

  r8vec_print ( element_num, element_area, '  Element areas:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   Mesh =   %f\n', mesh_area );

  return
end

author: johannesgerer

repo: jburkardt-m


text:
function [y,LL] = delta2static_ML(mu, variance, delta_order, dimS)
[nFr, dim] = size(mu);
nDelta = dim/dimS;

if length(delta_order)==1 && nDelta > 1
    delta_order = ones(nDelta, 1) * delta_order;
end

D = genDeltaTransform(nFr, delta_order(1));
A = D*D;

for i = 1:dimS
    ScaleS = diag(1./variance(:,i));
    ScaleD = diag(1./variance(:,i+dimS));
    ScaleA = diag(1./variance(:,i+dimS*2));

    R = ScaleS + D'*ScaleD*D + A'*ScaleA*A;
    p = ScaleS * mu(:,i) + D' * ScaleD * mu(:,dimS+i) + A' * ScaleA * mu(:,dimS*2+i);
    y(:,i) = inv(R) * p;
end

if 1
    X = comp_dynamic_feature(mu(:,1:dimS), 2, 2);
    Y = comp_dynamic_feature(y, 2, 2);
    XE = (X-mu).^2 ./ variance;
    YE = (Y-mu).^2 ./ variance;

    LL(:,1) = mean(XE);
    LL(:,2) = mean(YE);

end
end

author: singaxiong

repo: SignalGraph


text:
function nmt_addtopo(cfgplot,erp)
% configures and display topoplot on nutmegtrip viewer
% (not intended for use outside of nutmegtrip plot functions)

global st
figure(st.fig);
if(verLessThan('matlab','8.4')) % necessary to preserve colormap on functional image for Matlab R2014a and earlier
    for ii=1:3
        freezeColors(st.vols{1}.ax{ii}.ax);
    end
end

newway = true
if(newway)
    axes(st.nmt.gui.ax_ts(2,1));
    set(st.nmt.gui.ax_ts(2,1),'Visible');
    cfgplot.interactive = 'no';
    % cfgplot.colorbar = 'yes';  % how can we transfer colorbar over?
    cfgplot.comment = 'no';
    ft_topoplotER(cfgplot,erp);

    set(st.nmt.gui.ax_ts(2,1),'Units','pixels','Position',st.nmt.gui.ax_topo_pos);
    colormap(st.nmt.gui.ax_ts(2,1),jet)

else
    if(isfield(st.nmt.gui,'ax_topo'))
        delete(st.nmt.gui.ax_topo);
    end
    topofig = figure('Visible','off');
    cfgplot.interactive = 'no';
    % cfgplot.colorbar = 'yes';  % how can we transfer colorbar over?
    cfgplot.comment = 'no';
    ft_topoplotER(cfgplot,erp);

    st.nmt.gui.ax_topo = copyobj(gca,st.fig);
    delete(topofig);

    set(st.nmt.gui.ax_topo,'Units','pixels','Position',st.nmt.gui.ax_topo_pos);
    colormap(st.nmt.gui.ax_topo,jet)
end
end

author: fieldtrip

repo: fieldtrip

save_path: github-repos/MATLAB/fieldtrip-fieldtrip


text:
%RPY2R Roll-pitch-yaw angles to SO(3) rotation matrix
%
% R = RPY2R(ROLL, PITCH, YAW, OPTIONS) is an SO(3) orthonornal rotation
% matrix (3x3) equivalent to the specified roll, pitch, yaw angles angles.
% These correspond to rotations about the Z, Y, X axes respectively. If
% ROLL, PITCH, YAW are column vectors (Nx1) then they are assumed to
% represent a trajectory and R is a three-dimensional matrix (3x3xN), where
% the last index corresponds to rows of ROLL, PITCH, YAW.
%
% R = RPY2R(RPY, OPTIONS) as above but the roll, pitch, yaw angles are
% taken from the vector (1x3) RPY=[ROLL,PITCH,YAW]. If RPY is a matrix
% (Nx3) then R is a three-dimensional matrix (3x3xN), where the last index
% corresponds to rows of RPY which are assumed to be [ROLL,PITCH,YAW].
%
% Options::
%  'deg'      Compute angles in degrees (radians default)
%  'xyz'      Rotations about X, Y, Z axes (for a robot gripper)
%  'zyx'      Rotations about Z, Y, X axes (for a mobile robot, default)
%  'yxz'      Rotations about Y, X, Z axes (for a camera)
%  'arm'      Rotations about X, Y, Z axes (for a robot arm)
%  'vehicle'  Rotations about Z, Y, X axes (for a mobile robot)
%  'camera'   Rotations about Y, X, Z axes (for a camera)
%
% Note::
% - Toolbox rel 8-9 has XYZ angle sequence as default.
% - ZYX order is appropriate for vehicles with direction of travel in the X
%   direction.  XYZ order is appropriate if direction of travel is in the Z
%   direction.
% - 'arm', 'vehicle', 'camera' are synonyms for 'xyz', 'zyx' and 'yxz'
%   respectively.
%
% See also TR2RPY, EUL2TR.

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math

function R = rpy2r(roll, varargin)
    opt.order = {'zyx', 'xyz', 'yxz', 'arm', 'vehicle', 'camera'};
    opt.deg = false;
    [opt,args] = tb_optparse(opt, varargin);

    % unpack the arguments
    if numcols(roll) == 3
		pitch = roll(:,2);
		yaw = roll(:,3);
		roll = roll(:,1);
	elseif nargin >= 3
        pitch = args{1};
        yaw = args{2};
    else
        error('SMTB:rpy2r:badarg', 'bad arguments')
    end

    % optionally convert from degrees
    if opt.deg
        d2r = pi/180.0;
        roll = roll * d2r;
        pitch = pitch * d2r;
        yaw = yaw * d2r;
    end

    switch opt.order
        case {'xyz', 'arm'}
            % XYZ order
            if numrows(roll) == 1
                R = rotx(yaw) * roty(pitch) * rotz(roll);
            else
                R = zeros(3,3,numrows(roll));
                for i=1:numrows(roll)
                    R(:,:,i) = rotx(yaw(i)) * roty(pitch(i)) * rotz(roll(i));
                end
            end
        case {'zyx', 'vehicle'}
            % ZYX order
            if numrows(roll) == 1
                R = rotz(yaw) * roty(pitch) * rotx(roll);
            else
                R = zeros(3,3,numrows(roll));
                for i=1:numrows(roll)
                    R(:,:,i) = rotz(yaw(i)) * roty(pitch(i)) * rotx(roll(i));
                end
            end

        case {'yxz', 'camera'}
            % YXZ order
            if numrows(roll) == 1
                R = roty(yaw) * rotx(pitch) * rotz(roll);
            else
                R = zeros(3,3,numrows(roll));
                for i=1:numrows(roll)
                    R(:,:,i) = roty(yaw(i)) * rotx(pitch(i)) * rotz(roll(i));
                end
            end
    end
end

author: petercorke

repo: spatialmath-matlab


text:
close all
clear all
clc
diary('results1.txt')
n=0;            %initialize iteration counter
e = 1e-6;       %epsilon

%% problem 1a
f = @(x) 2*x(1) - 3*x(2);				%f(x)
G = @(x) [2; -3];						%Gradient of f(x)
H = @(x) [0, 0; 0, 0];					%Hessian of f(x)
g = @(x) x(1)^2 + x(2)^2 - 25;			%g(x)
Dg = @(x) [2*x(1); 2*x(2)];				%gradient of g(x)
Hg = {@(x) [2, 0; 0, 2]};				% Hessian of g(x)
x0 = [-2.8; 4.2];        				% set starting value
xt = [-10/sqrt(13); 15/sqrt(13)]; 		%true value
disp('Problem 1a');
[x, itr] = newton_constrained(x0, f, g, G, H, Dg, Hg, e, xt, 1);

%% Problem 1b
f = @(x) x(1)^2 + 2*x(1)*x(2) + x(2)^2;			%f(x)
G = @(x) [2*x(1) + 2*x(2); 2*x(2) + 2*x(1)];    %Gradient of f(x)
H = @(x) [2, 2; 2, 2];                          %Hessian of f(x)
g = @(x) 3*x(1)^2 + x(2)^2 - 9;                 %g(x)
Dg = @(x) [6*x(1); 2*x(2)];                     %gradient of g(x)
Hg = {@(x) [6, 0; 0, 2]};               		% Hessian of g(x)
x0 = [-1.6; 1.6];                           % set starting value
xt = [-1.5; 1.5];                               %true value
disp('Problem 1b');
[x, itr] = newton_constrained(x0, f, g, G, H, Dg, Hg, e, xt, 1);

%% Problem 1c
f = @(x) 3*x(1)^3 + 2*x(2)^3 + x(3)^3 + x(4)^3;                                           %f(x)
G = @(x) [9*x(1)^2; 6*x(2)^2; 3*x(3)^2; 3*x(4)^2];                                        %Gradient of f(x)
H = @(x) [18*x(1), 0, 0, 0; 0, 12*x(2)^2, 0, 0; 0, 0, 6*x(3), 0; 0, 0, 0, 6*x(4)];        %Hessian of f(x)
g = @(x) [x(1)^2 + x(2)^2 + x(3)^2 + x(4)^2 - 4; x(1) + x(2) + 2*x(3) + 3*x(4) - 1];      %g(x)
Dg = @(x) [2*x(1), 1; 2*x(2), 1; 2*x(3), 2; 2*x(4), 3];                                   %gradient of g(x)
Hg = {@(x) eye(4)*2; @(x) eye(4)*0};                                                      % Hessian of g(x)
x0 =[-1.84; 0.21; 0.42; 0.59];        %set starting value                                 % set starting value
xt = [-1.84931; 0.20806; 0.41612; 0.603003];                                              %true value
disp('Problem 2a');
[x, itr] = newton_constrained(x0, f, g, G, H, Dg, Hg, e, xt, 1);
diary off

author: Sable

repo: mcbench-benchmarks


text:
function blks = toBlocks__(im, w, mode)
% Crop blocks with no padding
% Set the anchor at the center of each block
hw = floor((w-1)/2);
offsetU = hw; offsetL = hw;
offsetD = w - hw - 1;
offsetR = w - hw - 1;

[rows, cols, chns] = size(im);
if strcmp(mode, 'distinct')
    step = w;
elseif strcmp(mode, 'sliding')
    step = 1;
else
    error('Invalid mode');
end

iiSeries = 1+offsetU:step:rows-offsetD;
jjSeries = 1+offsetL:step:cols-offsetR;
blks = zeros(length(iiSeries) * length(jjSeries), w*w*chns);
cnt = 1;    % Counter
for jj = jjSeries
    for ii = iiSeries
        blks(cnt, :) = reshape(im(ii-offsetU:ii+offsetD, jj-offsetL:jj+offsetR, :), 1, []);
        cnt = cnt + 1;
    end
end
end

author: Bobholamovic

repo: ChangeDetectionToolbox


text:
function plotExcitationIRF(varargin)
% Plots the excitation IRF for each hydro structure's bodies in
% the heave, surge and pitch degrees of freedom.
%
% Usage:
% ``plotExcitationIRF(hydro, hydro2, hydro3, ...)``
%
% Parameters
% ----------
%     varargin : struct(s)
%         The hydroData structure(s) created by the other BEMIO functions.
%         One or more may be input.
%

if isempty(varargin)
    error(['plotExcitationIRF: No arguments passed. Include one or more hydro ' ...
        'structures when calling: plotExcitationIRF(hydro1, hydro2, ...)']);
end

B=1;  % Wave heading index
figHandle = figure('Position',[950,100,975,521]);
titleString = ['Normalized Excitation Impulse Response Functions:   ',...
    '$$\bar{K}_i(t) = {\frac{1}{2\pi}}\int_{-\infty}^{\infty}{\frac{X_i(\omega,\theta)e^{i{\omega}t}}{{\rho}g}}d\omega$$'];
subtitleStrings = {'Surge','Heave','Pitch'};
xString = {'$$t (s)$$','$$t (s)$$','$$t (s)$$'};
yString = {['$$\bar{K}_1(t,\theta$$',' = ',num2str(varargin{1}.theta(B)),'$$^{\circ}$$)'],...
    ['$$\bar{K}_3(t,\theta$$',' = ',num2str(varargin{1}.theta(B)),'$$^{\circ}$$)'],...
    ['$$\bar{K}_5(t,\theta$$',' = ',num2str(varargin{1}.theta(B)),'$$^{\circ}$$)']};

notes = {'Notes:',...
    ['$$\bullet$$ The IRF should tend towards zero within the specified timeframe. ',...
    'If it does not, attempt to correct this by adjusting the $$\omega$$ and ',...
    '$$t$$ range and/or step size used in the IRF calculation.'],...
    ['$$\bullet$$ Only the IRFs for the first wave heading, surge, heave, and ',...
    'pitch DOFs are plotted here. If another wave heading or DOF is significant ',...
    'to the system, that IRF should also be plotted and verified before proceeding.']};

numHydro = length(varargin);
for ii = 1:numHydro
    numBod = varargin{ii}.Nb;
    tmp1 = strcat('X',num2str(ii));
    X.(tmp1) = varargin{ii}.ex_t;
    tmp2 = strcat('Y',num2str(ii));
    a = 0;
    for i = 1:numBod
        m = varargin{ii}.dof(i);
        Y.(tmp2)(1,i,:) = squeeze(varargin{ii}.ex_K(a+1,B,:));
        Y.(tmp2)(2,i,:) = squeeze(varargin{ii}.ex_K(a+3,B,:));
        Y.(tmp2)(3,i,:) = squeeze(varargin{ii}.ex_K(a+5,B,:));
        legendStrings{i,ii} = [varargin{ii}.body{i}];
        a = a + m;
    end
end

formatPlot(figHandle,titleString,subtitleStrings,xString,yString,X,Y,legendStrings,notes)
saveas(figHandle,'Excitation_IRFs.png');

end

author: WEC-Sim

repo: WEC-Sim

save_path: github-repos/MATLAB/WEC-Sim-WEC-Sim


text:
% GENERATE VARIOUS TIME AND FREQUENCY-VARYING TEST SIGNALS
%
% Three types of signals can be generated:
%
%   1. Deterministic signals (s(t))
%   2. Noisy signals (n(t))
%   3. Deterministic signals in additive noise (s(t) + n(t))
%   4. Analytic signals
%   4. Demo signals
%
%
% 1. The type of deterministic signals s(t) that can be generated are:
%
%   (i)   Linear Frequency Modulated (FM)
%   (ii)  Quadratic FM
%   (iii) Cubic FM
%   (iv)  Stepped FM
%   (v)   Sinusoidal FM
%   (vi)  Hyperbolic FM
%
%   The sampling frequency and length of the signal can be specified.
%   Also the frequency bounds (starting and ending frequency) can be
%   specified.
%
% 2. Noise types n(t) include:
%
%    (i) Uniformly distributed random variable
%    (ii) Gaussian distributed random variable
%
% 3. The signal s(t) can be added to noise n(t).
%
% 4. An analytic associate (complex) can be generated from a real
%    input time domain signal.
%
% 5. Various types of demonstration signals can be loaded.
%
%
%
%  See Also:  gsig, analyt

author: HeLiangHIT