text:
%kiopaque 'Opaquens image '
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros iopaque.pane file
%
% Parameters:
% InputFile: i 'Input ', required: 'First Input data object'
% OutputFile: o 'Output', required: 'Resulting output data object'
% Double: scale 'Scaling Factor', default: 1: 'float'
%
% Example: o = kiopaque(i, {'i','';'o','';'scale',1})
%
% Khoros helpfile follows below:
%
%  PROGRAM
% iopaque - no short description specified
%
%  DESCRIPTION
%
%
%
%  EXAMPLES
%
%  "SEE ALSO"
%
%  RESTRICTIONS
%
%  REFERENCES
%
%  COPYRIGHT
% Copyright (C) 1993 - 1997, Khoral Research, Inc. ("KRI")  All rights reserved.
%

function varargout = kiopaque(varargin)
if nargin ==0
  Inputs={};arglist={'',''};
elseif nargin ==1
  Inputs=varargin{1};arglist={'',''};
elseif nargin ==2
  Inputs=varargin{1}; arglist=varargin{2};
else error('Usage: [out1,..] = kiopaque(Inputs,arglist).');
end
if size(arglist,2)~=2
  error('arglist must be of form {''ParameterTag1'',value1;''ParameterTag2'',value2}')
 end
narglist={'i', '__input';'o', '__output';'scale', 1};
maxval={0,0,2};
minval={0,0,2};
istoggle=[0,0,0];
was_set=istoggle * 0;
paramtype={'InputFile','OutputFile','Double'};
% identify the input arrays and assign them to the arguments as stated by the user
if ~iscell(Inputs)
Inputs = {Inputs};
end
NumReqOutputs=1; nextinput=1; nextoutput=1;
  for ii=1:size(arglist,1)
  wasmatched=0;
  for jj=1:size(narglist,1)
   if strcmp(arglist{ii,1},narglist{jj,1})  % a given argument was matched to the possible arguments
     wasmatched = 1;
     was_set(jj) = 1;
     if strcmp(narglist{jj,2}, '__input')
      if (nextinput > length(Inputs))
        error(['Input ' narglist{jj,1} ' has no corresponding input!']);
      end
      narglist{jj,2} = 'OK_in';
      nextinput = nextinput + 1;
     elseif strcmp(narglist{jj,2}, '__output')
      if (nextoutput > nargout)
        error(['Output nr. ' narglist{jj,1} ' is not present in the assignment list of outputs !']);
      end
      if (isempty(arglist{ii,2}))
        narglist{jj,2} = 'OK_out';
      else
        narglist{jj,2} = arglist{ii,2};
      end

      nextoutput = nextoutput + 1;
      if (minval{jj} == 0)
         NumReqOutputs = NumReqOutputs - 1;
      end
     elseif isstr(arglist{ii,2})
      narglist{jj,2} = arglist{ii,2};
     else
        if strcmp(paramtype{jj}, 'Integer') & (round(arglist{ii,2}) ~= arglist{ii,2})
            error(['Argument ' arglist{ii,1} ' is of integer type but non-integer number ' arglist{ii,2} ' was supplied']);
        end
        if (minval{jj} ~= 0 | maxval{jj} ~= 0)
          if (minval{jj} == 1 & maxval{jj} == 1 & arglist{ii,2} < 0)
            error(['Argument ' arglist{ii,1} ' must be bigger or equal to zero!']);
          elseif (minval{jj} == -1 & maxval{jj} == -1 & arglist{ii,2} > 0)
            error(['Argument ' arglist{ii,1} ' must be smaller or equal to zero!']);
          elseif (minval{jj} == 2 & maxval{jj} == 2 & arglist{ii,2} <= 0)
            error(['Argument ' arglist{ii,1} ' must be bigger than zero!']);
          elseif (minval{jj} == -2 & maxval{jj} == -2 & arglist{ii,2} >= 0)
            error(['Argument ' arglist{ii,1} ' must be smaller than zero!']);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} < minval{jj})
            error(['Argument ' arglist{ii,1} ' must be bigger than ' num2str(minval{jj})]);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} > maxval{jj})
            error(['Argument ' arglist{ii,1} ' must be smaller than ' num2str(maxval{jj})]);
          end
        end
     end
     if ~strcmp(narglist{jj,2},'OK_out') &  ~strcmp(narglist{jj,2},'OK_in')
       narglist{jj,2} = arglist{ii,2};
     end
   end
   end
   if (wasmatched == 0 & ~strcmp(arglist{ii,1},''))
        error(['Argument ' arglist{ii,1} ' is not a valid argument for this function']);
   end
end
% match the remaining inputs/outputs to the unused arguments and test for missing required inputs
 for jj=1:size(narglist,1)
     if  strcmp(paramtype{jj}, 'Toggle')
        if (narglist{jj,2} ==0)
          narglist{jj,1} = '';
        end;
        narglist{jj,2} = '';
     end;
     if  ~strcmp(narglist{jj,2},'__input') && ~strcmp(narglist{jj,2},'__output') && istoggle(jj) && ~ was_set(jj)
          narglist{jj,1} = '';
          narglist{jj,2} = '';
     end;
     if strcmp(narglist{jj,2}, '__input')
      if (minval{jj} == 0)  % meaning this input is required
        if (nextinput > size(Inputs))
           error(['Required input ' narglist{jj,1} ' has no corresponding input in the list!']);
        else
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        end
      else  % this is an optional input
        if (nextinput <= length(Inputs))
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end;
     else
     if strcmp(narglist{jj,2}, '__output')
      if (minval{jj} == 0) % this is a required output
        if (nextoutput > nargout & nargout > 1)
           error(['Required output ' narglist{jj,1} ' is not stated in the assignment list!']);
        else
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
          NumReqOutputs = NumReqOutputs-1;
        end
      else % this is an optional output
        if (nargout - nextoutput >= NumReqOutputs)
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end
     end
  end
end
if nargout
   varargout = cell(1,nargout);
else
  varargout = cell(1,1);
end
global KhorosRoot
if exist('KhorosRoot') && ~isempty(KhorosRoot)
w=['"' KhorosRoot];
else
if ispc
  w='"C:\Program Files\dip\khorosBin\';
else
[s,w] = system('which cantata');
w=['"' w(1:end-8)];
end
end
[varargout{:}]=callKhoros([w 'iopaque"  '],Inputs,narglist);

author: aludnam

repo: MATLAB

save_path: github-repos/MATLAB/aludnam-MATLAB


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

% 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;

% Add bias to calibrated gyro values (same for each axis)
addedGyroBias = 0;

% load measurement data
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;

ILdeltaTime = [0; ILtime(2:end) - ILtime(1:end-1)];
SFdeltaTime = [0; SFtime(2:end) - SFtime(1:end-1)];
KdeltaTime = [0; Ktime(2:end) - Ktime(1:end-1)];

%start yaw from zero at time 468s (start of tests after calibration sequences in example data set)
StartIdx = find(ILtime > 464, 1, 'first');
StopIdx = find(ILtime > 774, 1, 'first');
time = ILtime(StartIdx:StopIdx);

%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)

vel_x = [0; x(2:end)-x(1:end-1)] ./ KdeltaTime;
vel_y = [0; y(2:end)-y(1:end-1)] ./ KdeltaTime;
vel_z = [0; z(2:end)-z(1:end-1)] ./ KdeltaTime;

% 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

% Resample reference data of KUKA robot 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;

%computing ypr for InertiaLink internal estimates (Rotation matrix R = -M')
[yaw_IL, pitch_IL, roll_IL] = yawpitchroll(-data.InertiaLink.M11, ...
    data.InertiaLink.M12, -data.InertiaLink.M13, -data.InertiaLink.M23, ...
    -data.InertiaLink.M33);
yaw_IL = un_modulo(yaw_IL, 2*pi); % remove 2pi jumps and make yaw continuous
yaw_IL = yaw_IL - yaw_IL(1); % starting yaw from zero (same as IMU estimates)

%% calibrate IMU data
imusWithKukaCalibration;

acc1 = [acc1_x, acc1_y, acc1_z];
acc2 = [acc2_x, acc2_y, acc2_z];

gyro1 = [w1_x, w1_y, w1_z];
gyro2 = [w2_x, w2_y, w2_z];

% add constant bias for testing purposes (if addedGyroBias is set above)
gyro1 = gyro1 + addedGyroBias*ones(size(gyro1));
gyro2 = gyro2 + addedGyroBias*ones(size(gyro2));

%% DCM IMU results for both imu datas
tic();
if (use_C_versions)
    [x_hist1, ypr_hist1, a_hist1, P_diag_hist1] = ...
        DCM_IMU_uC(gyro1, acc1, SFdeltaTime);
else
    IMU_DCM1 = DCM_IMU();
    x_hist1 = zeros(length(SFtime), 6);
    ypr_hist1 = zeros(length(SFtime), 3);
    P_diag_hist1 = zeros(length(SFtime), 6);
    for t = 1:length(SFtime)
        IMU_DCM1.UpdateIMU(gyro1(t,:), acc1(t,:), SFdeltaTime(t));	% gyroscope units must be radians
        x_hist1(t, :) = IMU_DCM1.state';
        ypr_hist1(t, :) = [IMU_DCM1.yaw, IMU_DCM1.pitch, IMU_DCM1.roll];
        P_diag_hist1(t, :) = diag(IMU_DCM1.P)';
    end
end
tDCM1 = toc()

tic();
if (use_C_versions)
    [x_hist2, ypr_hist2, a_hist2, P_diag_hist2] = ...
        DCM_IMU_uC(gyro2, acc2, ILdeltaTime);
else
    IMU_DCM2 = DCM_IMU();
    x_hist2 = zeros(length(ILtime), 6);
    ypr_hist2 = zeros(length(ILtime), 3);
    P_diag_hist2 = zeros(length(ILtime), 6);
    for t = 1:length(ILtime)
        IMU_DCM2.UpdateIMU(gyro2(t,:), acc2(t,:), ILdeltaTime(t));	% gyroscope units must be radians
        x_hist2(t, :) = IMU_DCM2.state';
        ypr_hist2(t, :) = [IMU_DCM2.yaw, IMU_DCM2.pitch, IMU_DCM2.roll];
        P_diag_hist2(t, :) = diag(IMU_DCM2.P)';
    end
end
tDCM2 = toc()

%% computing Madgwick and Mahony imu algorithms for comparison
if (use_comparisonAlgorithms)
    addpath('quaternion_library');      % include quaternion library

    tic();
    if (use_C_versions)
        quat_mad_B = Madgwick_IMU_C(gyro1, acc1);
    else
        IMU_mad_B = MadgwickAHRS('SamplePeriod', 1/150, 'Beta', 0.1);
        quat_mad_B = zeros(length(SFtime), 4);
        for t = 1:length(quat_mad_B)
            IMU_mad_B.UpdateIMU(gyro1(t,:), acc1(t,:));	% gyroscope units must be radians
            quat_mad_B(t, :) = IMU_mad_B.Quaternion;
        end
    end
    tIMU_mad_B = toc()

    tic();
    if (use_C_versions)
        quat_mah_B = Mahony_IMU_C(gyro1, acc1);
    else
        IMU_mah_B = MahonyAHRS('SamplePeriod', 1/150, 'Kp', 0.5);
        quat_mah_B = zeros(length(SFtime), 4);
        for t = 1:length(quat_mah_B)
            IMU_mah_B.UpdateIMU(gyro1(t,:), acc1(t,:));	% gyroscope units must be radians
            quat_mah_B(t, :) = IMU_mah_B.Quaternion;
        end
    end

    tIMU_mah_B = toc()

    tic();
    if (use_C_versions)
        quat_mad_A = Madgwick_IMU_C(gyro2, acc2);
    else
        IMU_mad_A = MadgwickAHRS('SamplePeriod', 1/150, 'Beta', 0.1);
        quat_mad_A = zeros(length(ILtime), 4);
        for t = 1:length(quat_mad_A)
            IMU_mad_A.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
            quat_mad_A(t, :) = IMU_mad_A.Quaternion;
        end
    end
    tIMU_mad_A = toc()

    tic();
    if (use_C_versions)
        quat_mah_A = Mahony_IMU_C(gyro2, acc2);
    else
        IMU_mah_A = MahonyAHRS('SamplePeriod', 1/150, 'Kp', 0.5);
        quat_mah_A = zeros(length(ILtime), 4);
        for t = 1:length(quat_mah_A)
            IMU_mah_A.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
            quat_mah_A(t, :) = IMU_mah_A.Quaternion;
        end
    end
    tIMU_mah_A = toc()

    % 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.
    euler_mad_B = quatern2euler(quaternConj(quat_mad_B)) * (180/pi);
    euler_mah_B = quatern2euler(quaternConj(quat_mah_B)) * (180/pi);
    euler_mad_A = quatern2euler(quaternConj(quat_mad_A)) * (180/pi);
    euler_mah_A = quatern2euler(quaternConj(quat_mah_A)) * (180/pi);
else
    euler_mad_B = nan(length(gyro1),3);
    euler_mah_B = nan(length(gyro1),3);
    euler_mad_A = nan(length(gyro2),3);
    euler_mah_A = nan(length(gyro2),3);

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

%% resampling & remove 2pi jumps
warning('off','interpolation:interpolation:noextrap');

%resample SparkFun data to InertiaLink time
yaw_sf_in_IL = resample(timeseries(un_modulo(ypr_hist1(:,1),2*pi), SFtime),time);
yaw_sf_in_IL = yaw_sf_in_IL.Data*180/pi;
yaw_sf_in_IL = yaw_sf_in_IL - yaw_sf_in_IL(1); %starting yaw from zero (same as IMU estimates)
pitch_sf_in_IL = resample(timeseries(ypr_hist1(:,2), SFtime),time);
pitch_sf_in_IL = pitch_sf_in_IL.Data*180/pi;
roll_sf_in_IL = resample(timeseries(ypr_hist1(:,3), SFtime),time);
roll_sf_in_IL = roll_sf_in_IL.Data*180/pi;

% start yaw from zero at the beginning of test sequence
yaw_IL = yaw_IL - yaw_IL(StartIdx);

%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(StartIdx,1); %DCM

euler_mad_B(:,3) = un_modulo(euler_mad_B(:,3), 360); %remove 2pi jumps and make continuous
r_data = resample(timeseries(euler_mad_B(:,1), SFtime),ILtime);
p_data = resample(timeseries(euler_mad_B(:,2), SFtime),ILtime);
y_data = resample(timeseries(euler_mad_B(:,3), SFtime),ILtime);
euler_mad_B = [r_data.data, p_data.data, y_data.data];
euler_mad_B(:,3) = euler_mad_B(:,3) - euler_mad_B(StartIdx,3); %Madgwick

euler_mah_B(:,3) = un_modulo(euler_mah_B(:,3), 360); %remove 2pi jumps and make continuous
r_data = resample(timeseries(euler_mah_B(:,1), SFtime),ILtime);
p_data = resample(timeseries(euler_mah_B(:,2), SFtime),ILtime);
y_data = resample(timeseries(euler_mah_B(:,3), SFtime),ILtime);
euler_mah_B = [r_data.data, p_data.data, y_data.data];
euler_mah_B(:,3) = euler_mah_B(:,3) - euler_mah_B(StartIdx,3); %Mahony

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

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

warning('on','interpolation:interpolation:noextrap');

%% plotting
markerSize = 8; %pl

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];

% Define locations from where tests start and stop (acceleration test and rotation test)

%ACC-test sequence: Linear motion with different accelerations
accStartIdx = find(time > 468, 1, 'first');
accStopIdx = find(time > 559, 1, 'first');
accStartIdx_IL = find(ILtime > 468, 1, 'first');
accStopIdx_IL = find(ILtime > 559, 1, 'first');
accStartIdx_SF = find(SFtime > 468, 1, 'first');
accStopIdx_SF = find(SFtime > 559, 1, 'first');
accStartIdx_K = find(Ktime > 468, 1, 'first');
accStopIdx_K = find(Ktime > 559, 1, 'first');

%sideways movement at x
accStartIdx_SF_x = find(SFtime > 468.5, 1, 'first');
accStopIdx_SF_x = find(SFtime > 496.5, 1, 'first');
%sideways movement at y
accStartIdx_SF_y = find(SFtime > 498, 1, 'first');
accStopIdx_SF_y = find(SFtime > 526, 1, 'first');
%up and down movement at z
accStartIdx_SF_z = find(SFtime > 530, 1, 'first');
accStopIdx_SF_z = find(SFtime > 558, 1, 'first');

%IMU-test sequence: free 6D motion
rotStartIdx = find(time > 564, 1, 'first');
rotStopIdx = find(time > 770, 1, 'first');

%raw accelerations
figIdx = 1;
figure(figIdx); clf;

arrowOffset = 2;
arrowOffset_y = 0.15;

subplot(4,1,1);
minY = -1;
maxY = 1;
plot(Ktime, vel_x, 'b', Ktime, vel_y, 'g--', Ktime, vel_z, 'r:', 'LineWidth', 1);
set(gca, 'Position', plot41Pos, 'FontSize', 12, 'FontName', 'Times');
title('Reference measurement and accelerations during the acceleration test', 'FontSize', 14, 'FontName', 'Times');
legend('x', 'y', 'z', 'Location', 'NorthEast');
ylabel('Reference velocity (m/s)', 'FontSize', 12, 'FontName', 'Times');
axis([Ktime(accStartIdx_K) Ktime(accStopIdx_K) minY maxY]);
hold on;
plot(SFtime([accStartIdx_SF_x accStartIdx_SF_x]), [minY maxY], 'k:', SFtime([accStopIdx_SF_x accStopIdx_SF_x]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_y accStartIdx_SF_y]), [minY maxY], 'k:', SFtime([accStopIdx_SF_y accStopIdx_SF_y]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_z accStartIdx_SF_z]), [minY maxY], 'k:', SFtime([accStopIdx_SF_z accStopIdx_SF_z]), [minY maxY], 'k:', 'LineWidth', 1);

subplot(4,1,2);
plot(ILtime(accStartIdx_IL), acc2_x(accStartIdx_IL), 'b--', ...
    SFtime(accStartIdx_SF:accStopIdx_SF), acc1_x(accStartIdx_SF:accStopIdx_SF), 'c', ...
    ILtime(accStartIdx_IL:accStopIdx_IL), acc2_x(accStartIdx_IL:accStopIdx_IL), 'b--');
set(gca, 'Position', plot42Pos, 'FontSize', 12, 'FontName', 'Times');
%title('Accelerations during the acceleration test', 'FontSize', 14, 'FontName', 'Times');
legend('A (Inertia-Link)', 'B (SparkFun)', 'Location', legendLocation(acc2_x(accStartIdx_IL:accStopIdx_IL)));
ylabel('acc_x (m/s^2)', 'FontSize', 12, 'FontName', 'Times');
hold on;

maxY = max([max(acc1_x(accStartIdx_SF:accStopIdx_SF)), max(acc2_x(accStartIdx_IL:accStopIdx_IL))]);
%maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(acc1_x(accStartIdx_SF:accStopIdx_SF)), min(acc2_x(accStartIdx_IL:accStopIdx_IL))]);
%minY = min([1.1*minY 0.9*minY]);
plot(SFtime([accStartIdx_SF_x accStartIdx_SF_x]), [minY maxY], 'k:', SFtime([accStopIdx_SF_x accStopIdx_SF_x]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_y accStartIdx_SF_y]), [minY maxY], 'k:', SFtime([accStopIdx_SF_y accStopIdx_SF_y]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_z accStartIdx_SF_z]), [minY maxY], 'k:', SFtime([accStopIdx_SF_z accStopIdx_SF_z]), [minY maxY], 'k:', 'LineWidth', 1);

axis([SFtime(accStartIdx_SF) SFtime(accStopIdx_SF) minY maxY]);

subplot(4,1,3);
plot(SFtime(accStartIdx_SF:accStopIdx_SF), acc1_y(accStartIdx_SF:accStopIdx_SF), 'c', ...
    ILtime(accStartIdx_IL:accStopIdx_IL), acc2_y(accStartIdx_IL:accStopIdx_IL), 'b--');
set(gca, 'Position', plot43Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('acc_y (m/s^2)', 'FontSize', 12, 'FontName', 'Times');
hold on;

maxY = max([max(acc1_y(accStartIdx_SF:accStopIdx_SF)), max(acc2_y(accStartIdx_IL:accStopIdx_IL))]);
%maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(acc1_y(accStartIdx_SF:accStopIdx_SF)), min(acc2_y(accStartIdx_IL:accStopIdx_IL))]);
%minY = min([1.1*minY 0.9*minY]);
plot(SFtime([accStartIdx_SF_x accStartIdx_SF_x]), [minY maxY], 'k:', SFtime([accStopIdx_SF_x accStopIdx_SF_x]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_y accStartIdx_SF_y]), [minY maxY], 'k:', SFtime([accStopIdx_SF_y accStopIdx_SF_y]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_z accStartIdx_SF_z]), [minY maxY], 'k:', SFtime([accStopIdx_SF_z accStopIdx_SF_z]), [minY maxY], 'k:', 'LineWidth', 1);
axis([SFtime(accStartIdx_SF) SFtime(accStopIdx_SF) minY maxY]);

subplot(4,1,4);
plot(SFtime(accStartIdx_SF:accStopIdx_SF), acc1_z(accStartIdx_SF:accStopIdx_SF), 'c', ...
    ILtime(accStartIdx_IL:accStopIdx_IL), acc2_z(accStartIdx_IL:accStopIdx_IL), 'b--');
set(gca, 'Position', plot44Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('acc_z (m/s^2)', 'FontSize', 12, 'FontName', 'Times');
xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
hold on;

maxY = max([max(acc1_z(accStartIdx_SF:accStopIdx_SF)), max(acc2_z(accStartIdx_IL:accStopIdx_IL))]);
%maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(acc1_z(accStartIdx_SF:accStopIdx_SF)), min(acc2_z(accStartIdx_IL:accStopIdx_IL))]);
%minY = min([1.1*minY 0.9*minY]);
plot(SFtime([accStartIdx_SF_x accStartIdx_SF_x]), [minY maxY], 'k:', SFtime([accStopIdx_SF_x accStopIdx_SF_x]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_y accStartIdx_SF_y]), [minY maxY], 'k:', SFtime([accStopIdx_SF_y accStopIdx_SF_y]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_z accStartIdx_SF_z]), [minY maxY], 'k:', SFtime([accStopIdx_SF_z accStopIdx_SF_z]), [minY maxY], 'k:', 'LineWidth', 1);

plot([SFtime(accStartIdx_SF_x)+arrowOffset SFtime(accStopIdx_SF_x)-arrowOffset], [minY minY]+arrowOffset_y, 'k:', ...
    SFtime(accStartIdx_SF_x)+arrowOffset, minY+arrowOffset_y, 'k<', SFtime(accStopIdx_SF_x)-arrowOffset, minY+arrowOffset_y, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize);
text(SFtime(accStartIdx_SF_x)+2*arrowOffset, minY-arrowOffset_y, 'x-axis movement', 'FontSize', 12, 'FontName', 'Times');

plot([SFtime(accStartIdx_SF_y)+arrowOffset SFtime(accStopIdx_SF_y)-arrowOffset], [minY minY]+arrowOffset_y, 'k:', ...
    SFtime(accStartIdx_SF_y)+arrowOffset, minY+arrowOffset_y, 'k<', SFtime(accStopIdx_SF_y)-arrowOffset, minY+arrowOffset_y, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize);
text(SFtime(accStartIdx_SF_y)+2*arrowOffset, minY-arrowOffset_y, 'y-axis movement', 'FontSize', 12, 'FontName', 'Times');

plot([SFtime(accStartIdx_SF_z)+arrowOffset SFtime(accStopIdx_SF_z)-arrowOffset], [minY minY]+arrowOffset_y, 'k:', ...
    SFtime(accStartIdx_SF_z)+arrowOffset, minY+arrowOffset_y, 'k<', SFtime(accStopIdx_SF_z)-arrowOffset, minY+arrowOffset_y, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize);
text(SFtime(accStartIdx_SF_z)+2*arrowOffset, minY-arrowOffset_y, 'z-axis movement', 'FontSize', 12, 'FontName', 'Times');

axis([SFtime(accStartIdx_SF) SFtime(accStopIdx_SF) minY-4*arrowOffset_y maxY]);

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

% DCM states
figure(figIdx); clf;
subplot(3,1,1);
plot(ILtime, x_hist2(:,1), 'b--',  SFtime, x_hist1(:,1), 'c', ILtime, -data.InertiaLink.M13, 'm-.', Ktime, R31, 'k--');
legend('DCM_A', 'DCM_B', 'Inertia-Link', 'Kuka', 'Location', legendLocation([x_hist2(:,1) -data.InertiaLink.M13]));
set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');
xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
ylabel('x1 (R31)', 'FontSize', 12, 'FontName', 'Times');
title('DCM states', 'FontSize', 14, 'FontName', 'Times');
axis([0 commonDuration -1.1 1.1]);

subplot(3,1,2);
plot(ILtime, x_hist2(:,2), 'b--',  SFtime, x_hist1(:,2), 'c', ILtime, -data.InertiaLink.M23, 'm-.', Ktime, R32, 'k--');
set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('x2 (R32)', 'FontSize', 12, 'FontName', 'Times');
axis([0 commonDuration -1.1 1.1]);

subplot(3,1,3);
plot(ILtime, x_hist2(:,3), 'b--', SFtime, x_hist1(:,3), 'c', ILtime, -data.InertiaLink.M33, 'm-.', Ktime, R33, 'k--');
set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');
xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
ylabel('x3 (R33)', 'FontSize', 12, 'FontName', 'Times');
axis([0 commonDuration -1.1 1.1]);

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/test_fig_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps'])
figIdx = figIdx + 1;

% yaw
disp(' ');
disp('Yaw errors:')
dt = 1/150;
yaw_DCM_error = (ypr_hist2(StartIdx:StopIdx,1)-yaw_in_IL)*180/pi;
yaw_DCM_delta_error = [0; yaw_DCM_error(2:end) - yaw_DCM_error(1:end-1)] ./dt;
yaw_DCM_error_acc = yaw_DCM_error(accStartIdx:accStopIdx)-yaw_DCM_error(accStartIdx);
yaw_DCM_error_rot = yaw_DCM_error(rotStartIdx:rotStopIdx)-yaw_DCM_error(rotStartIdx);
yaw_DCM_RMSE_acc = sqrt(mean(yaw_DCM_error_acc.^2))
yaw_DCM_RMSE_rot = sqrt(mean(yaw_DCM_error_rot.^2))

yaw_sf_error = yaw_sf_in_IL - yaw_in_IL*180/pi;
yaw_sf_delta_error = [0; yaw_sf_error(2:end) - yaw_sf_error(1:end-1)] ./dt;
yaw_sf_error_acc = yaw_sf_error(accStartIdx:accStopIdx)-yaw_sf_error(accStartIdx);
yaw_sf_error_rot = yaw_sf_error(rotStartIdx:rotStopIdx)-yaw_sf_error(rotStartIdx);
yaw_sf_RMSE_acc = sqrt(mean(yaw_sf_error_acc.^2))
yaw_sf_RMSE_rot = sqrt(mean(yaw_sf_error_rot.^2))

yaw_mad_B_error = euler_mad_B(StartIdx:StopIdx,3) - yaw_in_IL*180/pi;
yaw_mad_B_delta_error = [0; yaw_mad_B_error(2:end) - yaw_mad_B_error(1:end-1)] ./dt;
yaw_mad_B_error_acc = yaw_mad_B_error(accStartIdx:accStopIdx)-yaw_mad_B_error(accStartIdx);
yaw_mad_B_error_rot = yaw_mad_B_error(rotStartIdx:rotStopIdx)-yaw_mad_B_error(rotStartIdx);
yaw_mad_B_RMSE_acc = sqrt(mean(yaw_mad_B_error_acc.^2))
yaw_mad_B_RMSE_rot = sqrt(mean(yaw_mad_B_error_rot.^2))

yaw_mah_B_error = euler_mah_B(StartIdx:StopIdx,3) - yaw_in_IL*180/pi;
yaw_mah_B_delta_error = [0; yaw_mah_B_error(2:end) - yaw_mah_B_error(1:end-1)] ./dt;
yaw_mah_B_error_acc = yaw_mah_B_error(accStartIdx:accStopIdx)-yaw_mah_B_error(accStartIdx);
yaw_mah_B_error_rot = yaw_mah_B_error(rotStartIdx:rotStopIdx)-yaw_mah_B_error(rotStartIdx);
yaw_mah_B_RMSE_acc = sqrt(mean(yaw_mah_B_error_acc.^2))
yaw_mah_B_RMSE_rot = sqrt(mean(yaw_mah_B_error_rot.^2))

yaw_mad_A_error = euler_mad_A(StartIdx:StopIdx,3) - yaw_in_IL*180/pi;
yaw_mad_A_delta_error = [0; yaw_mad_A_error(2:end) - yaw_mad_A_error(1:end-1)] ./dt;
yaw_mad_A_error_acc = yaw_mad_A_error(accStartIdx:accStopIdx)-yaw_mad_A_error(accStartIdx);
yaw_mad_A_error_rot = yaw_mad_A_error(rotStartIdx:rotStopIdx)-yaw_mad_A_error(rotStartIdx);
yaw_mad_A_RMSE_acc = sqrt(mean(yaw_mad_A_error_acc.^2))
yaw_mad_A_RMSE_rot = sqrt(mean(yaw_mad_A_error_rot.^2))

yaw_mah_A_error = euler_mah_A(StartIdx:StopIdx,3) - yaw_in_IL*180/pi;
yaw_mah_A_delta_error = [0; yaw_mah_A_error(2:end) - yaw_mah_A_error(1:end-1)] ./dt;
yaw_mah_A_error_acc = yaw_mah_A_error(accStartIdx:accStopIdx)-yaw_mah_A_error(accStartIdx);
yaw_mah_A_error_rot = yaw_mah_A_error(rotStartIdx:rotStopIdx)-yaw_mah_A_error(rotStartIdx);
yaw_mah_A_RMSE_acc = sqrt(mean(yaw_mah_A_error_acc.^2))
yaw_mah_A_RMSE_rot = sqrt(mean(yaw_mah_A_error_rot.^2))

yaw_IL_error = (yaw_IL(StartIdx:StopIdx)-yaw_in_IL)*180/pi;
yaw_IL_delta_error = [0; yaw_IL_error(2:end) - yaw_IL_error(1:end-1)] ./dt;
yaw_IL_error_acc = yaw_IL_error(accStartIdx:accStopIdx)-yaw_IL_error(accStartIdx);
yaw_IL_error_rot = yaw_IL_error(rotStartIdx:rotStopIdx)-yaw_IL_error(rotStartIdx);
yaw_IL_RMSE_acc = sqrt(mean(yaw_IL_error_acc.^2))
yaw_IL_RMSE_rot = sqrt(mean(yaw_IL_error_rot.^2))

%pitch
disp(' ');
disp('Pitch errors:')
pitch_DCM_error = (ypr_hist2(StartIdx:StopIdx,2)-pitch_in_IL)*180/pi;
pitch_DCM_RMSE_acc = sqrt(mean(pitch_DCM_error(accStartIdx:accStopIdx).^2))
pitch_DCM_RMSE_rot = sqrt(mean(pitch_DCM_error(rotStartIdx:rotStopIdx).^2))

pitch_sf_error = pitch_sf_in_IL - pitch_in_IL*180/pi;
pitch_sf_RMSE_acc = sqrt(mean(pitch_sf_error(accStartIdx:accStopIdx).^2))
pitch_sf_RMSE_rot = sqrt(mean(pitch_sf_error(rotStartIdx:rotStopIdx).^2))

pitch_mad_B_error = euler_mad_B(StartIdx:StopIdx,2) - pitch_in_IL*180/pi;
pitch_mad_B_RMSE_acc = sqrt(mean(pitch_mad_B_error(accStartIdx:accStopIdx).^2))
pitch_mad_B_RMSE_rot = sqrt(mean(pitch_mad_B_error(rotStartIdx:rotStopIdx).^2))

pitch_mah_B_error = euler_mah_B(StartIdx:StopIdx,2) - pitch_in_IL*180/pi;
pitch_mah_B_RMSE_acc = sqrt(mean(pitch_mah_B_error(accStartIdx:accStopIdx).^2))
pitch_mah_B_RMSE_rot = sqrt(mean(pitch_mah_B_error(rotStartIdx:rotStopIdx).^2))

pitch_mad_A_error = euler_mad_A(StartIdx:StopIdx,2) - pitch_in_IL*180/pi;
pitch_mad_A_RMSE_acc = sqrt(mean(pitch_mad_A_error(accStartIdx:accStopIdx).^2))
pitch_mad_A_RMSE_rot = sqrt(mean(pitch_mad_A_error(rotStartIdx:rotStopIdx).^2))

pitch_mah_A_error = euler_mah_A(StartIdx:StopIdx,2) - pitch_in_IL*180/pi;
pitch_mah_A_RMSE_acc = sqrt(mean(pitch_mah_A_error(accStartIdx:accStopIdx).^2))
pitch_mah_A_RMSE_rot = sqrt(mean(pitch_mah_A_error(rotStartIdx:rotStopIdx).^2))

pitch_IL_error = (pitch_IL(StartIdx:StopIdx)-pitch_in_IL)*180/pi;
pitch_IL_RMSE_acc = sqrt(mean(pitch_IL_error(accStartIdx:accStopIdx).^2))
pitch_IL_RMSE_rot = sqrt(mean(pitch_IL_error(rotStartIdx:rotStopIdx).^2))

%roll
disp(' ');
disp('Roll errors:')
roll_DCM_error = (ypr_hist2(StartIdx:StopIdx,3)-roll_in_IL)*180/pi;
roll_DCM_RMSE_acc = sqrt(mean(roll_DCM_error(accStartIdx:accStopIdx).^2))
roll_DCM_RMSE_rot = sqrt(mean(roll_DCM_error(rotStartIdx:rotStopIdx).^2))

roll_sf_error = roll_sf_in_IL - roll_in_IL*180/pi;
roll_sf_RMSE_acc = sqrt(mean(roll_sf_error(accStartIdx:accStopIdx).^2))
roll_sf_RMSE_rot = sqrt(mean(roll_sf_error(rotStartIdx:rotStopIdx).^2))

roll_mad_B_error = euler_mad_B(StartIdx:StopIdx,1) - roll_in_IL*180/pi;
roll_mad_B_RMSE_acc = sqrt(mean(roll_mad_B_error(accStartIdx:accStopIdx).^2))
roll_mad_B_RMSE_rot = sqrt(mean(roll_mad_B_error(rotStartIdx:rotStopIdx).^2))

roll_mah_B_error = euler_mah_B(StartIdx:StopIdx,1) - roll_in_IL*180/pi;
roll_mah_B_RMSE_acc = sqrt(mean(roll_mah_B_error(accStartIdx:accStopIdx).^2))
roll_mah_B_RMSE_rot = sqrt(mean(roll_mah_B_error(rotStartIdx:rotStopIdx).^2))

roll_mad_A_error = euler_mad_A(StartIdx:StopIdx,1) - roll_in_IL*180/pi;
roll_mad_A_RMSE_acc = sqrt(mean(roll_mad_A_error(accStartIdx:accStopIdx).^2))
roll_mad_A_RMSE_rot = sqrt(mean(roll_mad_A_error(rotStartIdx:rotStopIdx).^2))

roll_mah_A_error = euler_mah_A(StartIdx:StopIdx,1) - roll_in_IL*180/pi;
roll_mah_A_RMSE_acc = sqrt(mean(roll_mah_A_error(accStartIdx:accStopIdx).^2))
roll_mah_A_RMSE_rot = sqrt(mean(roll_mah_A_error(rotStartIdx:rotStopIdx).^2))

roll_IL_error = (roll_IL(StartIdx:StopIdx)-roll_in_IL)*180/pi;
roll_IL_RMSE_acc = sqrt(mean(roll_IL_error(accStartIdx:accStopIdx).^2))
roll_IL_RMSE_rot = sqrt(mean(roll_IL_error(rotStartIdx:rotStopIdx).^2))

disp(' ');
disp('Error matrices:')
RMSE_acc = [yaw_DCM_RMSE_acc yaw_sf_RMSE_acc yaw_mad_A_RMSE_acc yaw_mad_B_RMSE_acc yaw_mah_A_RMSE_acc yaw_mah_B_RMSE_acc yaw_IL_RMSE_acc; ...
    pitch_DCM_RMSE_acc pitch_sf_RMSE_acc pitch_mad_A_RMSE_acc pitch_mad_B_RMSE_acc pitch_mah_A_RMSE_acc pitch_mah_B_RMSE_acc pitch_IL_RMSE_acc; ...
    roll_DCM_RMSE_acc roll_sf_RMSE_acc roll_mad_A_RMSE_acc roll_mad_B_RMSE_acc roll_mah_A_RMSE_acc roll_mah_B_RMSE_acc roll_IL_RMSE_acc];

disp('RMSEs of the acceleration test');
textCell = [{'DCM_A', 'DCM_B', 'Madgwick_A', 'Madgwick_B', 'Mahony_A', 'Mahony_B', 'Inertia-Link'}; cell(size(RMSE_acc))];
for i = 1:size(RMSE_acc,1)
    for j = 1:size(RMSE_acc,2)
        textCell{i+1,j} = num2str(RMSE_acc(i,j), '%.2f');
    end
end
disp(textCell');

RMSE_rot = [yaw_DCM_RMSE_rot yaw_sf_RMSE_rot yaw_mad_A_RMSE_rot yaw_mad_B_RMSE_rot yaw_mah_A_RMSE_rot yaw_mah_B_RMSE_rot yaw_IL_RMSE_rot; ...
    pitch_DCM_RMSE_rot pitch_sf_RMSE_rot pitch_mad_A_RMSE_rot pitch_mad_B_RMSE_rot pitch_mah_A_RMSE_rot pitch_mah_B_RMSE_rot pitch_IL_RMSE_rot; ...
    roll_DCM_RMSE_rot roll_sf_RMSE_rot roll_mad_A_RMSE_rot roll_mad_B_RMSE_rot roll_mah_A_RMSE_rot roll_mah_B_RMSE_rot roll_IL_RMSE_rot];

disp('RMSEs of the rotation test');
textCell = [{'DCM_A', 'DCM_B', 'Madgwick_A', 'Madgwick_B', 'Mahony_A', 'Mahony_B', 'Inertia-Link'}; cell(size(RMSE_rot))];
for i = 1:size(RMSE_rot,1)
    for j = 1:size(RMSE_rot,2)
        textCell{i+1,j} = num2str(RMSE_rot(i,j), '%.2f');
    end
end
disp(textCell');

% box and whiskers plot for the acceleration test
groups_yaw = {'DCM_A', 'Madgwick_A', 'Mahony_A', 'DCM_B','Madgwick_B', 'Mahony_B', 'Inertia-Link_ '};

groups_rollpitch = cell(1, 2*size(groups_yaw,2));
groups_rollpitch(1:2:end) = groups_yaw;
groups_rollpitch(2:2:end) = groups_yaw;

figure(figIdx); clf;
subplot(2,1,1);
boxplot([yaw_DCM_error_acc, yaw_mad_A_error_acc, yaw_mah_A_error_acc, yaw_sf_error_acc, yaw_mad_B_error_acc, yaw_mah_B_error_acc, yaw_IL_error_acc], ...
    groups_yaw, 'whisker', 15);
txt = findobj(gca,'Type','text');
set(txt, 'FontSize', 12, 'FontName', 'Times', 'Interpreter', 'tex', 'VerticalAlignment', 'middle');
set(gca, 'Position', plot21Pos, 'FontSize', 12, 'FontName', 'Times');
title('Error statistics in the acceleration test', 'FontSize', 14, 'FontName', 'Times');
ylabel('yaw errors (deg)');

subplot(2,1,2);

boxplot([pitch_DCM_error(accStartIdx:accStopIdx), roll_DCM_error(accStartIdx:accStopIdx), ...
    pitch_mad_A_error(accStartIdx:accStopIdx), roll_mad_A_error(accStartIdx:accStopIdx), ...
    pitch_mah_A_error(accStartIdx:accStopIdx), roll_mah_A_error(accStartIdx:accStopIdx), ...
    pitch_sf_error(accStartIdx:accStopIdx), roll_sf_error(accStartIdx:accStopIdx), ...
    pitch_mad_B_error(accStartIdx:accStopIdx), roll_mad_B_error(accStartIdx:accStopIdx), ...
    pitch_mah_B_error(accStartIdx:accStopIdx), roll_mah_B_error(accStartIdx:accStopIdx), ...
    pitch_IL_error(accStartIdx:accStopIdx), roll_IL_error(accStartIdx:accStopIdx)], ...
    groups_rollpitch, 'whisker', 15);

txt = findobj(gca,'Type','text');
set(txt, 'FontSize', 12, 'FontName', 'Times', 'Interpreter', 'tex', 'VerticalAlignment', 'middle');

set(gca, 'Position', plot22Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('combined roll and pitch errors (deg)');

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/test_fig_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps'])
figIdx = figIdx + 1;

% box and whiskers plot for the rotation test
figure(figIdx); clf;
subplot(2,1,1);
boxplot([yaw_DCM_error_rot, yaw_mad_A_error_rot, yaw_mah_A_error_rot, yaw_sf_error_rot, yaw_mad_B_error_rot, yaw_mah_B_error_rot, yaw_IL_error_rot], ...
    groups_yaw, 'whisker', 15);
txt = findobj(gca,'Type','text');
set(txt, 'FontSize', 12, 'FontName', 'Times', 'Interpreter', 'tex', 'VerticalAlignment', 'middle');

set(gca, 'Position', plot21Pos, 'FontSize', 12, 'FontName', 'Times');
title('Error statistics in the rotation test', 'FontSize', 14, 'FontName', 'Times');
ylabel('yaw errors (deg)');

subplot(2,1,2);
boxplot([pitch_DCM_error(rotStartIdx:rotStopIdx), roll_DCM_error(rotStartIdx:rotStopIdx), ...
    pitch_mad_A_error(rotStartIdx:rotStopIdx), roll_mad_A_error(rotStartIdx:rotStopIdx), ...
    pitch_mah_A_error(rotStartIdx:rotStopIdx), roll_mah_A_error(rotStartIdx:rotStopIdx), ...
    pitch_sf_error(rotStartIdx:rotStopIdx), roll_sf_error(rotStartIdx:rotStopIdx), ...
    pitch_mad_B_error(rotStartIdx:rotStopIdx), roll_mad_B_error(rotStartIdx:rotStopIdx), ...
    pitch_mah_B_error(rotStartIdx:rotStopIdx), roll_mah_B_error(rotStartIdx:rotStopIdx), ...
    pitch_IL_error(rotStartIdx:rotStopIdx), roll_IL_error(rotStartIdx:rotStopIdx)], ...
    groups_rollpitch, 'whisker', 15);
txt = findobj(gca,'Type','text');
set(txt, 'FontSize', 12, 'FontName', 'Times', 'Interpreter', 'tex', 'VerticalAlignment', 'middle');

set(gca, 'Position', plot22Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('combined roll and pitch errors (deg)');

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/test_fig_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps'])
figIdx = figIdx + 1;

% correlation tests for delays
yaw_ref = yaw_in_IL*180/pi;
pitch_ref = pitch_in_IL*180/pi;
roll_ref = roll_in_IL*180/pi;

yaw_dcm = ypr_hist2(StartIdx:StopIdx,1)*180/pi;
pitch_dcm = ypr_hist2(StartIdx:StopIdx,2)*180/pi;
roll_dcm = ypr_hist2(StartIdx:StopIdx,3)*180/pi;

madgwick = euler_mad_A(StartIdx:StopIdx,:);
mahony = euler_mah_A(StartIdx:StopIdx,:);

yaw_il = yaw_IL(StartIdx:StopIdx)*180/pi;
pitch_il = pitch_IL(StartIdx:StopIdx)*180/pi;
roll_il = roll_IL(StartIdx:StopIdx)*180/pi;

n_lags = 30;
[c_dcm_yaw,lags] = rootMeanSquaredErrors(yaw_dcm, yaw_ref, n_lags);
[min_c_dcm_yaw, min_c_dcm_yaw_idx] = min(c_dcm_yaw);
c_madgwick_yaw = rootMeanSquaredErrors(madgwick(:,3), yaw_ref, n_lags);
[min_c_madgwick_yaw, min_c_madgwick_yaw_idx] = min(c_madgwick_yaw);
c_mahony_yaw = rootMeanSquaredErrors(mahony(:,3), yaw_ref, n_lags);
[min_c_mahony_yaw, min_c_mahony_yaw_idx] = min(c_mahony_yaw);
c_il_yaw = rootMeanSquaredErrors(yaw_il, yaw_ref, n_lags);
[min_c_il_yaw, min_c_il_yaw_idx] = min(c_il_yaw);

c_dcm_pitch = rootMeanSquaredErrors(pitch_dcm, pitch_ref, n_lags);
[min_c_dcm_pitch, min_c_dcm_pitch_idx] = min(c_dcm_pitch);
c_madgwick_pitch = rootMeanSquaredErrors(madgwick(:,2), pitch_ref, n_lags);
[min_c_madgwick_pitch, min_c_madgwick_pitch_idx] = min(c_madgwick_pitch);
c_mahony_pitch = rootMeanSquaredErrors(mahony(:,2), pitch_ref, n_lags);
[min_c_mahony_pitch, min_c_mahony_pitch_idx] = min(c_mahony_pitch);
c_il_pitch = rootMeanSquaredErrors(pitch_il, pitch_ref, n_lags);
[min_c_il_pitch, min_c_il_pitch_idx] = min(c_il_pitch);

c_dcm_roll = rootMeanSquaredErrors(roll_dcm, roll_ref, n_lags);
[min_c_dcm_roll, min_c_dcm_roll_idx] = min(c_dcm_roll);
c_madgwick_roll = rootMeanSquaredErrors(madgwick(:,1), roll_ref, n_lags);
[min_c_madgwick_roll, min_c_madgwick_roll_idx] = min(c_madgwick_roll);
c_mahony_roll = rootMeanSquaredErrors(mahony(:,1), roll_ref, n_lags);
[min_c_mahony_roll, min_c_mahony_roll_idx] = min(c_mahony_roll);
c_il_roll = rootMeanSquaredErrors(roll_il, roll_ref, n_lags);
[min_c_il_roll, min_c_il_roll_idx] = min(c_il_roll);

lags = lags*dt;

% mean squared errors plot
figure(figIdx); clf;
subplot(3,1,1);
plot(lags, c_dcm_yaw, 'b', lags, c_madgwick_yaw, 'g--', lags, c_mahony_yaw, 'r:', lags, c_il_yaw, 'm-.', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');
title('RMSEs to the reference measurement as a function of time delay', 'FontSize', 14, 'FontName', 'Times');
ylabel('yaw RMSE (deg)');
legend('DCM', 'Madgwick', 'Mahony', 'Inertia-Link', 'Location', legendLocation([c_dcm_yaw' c_madgwick_yaw' c_mahony_yaw' c_il_yaw']));
hold on;
plot(lags(min_c_dcm_yaw_idx), c_dcm_yaw(min_c_dcm_yaw_idx), 'b*', ...
    lags(min_c_madgwick_yaw_idx), c_madgwick_yaw(min_c_madgwick_yaw_idx), 'g^', ...
    lags(min_c_mahony_yaw_idx), c_mahony_yaw(min_c_mahony_yaw_idx), 'ro', ...
    lags(min_c_il_yaw_idx), c_il_yaw(min_c_il_yaw_idx), 'mp', 'LineWidth', 1, 'MarkerSize', markerSize);

subplot(3,1,2);
plot(lags, c_dcm_pitch, 'b', lags, c_madgwick_pitch, 'g--', lags, c_mahony_pitch, 'r:', lags, c_il_pitch, 'm-.', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('pitch RMSE (deg)');
hold on;
plot(lags(min_c_dcm_pitch_idx), c_dcm_pitch(min_c_dcm_pitch_idx), 'b*', ...
    lags(min_c_madgwick_pitch_idx), c_madgwick_pitch(min_c_madgwick_pitch_idx), 'g^', ...
    lags(min_c_mahony_pitch_idx), c_mahony_pitch(min_c_mahony_pitch_idx), 'ro', ...
    lags(min_c_il_pitch_idx), c_il_pitch(min_c_il_pitch_idx), 'mp', 'LineWidth', 1, 'MarkerSize', markerSize);

subplot(3,1,3);
plot(lags, c_dcm_roll, 'b', lags, c_madgwick_roll, 'g--', lags, c_mahony_roll, 'r:', lags, c_il_roll, 'm-.', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('roll RMSE (deg)');
xlabel('time delay to reference (s)');
hold on;
plot(lags(min_c_dcm_roll_idx), c_dcm_roll(min_c_dcm_roll_idx), 'b*', ...
    lags(min_c_madgwick_roll_idx), c_madgwick_roll(min_c_madgwick_roll_idx), 'g^', ...
    lags(min_c_mahony_roll_idx), c_mahony_roll(min_c_mahony_roll_idx), 'ro', ...
    lags(min_c_il_roll_idx), c_il_roll(min_c_il_roll_idx), 'mp', 'LineWidth', 1, 'MarkerSize', markerSize);

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/test_fig_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps'])
figIdx = figIdx + 1;

% Yaw, pitch and roll plot
figure(figIdx); clf;
subplot(3,1,1);
plot(time, yaw_dcm, 'b', time, madgwick(:,3), 'g--', time, mahony(:,3), 'r:', time, yaw_il, 'm-.', time, yaw_ref, 'k--', 'LineWidth', 1);
set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');
legend('DCM', 'Madgwick', 'Mahony', 'Inertia-Link', 'Reference', 'Location', legendLocation([yaw_dcm yaw_ref madgwick(:,3) mahony(:,3)]));
hold on;
maxY = max([max(yaw_dcm), max(madgwick(:,3)), max(mahony(:,3)), max(yaw_il), max(yaw_ref)]);
maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(yaw_dcm), min(madgwick(:,3)), min(mahony(:,3)), min(yaw_il), min(yaw_ref)]);
minY = min([1.1*minY 0.9*minY]);
plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
ylabel('yaw (deg)', 'FontSize', 12, 'FontName', 'Times');
title('Euler angles', 'FontSize', 14, 'FontName', 'Times');
axis([time(1) time(end) minY maxY]);

subplot(3,1,2);
plot(time, pitch_dcm, 'b', time, madgwick(:,2), 'g--', time, mahony(:,2), 'r:', time, pitch_il, 'm-.', time, pitch_ref, 'k--', 'LineWidth', 1);
hold on;
maxY = max([max(pitch_dcm), max(madgwick(:,2)), max(mahony(:,2)), max(pitch_il), max(pitch_ref)]);
maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(pitch_dcm), min(madgwick(:,2)), min(mahony(:,2)), min(pitch_il), min(pitch_ref)]);
minY = min([1.1*minY 0.9*minY]);
plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('pitch (deg)', 'FontSize', 12, 'FontName', 'Times');
axis([time(1) time(end) minY maxY]);

subplot(3,1,3);
plot(time, roll_dcm, 'b', time, madgwick(:,1), 'g--', time, mahony(:,1), 'r:', time, roll_il, 'm-.', time, roll_ref, 'k--', 'LineWidth', 1);
set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');
hold on;
maxY = max([max(roll_dcm), max(madgwick(:,1)), max(mahony(:,1)), max(roll_il), max(roll_ref)]);
maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(roll_dcm), min(madgwick(:,1)), min(mahony(:,1)), min(roll_il), min(roll_ref)]);
minY = min([1.1*minY 0.9*minY]);
arrowOffset = 5;
plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot([time(accStartIdx)+arrowOffset time(accStopIdx)-arrowOffset], [minY minY]+arrowOffset, 'k:', ...
    time(accStartIdx)+arrowOffset, minY+arrowOffset, 'k<', time(accStopIdx)-arrowOffset, minY+arrowOffset, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize);
text(time(accStartIdx)+2*arrowOffset, minY-arrowOffset, 'Acceleration test', 'FontSize', 12, 'FontName', 'Times');
plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot([time(rotStartIdx)+arrowOffset time(rotStopIdx)-arrowOffset], [minY minY]+arrowOffset, 'k:', ...
    time(rotStartIdx)+arrowOffset, minY+arrowOffset, 'k<', time(rotStopIdx)-arrowOffset, minY+arrowOffset, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize);
text(time(rotStartIdx)+2*arrowOffset, minY-arrowOffset, 'Rotation test', 'FontSize', 12, 'FontName', 'Times');
xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
ylabel('roll (deg)', 'FontSize', 12, 'FontName', 'Times');

axis([time(1) time(end) minY-20 maxY]);

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/test_fig_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps'])
figIdx = figIdx + 1;

% error plot with reference
figure(figIdx); clf;
subplot(4,1,1);
plot(time, yaw_ref, 'b', time, pitch_ref, 'g--', time, roll_ref, 'r:', 'LineWidth', 1);
set(gca, 'Position', plot41Pos, 'FontSize', 12, 'FontName', 'Times');
legend('yaw', 'pitch', 'roll', 'Location', legendLocation([yaw_ref pitch_ref roll_ref]));
title('The reference measurement and measurement errors', 'FontSize', 14, 'FontName', 'Times');
ylabel('reference angles (deg)', 'FontSize', 12, 'FontName', 'Times');

hold on;
maxY = max([max(yaw_ref), max(pitch_ref), max(roll_ref)]);
maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(yaw_ref), min(pitch_ref), min(roll_ref)]);
minY = min([1.1*minY 0.9*minY]);
arrowOffset = 5;
plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);

axis([time(1) time(end) minY maxY]);

subplot(4,1,2);
plot(time, yaw_DCM_error, 'b', time, yaw_mad_A_error, 'g--', time, yaw_mah_A_error, 'r:', time, yaw_IL_error, 'm-.', 'LineWidth', 1, 'EraseMode', 'xor');
set(gca, 'Position', plot42Pos, 'FontSize', 12, 'FontName', 'Times');
legend('DCM', 'Madgwick', 'Mahony', 'Inertia-Link', 'Location', legendLocation([yaw_DCM_error yaw_IL_error yaw_mad_A_error yaw_mah_A_error]));
hold on;
ylabel('yaw errors (deg)', 'FontSize', 12, 'FontName', 'Times');
set(gca, 'XLim', [time(1) time(end)]);

maxY = max([max(yaw_DCM_error), max(yaw_mad_A_error), max(yaw_mah_A_error), max(yaw_IL_error)]);
maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(yaw_DCM_error), min(yaw_mad_A_error), min(yaw_mah_A_error), min(yaw_IL_error)]);
minY = min([1.1*minY 0.9*minY]);

plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);

axis([time(1) time(end) minY maxY]);

subplot(4,1,3);
plot(time, pitch_DCM_error, 'b', time, pitch_mad_A_error, 'g--', time, pitch_mah_A_error, 'r:', time, pitch_IL_error, 'm-.', 'LineWidth', 1, 'EraseMode', 'xor');
hold on;
set(gca, 'Position', plot43Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('pitch errors (deg)', 'FontSize', 12, 'FontName', 'Times');

maxY = max([max(pitch_DCM_error), max(pitch_mad_A_error), max(pitch_mah_A_error), max(pitch_IL_error)]);
maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(pitch_DCM_error), min(pitch_mad_A_error), min(pitch_mah_A_error), min(pitch_IL_error)]);
minY = min([1.1*minY 0.9*minY]);

plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);

axis([time(1) time(end) minY maxY]);

subplot(4,1,4);
plot(time, roll_DCM_error, 'b', time, roll_mad_A_error, 'g--', time, roll_mah_A_error, 'r:', time, roll_IL_error, 'm-.', 'LineWidth', 1, 'EraseMode', 'xor');
hold on;
set(gca, 'Position', plot44Pos, 'FontSize', 12, 'FontName', 'Times');
xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
ylabel('roll errors (deg)', 'FontSize', 12, 'FontName', 'Times');

maxY = max([max(roll_DCM_error), max(roll_mad_A_error), max(roll_mah_A_error), max(roll_IL_error)]);
maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(roll_DCM_error), min(roll_mad_A_error), min(roll_mah_A_error), min(roll_IL_error)]);
minY = min([1.1*minY 0.9*minY]);

arrowOffset_y = 0.5;

plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot([time(accStartIdx)+arrowOffset time(accStopIdx)-arrowOffset], [minY minY]+arrowOffset_y, 'k:', ...
    time(accStartIdx)+arrowOffset, minY+arrowOffset_y, 'k<', time(accStopIdx)-arrowOffset, minY+arrowOffset_y, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize);
text(time(accStartIdx)+2*arrowOffset, minY-arrowOffset_y, 'Acceleration test', 'FontSize', 12, 'FontName', 'Times');
plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot([time(rotStartIdx)+arrowOffset time(rotStopIdx)-arrowOffset], [minY minY]+arrowOffset_y, 'k:', ...
    time(rotStartIdx)+arrowOffset, minY+arrowOffset_y, 'k<', time(rotStopIdx)-arrowOffset, minY+arrowOffset_y, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize);
text(time(rotStartIdx)+2*arrowOffset, minY-arrowOffset_y, 'Rotation test', 'FontSize', 12, 'FontName', 'Times');

axis([time(1) time(end) minY-4*arrowOffset_y maxY]);

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

% bias plot (states with variances)
figure(figIdx); clf;

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

plotLims = 1.25*[mean(x_sigma), mean(y_sigma), mean(z_sigma)];

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

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/test_fig_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps'])

author: hhyyti

repo: dcm-imu

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


text:
function [ns] = min2ns(min)
% Convert time from minutes to nanoseconds.
% Chad Greene 2012
ns = min*60000000000;

author: Sable

repo: mcbench-benchmarks


text:
function ell_demo ( )

%*****************************************************************************80
%
%% ELL_DEMO demonstrates MESH2D on the L-shaped region.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 April 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ELL_DEMO:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Demonstrate MESH2D on the L-shaped region.\n' );

  clf

  warning off
%
%  #1) Simple input, 6 vertices.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EXAMPLE 1:\n' );
  fprintf ( 1, '  Minimal Input\n' );
  fprintf ( 1, '  Use 6 vertices on the boundary.\n' );

  v = [ 0.0, 0.0; 2.0, 0.0; 2.0, 1.0; 1.0, 1.0; 1.0, 2.0; 0.0, 2.0 ];

  [ p, t ] = mesh2d ( v );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );
  pause
%
%  #2) = Example #1, with some small segments.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EXAMPLE 2:\n' );
  fprintf ( 1, '  Set a few small boundary segments.\n' );
  fprintf ( 1, '  Use 8 vertices on the boundary.\n' );

  v = [ 0.0, 0.0; 2.0, 0.0; 2.0, 0.25; 2.0, 0.5; 2.0, 1.0; 1.0, 1.0; 1.0, 2.0; 0.0, 2.0 ];

  [ p, t ] = mesh2d ( v );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, ...
    '  %d boundary vertices input, %d nodes and %d triangles created\n', ...
    nv, np, nt );
  pause
%
%  #3) = Example #1, but now I specify a maximum element size of 0.2.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EXAMPLE 3:\n' );
  fprintf ( 1, '  Set maximum element size HDATA.HMAX = 0.1\n' );

  v = [ 0.0, 0.0; 2.0, 0.0; 2.0, 1.0; 1.0, 1.0; 1.0, 2.0; 0.0, 2.0 ];

  hdata = [];
  hdata.hmax = 0.1;

  [ p, t ] = mesh2d ( v, [], hdata );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );
  pause
%
%  #4) = Example #1, but now specify a density function.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EXAMPLE 4:\n' );
  fprintf ( 1, '  Specify small elements near reentrant corner using a size function.\n' );

  v = [ 0.0, 0.0; 2.0, 0.0; 2.0, 1.0; 1.0, 1.0; 1.0, 2.0; 0.0, 2.0 ];

  hdata = [];
  hdata.fun = @hfun1;

  [ p, t ] = mesh2d ( v, [], hdata );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );
  pause
%
%  #5) Same as #1, but refine.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EXAMPLE 5:\n' );
  fprintf ( 1, '  Repeat example #1, then call refine ( ).\n' );

  v = [ 0.0, 0.0; 2.0, 0.0; 2.0, 1.0; 1.0, 1.0; 1.0, 2.0; 0.0, 2.0 ];

  [ p, t ] = mesh2d ( v );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );
  pause

  [ p, t ] = refine ( p, t );
%
%  Since REFINE doesn't redraw the mesh, I had to guess how to do it here.
%
   clf
   hold on
   plot ( v(:,1 ), v(:,2), 'r.', 'MarkerSize', 32 );
   axis equal
   pause
   plot(p(:,1),p(:,2),'b.', 'Markersize', 16 )
   plot ( v(:,1), v(:,2), 'r.', 'MarkerSize', 32 );
   pause ( )
   patch('faces',t(:,:),'vertices',p,'facecolor','w','edgecolor','b');
%  patch('faces',edge,'vertices',v,'facecolor','none','edgecolor','k')
   plot ( p(:,1), p(:,2), 'b.', 'Markersize', 16 )
   plot ( v(:,1), v(:,2), 'r.', 'MarkerSize', 32 );
   axis equal off;
   hold off

   print ( '-dpng', 'ell_mesh5.png' );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );

  pause
%
%  #6) Same as #2, but smooth.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EXAMPLE 6:\n' );
  fprintf ( 1, '  Repeat example #2, then call smoothmesh ( ).\n' );

  v = [ 0.0, 0.0; 2.0, 0.0; 2.0, 0.25; 2.0, 0.5; 2.0, 1.0; 1.0, 1.0; 1.0, 2.0; 0.0, 2.0 ];

  [ p, t ] = mesh2d ( v );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );
  pause

  [ p, t ] = smoothmesh ( p, t );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );

  redisplay ( v, p, t );

  pause
%
%  Close the figure.
%
  close ( gcf )
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ELL_DEMO:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end
function h = hfun1 ( x, y )

%*****************************************************************************80
%
%% HFUN1 is a size-function for the L-shaped region.
%
%  Discussion:
%
%    The smallest size is at (1.0,1.0), and sizes increase as their distance
%    from that point increases.
%
  h = 0.01 + 0.1 * sqrt ( ( x - 1.0 ).^2  + ( y - 1.0 ).^2 );

  return
end

author: johannesgerer

repo: jburkardt-m


text:
classdef UGraph < BetterHandle
    %
    % UGraph : Undirected graph for an image skeleton
    %
    properties
        G  % [n x 2] node coordinates
        E  % [n x n boolean] adjacency matrix
        EI % [n x n cell], each cell is that edge's index into trajectory list
           %  S. It should be a cell array, because there could be two paths
        S  % [k x 1 cell] edge paths in the image
        I  % [nimg x nimg boolean] original image
    end

    properties (Dependent = true)
        n % number of nodes in a graph
        imsize % image size
        link_ei_to_ni % [k x 2] for each edge (row), the source and dest. node,
                      % given the direction it is listed in S
    end

    properties (SetAccess = private)
       list_mask % circular masks for merging critical points
    end

    methods

        % constructor for making an undirected graph
        function this = UGraph(G,E,EI,S,I)
           this.G = G;
           this.E = E;
           this.EI = EI;
           this.S = S;
           this.I = I;

           load('circle_masks','list_mask');
           this.list_mask = list_mask;
        end

        function y = get.n(this)
           y = size(this.G,1);
        end

        function sz = get.imsize(this)
            sz = size(this.I);
        end

        % get what nodes the edges are connceted to
        function Y = get.link_ei_to_ni(this)
           k = numel(this.S);
           Y = zeros(k,2);
           for i=1:k
               source_pt = this.S{i}(1,:);
               dest_pt = this.S{i}(end,:);
               source_ni = this.map_pts_to_ni(source_pt);
               dest_ni = this.map_pts_to_ni(dest_pt);
               Y(i,1) = source_ni;
               Y(i,2) = dest_ni;
           end
        end

        % Plot the skeleton on-top of the image
        function plot_skel(this)
           this.plot_on_image(~this.I);
        end

        % Plot the maximum circle analysis, with the
        % original image and the skeleton
        function plot_circle(this)
            I_cluster = this.max_circle_mask;
            I_mask = double(this.I);
            I_mask(I_cluster) = 0.5;
            this.plot_on_image(1-I_mask);
        end

        % Merge groups of graph nodes connected
        % by the maximum circle criterion
        function clean_skeleton(this)
           I_mask = this.max_circle_mask();

           % cluster nodes
           C = this.cluster_G(I_mask);
           nc = numel(unique(C));

           % until we no longer have things to remove
           while has_rmv(C)

              % for each cluster index, look for
              % things to merge
              for i=1:nc
                  sel = C == i;
                  if sum(sel)>1
                     this.merge(sel,I_mask);

                     % cluster nodes
                     C = this.cluster_G(I_mask);
                     [~,nc] = has_rmv(C);
                     break
                  end
              end

           end

           % do we still have groups to remove?
           %
           % max_c: maximum cluster index
           function [yyyy,max_c] = has_rmv(myC)
               myC(myC==0) = [];
               yyyy = numel(myC) ~= numel(unique(myC));
               max_c = max(myC);
           end

        end

        % Image based on maximum circles surrounding
        % each critical point
        function I_mask = max_circle_mask(this)
            I_mask = false(this.imsize);
            for indx=1:this.n
                I_mask = this.max_circle(indx,I_mask);
            end
        end

        % merge a collection of nodes
        %
        % Input
        %   sel: [n x 1 logical] or [k x 1 index]
        %
        function merge(this,sel,I_mask)

            % process input
            if islogical(sel)
                assert(numel(sel)==this.n);
            else
                choose = false(this.n,1);
                choose(sel) = true;
                sel = choose;
            end

            % partition
            E_sel_to_not = this.E(sel,~sel);
            EI_sel_to_sel = this.EI(sel,sel);
            EI_sel_to_not = this.EI(sel,~sel);

            % create the new row in adjacency matrix, which
            % includes all the nodes connected to the selected set/pair
            new_row_E = any(E_sel_to_not,1);
            new_row_EI = cell(size(new_row_E));
            for i=1:size(EI_sel_to_not,2);
                new_row_EI{i} = tovec(EI_sel_to_not(:,i));
            end
            list_ei_sel_to_sel = tovec(EI_sel_to_sel);
            list_ei_sel_to_sel = unique(list_ei_sel_to_sel);

            % see which of the edges, from sel to sel, should be removed.
            % we don't want to remove edges that make a long arc and then return
            nss = length(list_ei_sel_to_sel);
            rmv_edge = false(nss,1);
            for i=1:nss
               indx = list_ei_sel_to_sel(i);

               % remove edge if all the pixels are in the circle criterion mask
               pix = this.S{indx};
               sz = size(I_mask);
               ind_px = sub2ind(sz,pix(:,1),pix(:,2));
               if all(I_mask(ind_px));
                    rmv_edge(i) = true;
               end

            end
            keep_ei_sel_to_sel =  list_ei_sel_to_sel(~rmv_edge);
            rmv_ei_sel_to_sel = list_ei_sel_to_sel(rmv_edge);

            % remove node positions
            G_sel = this.G(sel,:);
            new_node = mean(G_sel,1);
            Grmv = this.G(sel,:);
            this.G(sel,:) = [];
            this.G = [this.G; new_node];

            % modify adjacency matrix with the new row
            this.E(sel,:) = [];
            this.E(:,sel) = [];
            this.E(end+1,:) = new_row_E;
            this.E(:,end+1) = [new_row_E,false];
            if ~isempty(keep_ei_sel_to_sel)
                this.E(end,end) = true;
            end

            % modify the list of edges with each cell
            this.EI(sel,:) = [];
            this.EI(:,sel) = [];
            this.EI(end+1,:) = new_row_EI;
            this.EI(:,end+1) = [new_row_EI, {keep_ei_sel_to_sel} ];

            % modify the paths
            this.S(rmv_ei_sel_to_sel) = [];
            for i=1:numel(this.EI)
               mycell = this.EI{i};

               % update counts in adjacency matrix
               for j=1:length(mycell)
                  el = mycell(j);
                  this.EI{i}(j) = el - sum(rmv_ei_sel_to_sel < el);
               end
            end

            % update the paths, stored in this.S, such that all
            % nodes we have now replaced are updated with their
            % new coordinates
            for i=1:length(this.S)
                for j=1:size(Grmv,1);
                    node_rmv = Grmv(j,:);
                    d = pdist2(node_rmv,this.S{i});
                    swap = d < .001;
                    nswap = sum(swap);
                    this.S{i}(swap,:) = repmat(new_node,[nswap 1]);
                end
            end

            % Make sure all the trajectories are roughly uniform distance
            for i=1:length(this.S)
                this.S{i} = expand_unif_interp(this.S{i},1);
            end

            this.assert_valid_graph();
        end

        % Check to make sure this is a valid graph structure
        % Throw assert if it is not
        function assert_valid_graph(this)

            n = this.n;

            % check that we have the correct sizes
            % of adjacency matrices
            assert(size(this.G,1)==n);
            assert(size(this.E,1)==n);
            assert(size(this.EI,1)==n);

            % make sure all of the paths in S
            % have a corresponding edge
            v = tovec(this.EI);
            uq = unique(v);
            assert(isequal(uq(:),(1:length(uq))'));

            % check on the paths throught the graph
            for i=1:size(this.EI,1) % for each edge in the graph
                for j=1:size(this.EI,2)

                    v = this.EI{i,j}; % get the associated path
                    if isempty(v)
                       assert(this.E(i,j)==false);
                    else
                       assert(this.E(i,j)==true);
                    end

                    for k=1:length(v)
                       indx = v(k);

                       % check that the start and end points
                       % align with the coordinates stored in G
                       mystart = this.S{indx}(1,:);
                       myend = this.S{indx}(end,:);
                       assert(aeq(mystart,this.G(i,:)) || aeq(mystart,this.G(j,:)));
                       assert(aeq(myend,this.G(i,:)) || aeq(myend,this.G(j,:)));
                    end

                end
            end

            % check that the matrix E and EI are in correspondence
            for i=1:size(this.EI,1) % for each edge in the graph
                for j=1:size(this.EI,2)
                    if this.E(i,j)
                       assert(~isempty(this.EI(i,j)));
                    end
                end
            end

        end

        %
        % Map list of nodes (as points) to their indices
        %
        % Input
        %  vpts: [n x 2] points
        %
        % Output
        %  vni: [n x 1] indices
        function vni = map_pts_to_ni(this,vpts)
            [n,dim] = size(vpts);
            assert(dim==2);
            vni = zeros(n,1);
            for i=1:n
                pt = vpts(i,:);
                dist = pdist2(pt,this.G);
                assert(isvector(dist));
                dist = dist(:);
                [minval,minindx] = min(dist);
                assert(aeq(minval,0));
                vni(i) = minindx;
            end
        end

       % Given the current point, where can we go?
        %
        % Input: pt: [1x2] current point we want to analyze
        %
        % Output
        %  vei: [m x 1] list of edges we can go to
        %  vei_flip: [m x 1 boolean] do we need to flip that edge
        %    to align it to the current point
        function [vei,vei_flip] = get_branches(this,pt)
            assert(numel(pt)==2);
            ni = this.map_pts_to_ni(pt);
            list_ei = this.EI(ni,:);
            vei = concat_cell(list_ei);
            vei = vei(:);
            nn = numel(vei);
            list_traj = this.S(vei);
            vei_flip = false(size(vei));
            for i=1:nn
               traj = list_traj{i};
               if isequal(pt,traj(1,:))
                   vei_flip(i) = false;
               elseif isequal(pt,traj(end,:))
                   vei_flip(i) = true;
               else
                   error('cannot map point to its paths');
               end
            end
        end

    end

    methods (Access = private)

        % find the maximum circle in the image surrounding a point
        function I_cluster = max_circle(this,indx,I_cluster)
            pt = this.G(indx,:);
            pt = round(pt);
            nmask = numel(this.list_mask);
            bool_fit = false(nmask,1);
            for i=1:nmask
               x_mask = this.list_mask{i};
               x_mask(:,1) = x_mask(:,1) + pt(1);
               x_mask(:,2) = x_mask(:,2) + pt(2);
               x_mask = this.cut_image_plane(x_mask);
               xlind = sub2ind(this.imsize,x_mask(:,1),x_mask(:,2));
               bool_fit(i) = all(this.I(xlind));
               if bool_fit(i)
                   I_cluster(xlind) = true;
               else
                   return
               end
            end
        end

        % remove pixels that are out of the image plane
        function x = cut_image_plane(this,x)
            sz = this.imsize;
            xx = x(:,1);
            xy = x(:,2);
            rmvx = xx<=0 | xx>sz(1);
            rmvy = xy<=0 | xy>sz(2);
            rmv = rmvx | rmvy;
            x(rmv,:) = [];
        end

        %
        %  Plot the graph skeleton ontop of an image
        %
        function plot_on_image(this,I)
            sz = size(I);
            if size(I,3) == 1
                I = repmat(I,[1 1 3]);
            end
            hold on
            ns = length(this.S);
            image([1 sz(1)],[1 sz(2)],I);
            for i=1:ns
                stk = this.S{i};
                %color = rand(3,1);
                plot_traj(stk,'g');
            end
            for i=1:this.n
               plot(this.G(i,2),this.G(i,1),'r.','MarkerSize',7);
            end
            set(gca,'YDir','reverse','XTick',[],'YTick',[]);
            xlim([1 sz(1)]);
            ylim([1 sz(2)]);

        end

        % Cluster the vertices in G
        % into clusters based on the maximum circle criterion
        function C = cluster_G(this,I_mask)
            assert(UtilImage.check_black_is_true(I_mask));
            L = bwlabel(I_mask,4);
            GG = round(this.G);
            lind_G = sub2ind(this.imsize,GG(:,1),GG(:,2));
            C = L(lind_G);
        end

    end

end

% flatten a cell array into a regular array
% by vert cat each cell
function v = concat_cell(vcell)
    assert(iscell(vcell));
    v = [];
    for i=1:numel(vcell)
        v = [v; vcell{i}];
    end
end

% plot a stroke trajectory in image space,
% where the x and y dimensions are reversed
function plot_traj(stk,color)
    ystk = stk(:,2);
    stk(:,2) = stk(:,1);
    stk(:,1) = ystk;
    plot(stk(:,1),stk(:,2),'Color',color,'LineWidth',1);
end

author: brendenlake

repo: BPL

save_path: github-repos/MATLAB/brendenlake-BPL


text:
function faces = sphDelaunay(dirs)
%SPHDELAUNAY Computes the Delaunay triangulation on the unit sphere
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% SPHDELAUNAY.M - 10/10/2013
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Convert to cartesian
N_vert = size(dirs, 1);
[tempx, tempy, tempz] = sph2cart(dirs(:,1), dirs(:,2), ones(N_vert,1));
U_vert = [tempx, tempy, tempz];

% Find the convex hull of the points on the sphere - in this special case
% the result equals the Delaunay triangulation of the points
faces = convhulln(U_vert);

% Invert the triangles
faces = faces(:, 3:-1:1);

% Shift the results to begin each triangle from the smallest entry
for n = 1:size(faces,1)
    tempface = faces(n,:);
    [~, minIdx] = min(tempface);
    faces(n, :) = circshift(tempface, [0 1-minIdx]);
end

% Sort through triangles with smaller entries first
faces = sortrows(faces, 1); % sort through first entry
maxentry = max(faces(:,1)); % sort through second entry
n = 1;
while n <= maxentry
    startIdx = find(faces(:,1) == n, 1, 'first');
    if ~isempty(startIdx)
        endIdx = find(faces(:,1) == n, 1, 'last');
        faces(startIdx:endIdx, :) = sortrows(faces(startIdx:endIdx, :), 2);
        n = n + 1;
    else
        n = n + 1;
    end
end

author: polarch

repo: Spherical-Harmonic-Transform


text:
filename = 'CantileverSquare';%'CantileverSquareSYmmetricMesh';%'CantileverSquareNewFine';%'LshapeTriFine';%'CantileverSquareNew';'ArchTriFine';%'CantileverSquare';%'ArchTriFine';%'CantileverSquare';%'Lshape';%'LshapeTriFine';%'CantileverSquareSmall';%'';%'Lshape';'CantileverSquare';'Lshape';%'CantileverSquare';%'LshapeFine';%'Bridge_quad_coarse';%'Arch_quad_coarse';%'BridgeCool_Quadrilateral_Bilinear_Structured_Coarse';%'Bridge';%'CantileverSquareSmall';
ptype = 'MACRO';
initial_case = 'given';
m1 = 0.0101;
m2 = 0.0101;
%cost = {'compliance'};
cost = {'stressNorm'};
%cost = {'stressNorm','compliance'};
%weights = [0.55,0.45];
weights = 1;
constraint = {'volumeConstraint'};
filterType = 'P1';
constraint_case = 'EQUALITY';

Vfrac_initial = 0.3;
optimality_initial = 1e-5;
constr_initial = 1e-5;

Vfrac_final = 0.3;
optimality_final = 1e-5;
constr_final = 1e-5;

stressNormExponent_initial = 2;
stressNormExponent_final = 64;

optimizer = 'DualNestedInPrimal';
optimizerUnconstrained = 'PROJECTED GRADIENT';

designVariable = 'MicroParams';
ub = 0.989;
lb = 0.011;
homegenizedVariablesComputer = 'ByVademecum';
%
vademecumFileName = 'SuperEllipseQMax';
%vademecumFileName = 'SuperEllipseQ2';
%vademecumFileName = 'SuperEllipseQOptAnalytic';
%
% designVariable = 'Density';
% homegenizedVariablesComputer = 'ByInterpolation';
% method = 'SIMPALL';
% materialType = 'ISOTROPIC';

kfrac = 2;
nsteps = 20;

plotting = false;
printing = true;
monitoring = true;
monitoring_interval = 1;
maxiter = 500;

author: SwanLab

repo: Swan

save_path: github-repos/MATLAB/SwanLab-Swan


text:
function [C, L] = wave_cross_cov(X, Y, LMAX)
%%
%% Compute wavelet cross-correlations for arbitrary lag
%% -----------------------------------------------------------------------
%% Input: X     Matrix containing wavelet coefficients with appropriate
%%              boundary condition
%%        Y     Matrix containing wavelet coefficients with appropriate
%%              boundary condition
%%        LMAX  Maximum lag to compute
%%
%% Output: C  Matrix containing the wavelet cross-correlation (same
%%            number of columns as X and Y)
%%         L  Vector of lags (for plotting)
%%
[I J] = size(X);
L = (-LMAX+1):(LMAX-1);

XYCrossCov = []; C = [];
for j = 1:J
  XNaN = X(~isnan(X(:,j)),j)';
  ZNaN = XNaN;
  YNaN = Y(~isnan(Y(:,j)),j)';
  NX = length(XNaN);
  NXX = min(length(XNaN) - 1, LMAX);
  XNaNVar = mean(XNaN.^2);
  YNaNVar = mean(YNaN.^2);

  LAG1 = []; LAG2 = [];
  for i = 1:NXX
    XYNaN = XNaN .* YNaN;
    ZYNaN = ZNaN .* YNaN;
    LAG1 = [LAG1 sum(XYNaN(~isnan(XYNaN))) / sqrt(XNaNVar * YNaNVar) / NX];
    LAG2 = [LAG2 sum(ZYNaN(~isnan(ZYNaN))) / sqrt(XNaNVar * YNaNVar) / NX];
    XNaN = [XNaN(2:NX) NaN];
    ZNaN = [NaN ZNaN(1:(NX-1))];
  end
  XYCrossCov = [fliplr(LAG2) LAG1(2:LMAX)];
  C = [C XYCrossCov'];
end


text:
function [id,wasOpen] = mrmCheckMeshServer(id, host)
%
%     [id,wasOpen] = mrmCheckMeshServer(id, [host='localhohst']);
%
% Author: Ress
% Purpose:
%   Test whether a window with id is open on the host (defaults to
%   localhost). If not, start a new window is opened and its id is
%   returned. If id is set to -1, no possible window could be open so a new
%   window is opened and its value is returned in id.
%
%   The (boolean) variable wasOpen specifies that the window numbered id
%   was open (or not).
%
% Examples:
%    [id,wasOpen] = mrmCheckMeshServer(5);   % Checks for window 5
%
% Notes:
%    It would be nice to really just test, and not to test and open a new
%    one.  Also, I am not sure why the pauses are there.  Must be some
%    mrMesh bogosity. (BW)

warndlg('Obsolete.  mrmCheckMeshServer')

wasOpen = 0;
if ieNotDefined('id'), id = -1; end
if ieNotDefined('host') host = 'localhost'; end
oldId = id;

% Refreshing the window with this idea gives us the status.
[id,stat,res] = mrMesh(host, id, 'refresh');

if(id<-1)
    % If the server isn't up, id should be -1000.
    wasOpen = 0;
    switch computer
        case 'PCWIN'
            srvPath = which('mrMeshSrv.exe');
            dos([srvPath ' &']);
        case 'GLNX86'
            srvPath = which('mrMeshSrv.glx');
            eval(['! ' srvPath ' &']);
        otherwise
            error(['Platform "' computer '" is currently not supported!']);
        end
        % Some annoying inter-process communication pause.  Do not remove.
    pause(2); id = mrMesh(host, -1, 'refresh'); pause(1);
elseif(stat<0)
    % this means the server is up, but the specified ID doesn't exist (user
    % probably closed the window). So, we get a new window and return that
    % new id.
    id = mrMesh(host, -1, 'refresh');
    pause(1);
else
    if(oldId>=0)
        wasOpen = 1;
    end
end

return;

author: vistalab

repo: vistasoft

save_path: github-repos/MATLAB/vistalab-vistasoft


text:
%==========================================================================
% [Saliency_Map, Feature_Maps, ICA_Maps, img, SUN] = Run_SUN(img, SUN)
% returns the SUN Saliency Map of img as well as other useful quantities.
%
% Input:    img is a color (preferably) or black and white image.
%           SUN (optional) is a structure containing SUN's parameters.
%
% Output:   Saliency_Map is a saliency map created out of ICA responses
%               using the SUN framework.
%           Feature_Maps contains ICA filter responses nonlinearly
%               weighted by their statistical frequency.
%           ICA_Maps contains the ICA filter responses with the sign
%               discarded.
%           img is the image after being preprocessed.
%           SUN is a structure that contains SUN's parameters.
%
% Note:     If you are making subsequent calls to this function, pass
%           an empty array to SUN and then use the returned version. This
%           will substantially speed up the function.
%
% Example:  img = imread('wpeppers.jpg');figure;
%           subplot(1, 4, 1);imagesc(img);title('Original Image');axis off;
%           [Saliency_Map, Feature_Maps, ICA_Maps, img] = ...
%               Run_SUN(img, []);
%           subplot(1, 4, 2);imagesc(img);title('Preprocessed Image');
%           axis off;
%           subplot(1, 4, 3);imagesc(Saliency_Map);title('Saliency Map');
%           axis off;subplot(1, 4, 4);imagesc(mean(Feature_Maps, 3));axis off;
%           title('Mean Feature Map');
%
%
% Author:   Christopher Kanan (chriskanan@gmail.com)
%
% Cite as:  Kanan, C. & Cottrell, G. W. (2010). Robust classification of
%             objects, faces, and flowers using natural image Statistics.
%             In Proceedings of the IEEE Conference on Computer Vision
%             and Pattern Recognition (CVPR), 2010.
%==========================================================================
function [Saliency_Map, Feature_Maps, ICA_Maps, img, SUN] = Run_SUN(img, SUN, max_filters, img_size, skip_preprocessing)

%set up default parameters
if ~exist('img_size', 'var')
    img_size = 128; %default canonical image size
end

pad_image = false;
use_probit = false; %keep false in general for more discriminability
approx_gammainc = true; %makes the algorithm much faster

if nargin == 0
    img = imread('wpeppers.jpg');
    orig = img;
    img_size = 192;
end

if ~exist('img', 'var')
    error('No image was passed to SUN_RICA.');
end

if ~exist('SUN', 'var') || isempty(SUN)
    SUN = load('SUN.mat');
    if isfield(SUN, 'SUN')
        SUN = SUN.SUN;
    end
end

if ~exist('max_filters', 'var')
    max_filters = inf;
end
Num_Filters = min(SUN.Num_Filters, max_filters);

if approx_gammainc && isempty(SUN.Filters(1).Inc_Gamma_List_X)
    %this takes a few seconds, but only needs to be repeated once as long
    %as you use the returned SUN structure on subsequent function calls
    for f = 1:Num_Filters
        x_max = SUN.Filters(f).x_max;
        x_inc = SUN.Filters(f).x_inc;
        SUN.Filters(f).Inc_Gamma_List_X = single(0:x_inc:x_max)';
        SUN.Filters(f).Inc_Gamma_List_Y = single(gammainc(SUN.Filters(f).Inc_Gamma_List_X, 1 ./ SUN.Theta(f), 'lower'));
    end
end

%preprocess the image to make it into the "canonical" size
if ~exist('skip_preprocessing', 'var') || ~skip_preprocessing
    img = Preprocess_Image(img, img_size);
end

Filters = SUN.Filters;

Filt_Size = size(Filters(1).R, 1);
half_filt = Filt_Size / 2;
if pad_image
    img = padarray(img, [half_filt, half_filt], 'both', 'replicate');
end

[SY, SX, SZ] = size(img);
prec = 'single';

%subtract chanel means
R = img(:, :, 1) - SUN.Channel_Mean(1);
G = img(:, :, 2) - SUN.Channel_Mean(2);
B = img(:, :, 3) - SUN.Channel_Mean(3);

%prepare image for FFT
FFT_R = fftn(R);
FFT_G = fftn(G);
FFT_B = fftn(B);

%prepare filters for FFT
for f = 1:Num_Filters
    if isempty(Filters(f).FFT_R) || size(Filters(f).FFT_R, 1) ~= SY || size(Filters(f).FFT_R, 2) ~= SX
        Filt_Size = size(Filters(f).R, 1);
        Filters(f).FFT_R = zeros(size(img, 1), size(img, 2), prec);
        Filters(f).FFT_G = zeros(size(img, 1), size(img, 2), prec);
        Filters(f).FFT_B = zeros(size(img, 1), size(img, 2), prec);

        Filters(f).FFT_R(1:Filt_Size, 1:Filt_Size) = fliplr(flipud(Filters(f).R));
        Filters(f).FFT_G(1:Filt_Size, 1:Filt_Size) = fliplr(flipud(Filters(f).G));
        Filters(f).FFT_B(1:Filt_Size, 1:Filt_Size) = fliplr(flipud(Filters(f).B));

        Filters(f).FFT_R = fftn(Filters(f).FFT_R);
        Filters(f).FFT_G = fftn(Filters(f).FFT_G);
        Filters(f).FFT_B = fftn(Filters(f).FFT_B);
    end
end

Filt_Size = size(Filters(1).R, 1);
shape_flag = 'valid';

if strcmpi(shape_flag, 'valid')
    deduct = Filt_Size;
else
    deduct = 0;
end

%pre-allocate memory for speed
ICA_Maps = zeros(SY - deduct, SX - deduct, Num_Filters, prec);
Feature_Maps = zeros(SY - deduct, SX  - deduct, Num_Filters, prec);
SUN_Maps = zeros(SY  - deduct, SX  - deduct, Num_Filters, prec);

[SY, SX, SZ] = size(Feature_Maps);

%get the filter responses
inv_theta = 1 ./ SUN.Theta(f);
for f = 1:Num_Filters
    theta = SUN.Theta(f);
    sigma = SUN.Sigma(f);

    %do FFT filtering
    filt_resp = (ifftn(Filters(f).FFT_R .* FFT_R + Filters(f).FFT_G .* FFT_G + Filters(f).FFT_B .* FFT_B, 'symmetric'));
    filt_resp = filt_resp(Filt_Size+1:end, Filt_Size+1:end);
    filt_resp = abs(single(filt_resp));
    if deduct == 0
        filt_resp = imresize(filt_resp, [SY, SX], 'nearest');
    end

    filt_resp = filt_resp / max(filt_resp(:));
    ICA_Maps(:, :, f) = filt_resp;
    filt_resp = (filt_resp ./  sigma) .^ theta;
    SUN_Maps(:, :, f) = filt_resp;

    if ~approx_gammainc
        R_Map = gammainc(filt_resp, inv_theta(f), 'lower');
    else
        Yi = qinterp1c(Filters(f).Inc_Gamma_List_X, Filters(f).Inc_Gamma_List_Y, filt_resp);
        R_Map = reshape(Yi, [SY, SX]);
    end

    %R_Map = R_Map ./ max(abs(R_Map(:)));
    Feature_Maps(:, :, f) = R_Map;
end

if use_probit
    Feature_Maps = norminv(Feature_Maps);
end

Feature_Maps(~isfinite(Feature_Maps)) = 0;

Saliency_Map = sum(SUN_Maps, 3);

Saliency_Map = double(Saliency_Map);

%this actually alters the saliency map, but I do it here for numerical purposes
Saliency_Map = Saliency_Map - min(Saliency_Map(:));
Saliency_Map = Saliency_Map ./ max(Saliency_Map(:));

%some filtering to smooth the saliency map
sz = 9;
h = fspecial('gaussian', sz, sz / 2);
Saliency_Map = imfilter(Saliency_Map, h, 'conv');
Saliency_Map = exp(Saliency_Map - log(sum(exp(Saliency_Map(:)))));

SUN.Filters = Filters;

if nargin == 0
    figure;
    subplot(1, 2, 1)
    imagesc(orig);
    title('Input Image');
    axis off
    subplot(1, 2, 2);
    imagesc(Saliency_Map);
    axis off;
    title('Saliency Map');
    img = [];
    Saliency_Map = [];
    Feature_Maps = [];
    ICA_Maps = [];
    SUN = [];
end

return;

%==========================================================================
% This function converts to LMS space and applies the log nonlinearity to
% to the image.
%
% Author:   Christopher Kanan (chriskanan@gmail.com)
%==========================================================================
function img = Preprocess_Image(img, img_size)

do_smoothing = true;

img = single(img);
if max(img(:) > 1)
    img = img ./ 255;
end

if do_smoothing
    %this helps with our noisy JPEG images, and is similar to some
    %computations done in the retina
    sz_per = 0.03;
    sz = max(floor(sz_per*min(size(img, 1), size(img, 2))), 3);
    sz = sz + (mod(sz, 2) == 0);h = fspecial('gaussian', sz, 1);
    img = imfilter(img, h, 'same');
end

if ndims(img) == 2 %handle grayscale images by making them color
    img = repmat(img, [1, 1, 3]);
end

img = SRGB2LMS(img); %convert from sRGB to LMS color space

[sy, sx, sz] = size(img);
rescale_factor = img_size / min(sy, sx);
img = imresize(img, round([sy, sx]*rescale_factor));

img = min(max(img, 0), 1); %bicubic interpolation can make some values greater than 1 or less than 0

%normalize image
img = img - min(img(:));
img = img ./ max(img(:));
%img = min(max(img, 0), 1); %should do nothing

%now log nonlinearity
C = 0.005; %arbitrary choice
%img_log = (log((img + C) ./ C)) ./ (log((1 + C) ./ C)); %output is in [0,1]
img_log = (log(img + C) - log(C)) ./ (log(1 + C) - log(C));
img_log = max(img_log, 0); %in case of numerical issues

img = img_log;

img(isnan(img)) = 0; %This should never be needed

%==================================================================
% This function converts from the default standard RGB color space
% to LMS color space, which approximates the response of
% human cone photoreceptors
%
% Input: A color image in SRGB color space, the default using
% imread(.), either as an NxMx3 matrix or as a (N*M)x3 matrix where
% each column represent a color channel
%
% Output: The image in LMS color space
%
% Example:  This will do nothing, since the functions are inverses,
%           peppers = imread('wpeppers.jpg');
%           figure;
%           subplot(1, 3, 1);imshow(peppers);
%           title('Original (SRGB)');
%           subplot(1, 3, 2);imshow(SRGB2LMS(peppers));
%           title('LMS Space');
%           subplot(1, 3, 3);imshow(LMS2SRGB(SRGB2LMS(peppers)));
%           title('Invert back to SRGB space')
%
% Author: Christopher Kanan (chriskanan@gmail.com)
%==================================================================
function LMS_Feat = SRGB2LMS(RGB_Feat)

XYZ_RGB = [0.4124, .3576, .1805; 0.2126, 0.7152, 0.0722; 0.0193, 0.1192, 0.9505];
XYZ_RGB = XYZ_RGB ./ repmat(sum(XYZ_RGB, 2), [1, 3]);
LMS_XYZ = [0.7328, 0.4296, -0.1624; -0.7036, 1.6975, 0.0061; 0.0030, 0.0136, 0.9834];
LMS_RGB = LMS_XYZ * XYZ_RGB; %CIECAM02

%see if we need to reshape the values
if size(RGB_Feat, 3) == 3 || size(RGB_Feat, 2) ~= 3
    is_image = true;
    [sy, sx, sz] = size(RGB_Feat);
    R = RGB_Feat(:, :, 1);
    G = RGB_Feat(:, :, 2);
    B = RGB_Feat(:, :, 3);
    RGB_Feat = [R(:), G(:), B(:)];
else
    is_image = false;
end
RGB_Feat = single(RGB_Feat);

% normalize:
if any(RGB_Feat(:) > 1)
    RGB_Feat = RGB_Feat ./ 255;
end

% remove sRGB gamma nonlinearity:
mask = RGB_Feat <= 0.04045;
RGB_Feat(mask) = RGB_Feat(mask) ./ 12.92;
RGB_Feat(~mask) = ((RGB_Feat(~mask) + 0.055) ./ 1.055) .^ 2.4;

%now do the conversion
LMS_Feat = (LMS_RGB * RGB_Feat')';

LMS_Feat = max(LMS_Feat, 0);
LMS_Feat = min(LMS_Feat, 1);

%turn back into an image if necessary
if is_image
    L = reshape(LMS_Feat(:, 1), [sy, sx]);
    M = reshape(LMS_Feat(:, 2), [sy, sx]);
    S = reshape(LMS_Feat(:, 3), [sy, sx]);
    LMS_Feat = zeros(sy, sx, 3, 'single');
    LMS_Feat(:, :, 1) = L;
    LMS_Feat(:, :, 2) = M;
    LMS_Feat(:, :, 3) = S;
end

%This is used for nearest neighbor interpolation
function Yi = qinterp1c(x,Y,xi)

% Forces vectors to be columns
x = x(:); xi = xi(:);
sx = size(x); sY = size(Y);
if sx(1)~=sY(1)
    if sx(1)==sY(2)
        Y = Y';
    else
        error('x and Y must have the same number of rows');
    end
end

% Gets the x spacing
warning('off');
ndx = 1/(x(2)-x(1)); % one over to perform divide only once
warning('on');
if x(1) ~= 0
    xi = xi - x(1);      % subtract minimum of x
end

rxi = round(xi*ndx)+1;        % indices of nearest-neighbors
len = length(x);
rxi(rxi < 1) = 1;
rxi(rxi > len) = len;
Yi = Y(rxi, :);


text:
function [in] = um2in(um)
% Convert length from micrometers (or microns) to inches.
% Chad A. Greene 2012
in = um/25400;


text:
function startNodeIdx = flatFindStartNodeIndex(mesh)
%
%  startNodeIdx = flatFindStartNodeIndex(mesh)
%
%Author: Wandell
%Purpose:
%  Use the data in the flat.mat type file for the mesh and determine the
%  index into the start node used for flattening the Layer 0 (white matter
%  boundary) mesh.
%
%  Called from flatAdjustSpacing

% We need to make sure that the inputs to assignToNearest each have 3 rows
if ~isequal(size(mesh.uniqueVertices, 1), 3),
    mesh.uniqueVertices = mesh.uniqueVertices';
end
if ~isequal(size(mesh.startCoords, 1), 3),
    mesh.startCoords = mesh.startCoords';
end

% This is the line used in mrFlatMesh.  There it is used before we have
% clipped down the matrices.
[startNodeIdx,snDist]=assignToNearest(mesh.uniqueVertices,mesh.startCoords);

% Not sure this is the right function.
% l = find(ismember(round(mesh.uniqueVertices),mesh.startCoords,'rows'));
% startNodeIdx = l(1);

return;


text:
function r8vec_permute_uniform_test ( )

%*****************************************************************************80
%
%% R8VEC_PERMUTE_UNIFORM_TEST tests R8VEC_PERMUTE_UNIFORM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2015
%
%  Author:
%
%    John Burkardt
%
  n = 12;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8VEC_PERMUTE_UNIFORM_TEST\n' );
  fprintf ( 1, '  R8VEC_PERMUTE_UNIFORM randomly reorders an R8VEC.\n' );

  a = 100 + ( 1 : n );

  r8vec_print ( n, a, '  A, before rearrangement:' );

  [ a, seed ] = r8vec_permute_uniform ( n, a, seed );

  r8vec_print ( n, a, '  A, after rearrangement:' );

  return
end


text:
filename='ImprovedCantilever_hexahedra';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'SLERP'; incrementFactor = 1;
filterType = 'P1';

nsteps = 1;
Vfrac_final = 0.1;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-2;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;