Initial commit for PhD code, to be refactored.

+database/prepareC3Ddataset.m

+% Load the C3D data files.
+% Author: Frank Wouda 20th of June 2017
+
+function sessionData = prepareC3Ddataset(filename,folder)
+
+
+    
+% Store all the data from the current C3D file.
+[XYZpos, FsVideo, Analog, FsAnalog, ~, ~, ~, ~, ~] = ...
+    database.readC3D_All_data_GTV121_v2_42([folder filename]);
+% the positional and PlugInGait datafields from the C3D file.
+    C3DdataFields = fieldnames(XYZpos);
+
+    % Store the sampling frequencies.
+    sessionData.MarkerFs = FsVideo;
+    sessionData.AnalogFs = FsAnalog;
+    IndexFiles(1,iFile) = 1;
+
+
+% Loop over the datafields.
+for i = 1:length(C3DdataFields)
+
+    % Find the angular fields and extract that information.
+    if contains(string(C3DdataFields(i)),'Angle')
+
+        % Find the Angle string, such that we have correct field names.
+        AngleIndex = strfind(string(C3DdataFields(i)),'Angle');
+
+        % Extract the name of the current field.
+        CurrChar = char(C3DdataFields(i));
+
+        if iFile == 1
+            % Store the angular data in the appropriate fields of the
+            % structure.
+            sessionData.Angle.(CurrChar(1:AngleIndex-1)) = XYZpos.(CurrChar);
+        else
+            sessionData.Angle.(CurrChar(1:AngleIndex-1)) = cat(1,...
+                sessionData.Angle.(CurrChar(1:AngleIndex-1)),...
+                XYZpos.(CurrChar));
+        end
+
+    % Do not store any of the other field since these are not processed by PlugInGait currently.    
+    elseif contains(string(C3DdataFields(i)),'Power')
+    elseif contains(string(C3DdataFields(i)),'Force')
+    elseif contains(string(C3DdataFields(i)),'Moment')
+    elseif contains(string(C3DdataFields(i)),'Centre')
+
+    % The bony landmark positions are stored as follows.
+    else
+        % Extract the name of the current field.
+        CurrChar = char(C3DdataFields(i));
+
+        if iFile == 1
+            % Those positions are saved in the appropriate fields.
+            sessionData.MarkerPosition.(CurrChar) = XYZpos.(CurrChar);
+        else
+            sessionData.MarkerPosition.(CurrChar) = cat(1,...
+                sessionData.MarkerPosition.(CurrChar),...
+                XYZpos.(CurrChar));
+        end
+    end
+end
+
+L = length(sessionData.MarkerPosition.(char(C3DdataFields(1))));
+
+IndexFiles(2,iFile) = L;
+
+% Store the indexes.
+sessionData.indexes = IndexFiles;
+

+database/readC3D.m

+% Example of how to use the readC3D reader " readC3D_All_data_GTV121_v2_xx"  as has been 
+% written by Wiebe de Vries and Leendert Schaake (Roessingh Research and Development 2010 - 2017)
+% This software is delivered as is, without warranty
+% Define filename and call the function with that specification - make sure
+% to read all output data in this specific order
+% XYZ = markerdata
+% Fs_Video = frame rate camera
+% Analog contains the analog data for each channel
+% Fs_Analog = sample rate analog channels
+% Event contains all defined events (IC, FO etc)
+% Actial start_end contains the startframe and endframe definition
+% etc. (PArametergroup, camerainfo and Residual Error are possibly only needed in special applications)
+Vicon_fname = 'E:\ViXs\ViXs\Top Level 2\Subject 1\Session 1\Trial07.c3d';
+[XYZpos, FsVideo, Analog, FsAnalog, Event, ActualStartEnd, ParameterGroup, CameraInfo, ResidualError] = readC3D_All_data_GTV121_v2_42(Vicon_fname);
+
+% Draw the output of any of the markers
+figure(1);
+p=plot((XYZpos.RANK))  
+xlabel('Frames')
+ylabel('Position ' )
+title('Position Cylinder 1')
+hold on

+math/FindSync.m

+%% This function determines the synchronisation between optical and inertial.
+
+function [Istart, Ostart] = FindSync(Opt,Int,osync,isync,maxTime)
+
+OptStartOffset = 20;
+
+Istart = find(sqrt(sum(Int.^2,2)) > isync, 1);
+Ostart = find(diff(diff(sqrt(sum(Opt((OptStartOffset+1):end,:).^2,2)))) ...
+    > osync, 1) + OptStartOffset;
+
+
+if abs(Ostart - Istart) > maxTime
+    
+    Ostart = find(diff(diff(sqrt(sum(Opt((OptStartOffset+1):end,:).^2,2)))) ...
+        > osync/2, 1) + OptStartOffset;
+    
+end
+
+if abs(Ostart - Istart) > maxTime
+    
+    Ostart = find(diff(diff(sqrt(sum(Opt((OptStartOffset+1):end,:).^2,2)))) ...
+        > osync/4, 1) + OptStartOffset;
+    
+end
+
+if abs(Ostart - Istart) > maxTime
+    
+    Ostart = find(diff(diff(sqrt(sum(Opt((OptStartOffset+1):end,:).^2,2)))) ...
+        > osync/8, 1) + OptStartOffset;
+    
+end
+
+end
\ No newline at end of file

+math/IntSignals.m

+function data = IntSignals(data,index)
+
+if index == 1
+    data.rG = nan(data.N,3);
+    data.vG = nan(data.N,3);
+    data.aG = nan(data.N,3);
+end
+    
+% start with initial orientation of unit rotation matrix.
+rt=eye(3);
+
+% Current part of analysis.
+indexes = data.tm(index):data.tm(index+1);
+
+% Rotate the gyroscope data to Foot-frame.
+gF = (data.Rcal*data.dgyr(indexes,:)');
+
+% Integrate the signals from this orientation, such that everything is 
+% mapped in that frame.
+% RgsHat = intOmega(rt,data.dgyr(indexes,:),1/data.fs);
+RgsHat = intOmega(rt,gF',1/data.fs);
+
+aR = (data.Rcal*data.dacc(indexes,:)')';
+
+% Pre-allocate space.
+aG = nan(length(indexes),3);
+
+% Gravity.
+g = data.AccOffsetR;
+
+%% Get the free acceleration.
+for i = indexes
+    aG(i-indexes(1)+1,:) = permute(RgsHat(i-indexes(1)+1,:,:),[2 3 1])*...
+        aR(i-indexes(1)+1,:)'-g';
+end
+
+%% Integrate the free acceleration to obtain velocity.
+vG = nan(length(indexes),3);
+vG(1,:) = zeros(1,3);
+for i = 2:length(indexes)
+    vG(i,:) = vG(i-1,:) + aG(i,:)./data.fs;
+end
+
+timeAxis=(0:length(vG)-1)/data.fs;
+%% Linear velocity drift compensation.
+for i = 2:length(indexes)
+    vG(i,:) = vG(i,:) - timeAxis(i)/timeAxis(end) * vG(end,:);
+end
+
+%% Strapdown integration: obtain position.
+rG = nan(length(indexes),3);
+rG(1,:) = zeros(1,3);
+for i = 2:length(indexes)
+    rG(i,:) = rG(i-1,:) + vG(i,:)./data.fs;
+end
+
+data.FPA(index) = atand(rG(end,2)/rG(end,1));
+data.rG(indexes,:) = rG;
+data.vG(indexes,:) = vG;
+data.aG(indexes,:) = aG;
+
+end
\ No newline at end of file

+math/OptFPA.m

+function [data] = OptFPA(data, intdata, index)
+
+if index == 1
+    data.MarkerPosition.RFootF(data.MarkerPosition.RFootF == 0) = nan;
+    data.MarkerPosition.LFootF(data.MarkerPosition.RFootF == 0) = nan;
+    data.MarkerPosition.RFootB(data.MarkerPosition.RFootF == 0) = nan;
+    data.MarkerPosition.LFootB(data.MarkerPosition.RFootF == 0) = nan;
+
+    data.MarkerPosition.RFootM = (data.MarkerPosition.RFootF + data.MarkerPosition.RFootB)/2;
+    data.MarkerPosition.LFootM = (data.MarkerPosition.LFootF + data.MarkerPosition.LFootB)/2;
+end
+
+startIndex = intdata.tm(index);
+stopIndex = intdata.tm(index+1);
+
+if ~isempty(data.syncCorr)
+    if (stopIndex + data.syncCorr) <= length(data.MarkerPosition.RFootF) && ...
+            (startIndex + data.syncCorr) > 0
+
+        FootDir = data.MarkerPosition.RFootF(startIndex + data.syncCorr,:) - ...
+            data.MarkerPosition.RFootB(startIndex + data.syncCorr,:);
+
+        WalkDir = data.MarkerPosition.RFootM(stopIndex + data.syncCorr,:) - ...
+            data.MarkerPosition.RFootM(startIndex + data.syncCorr,:);
+
+        % We only need the xy-plane.
+        FootDir = FootDir(1:2);
+        WalkDir = WalkDir(1:2);
+
+         FPAcurrent = acosd(dot(WalkDir,FootDir)/(sqrt(sum(FootDir.^2))*sqrt(sum(WalkDir.^2))));
+
+         if FPAcurrent > 90
+
+             FPAcurrent = 180 - FPAcurrent;
+
+         end
+
+         V3 = cross([FootDir 0],[WalkDir 0]);
+         signage = sign(dot(V3, [0 0 1]));
+
+         data.FPA(index) = signage*FPAcurrent;
+
+    %     if data.FPA(index) > 90
+
+    end
+end
+end
\ No newline at end of file

+plotting/PlotBars.m

+function PlotBars(InputStruct,ylimits,labels)
+
+grid on
+grid minor
+hold on;
+
+yd = [InputStruct.Optical.Mean InputStruct.Inertial.Mean]';
+
+bar1 = bar([InputStruct.Optical.Mean InputStruct.Inertial.Mean]);
+
+errbar = [InputStruct.Optical.Std InputStruct.Inertial.Std]';
+hold on;
+pause(1)
+
+for i = 1:2
+    errorbar(bar1(i).XData + bar1(i).XOffset, yd(i,:),  errbar(i,:), 'ok', 'LineWidth',2)
+end
+
+ylim(ylimits)
+xticks(1:5)
+
+set(gca, 'XTickLabel', labels);
+set(gca,'FontSize',15)
+legend('Optical','Inertial','FontSize',10)
+xlabel('Subjects')
+ylabel('Angle (degrees)')
+end
\ No newline at end of file

+plotting/mynum2str.m

+% str = mynum2str(data, sigfig, maxdec) - converts a number to a string
+%
+% mynum2str examples (sigfig=3 and maxdec=2)
+% 123456.0 --> 123000
+% 123.4560 --> 123
+% 12.34560 --> 12.3
+% 1.234560 --> 1.23
+% 0.1234560 --> 0.12
+
+% By Ran Klein 2012???
+% Modified:
+% 2013-12-31  RK  Added NA result for empty data.
+
+function str = mynum2str(data, sigfig, maxdec)
+if isempty(data) || ~isfinite(data)
+	str = 'NA';
+	return
+end
+
+if nargin<2
+	sigfig = 3; % number of significant figures to display (before and after .)
+end
+if nargin<3
+	maxdec = 2; % maximum of decimals (after the .)
+end
+
+if isempty(sigfig)
+	sigfig = floor(log10(abs(data)))+ 1 + maxdec;
+end
+e = max(-sigfig+1,floor(log10(abs(data))));
+prec = max(-maxdec,e-sigfig+1); % precision
+p = round(data/10^(prec))*10^(prec);
+str = num2str(p);
+i = find(str=='.');
+% number of figures after the decimal
+if isempty(i)
+	i = length(str)+1;
+	na = 0;
+else
+	na = length(str)-i;
+end
+% number of figures befor the decimal
+t = str(1:i-1);
+if strcmpi(t,'0')
+	nb = 0; % a preceding zero doesn't count
+else
+	nb = sum(t>='0' & t<='9');
+end
+
+% number of trailing zeros to add
+n = min(sigfig-(na+nb),...
+	maxdec - na);
+if n>0
+	if i>length(str)
+		str = [str '.'];
+	end
+	str = [str '0'*ones(1,n)];
+end
\ No newline at end of file

+plotting/suptitle.m

+%SUPTITLE Puts a title above all subplots.
+%	SUPTITLE('text') adds text to the top of the figure
+%	above all subplots (a "super title"). Use this function
+%	after all subplot commands.
+
+% Drea Thomas 6/15/95 drea@mathworks.com
+
+% Warning: If the figure or axis units are non-default, this
+% will break.
+
+function hout=suptitle(varargin)
+
+str = varargin{1};
+if length(varargin)>1
+	params = varargin(2:2:end);
+	vals = varargin(3:2:end);
+	if length(params)~=length(vals)
+		error('Parameter and value pairs are not balanced');
+	end
+else
+	params = {};
+	vals = {};
+end
+
+% Parameters used to position the supertitle.
+
+% Amount of the figure window devoted to subplots
+plotregion = .92;
+
+% Y position of title in normalized coordinates
+titleypos  = .95;
+
+% Fontsize for supertitle
+fs = get(gcf,'defaultaxesfontsize')+4;
+
+% Fudge factor to adjust y spacing between subplots
+fudge=1;
+
+haold = get(gcf,'currentAxes');
+figunits = get(gcf,'units');
+
+% Get the (approximate) difference between full height (plot + title
+% + xlabel) and bounding rectangle.
+
+	if (~strcmp(figunits,'pixels')),
+		set(gcf,'units','pixels');
+		pos = get(gcf,'position');
+		set(gcf,'units',figunits);
+	else
+		pos = get(gcf,'position');
+	end
+	ff = (fs-4)*1.27*5/pos(4)*fudge;
+
+        % The 5 here reflects about 3 characters of height below
+        % an axis and 2 above. 1.27 is pixels per point.
+
+% Determine the bounding rectange for all the plots
+
+% h = findobj('Type','axes');   
+
+% findobj is a 4.2 thing.. if you don't have 4.2 comment out
+% the next line and uncomment the following block.
+	
+h = findobj(gcf,'Type','axes');  % Change suggested by Stacy J. Hills
+
+% If you don't have 4.2, use this code instead
+%ch = get(gcf,'children');
+%h=[];
+%for i=1:length(ch),
+%  if strcmp(get(ch(i),'type'),'axes'),
+%    h=[h,ch(i)];
+%  end
+%end
+
+	
+
+
+max_y=0;
+min_y=1;
+
+oldtitle =[];
+for i=1:length(h),
+	if (~strcmp(get(h(i),'Tag'),'suptitle')),
+		pos=get(h(i),'pos');
+		if (pos(2) < min_y), min_y=pos(2)-ff/5*3;end;
+		if (pos(4)+pos(2) > max_y), max_y=pos(4)+pos(2)+ff/5*2;end;
+	else
+		oldtitle = h(i);
+	end
+end
+
+if max_y > plotregion,
+	scale = (plotregion-min_y)/(max_y-min_y);
+	for i=1:length(h),
+		pos = get(h(i),'position');
+		pos(2) = (pos(2)-min_y)*scale+min_y;
+		pos(4) = pos(4)*scale-(1-scale)*ff/5*3;
+        pos(pos<0) = 0.05;
+		set(h(i),'position',pos);
+	end
+end
+
+np = get(gcf,'nextplot');
+set(gcf,'nextplot','add');
+if ~isempty(oldtitle)
+	delete(oldtitle);
+end
+ha=axes('pos',[0 1 1 1],'visible','off','Tag','suptitle');
+ht=text(.5,titleypos-1,str);set(ht,'horizontalalignment','center','fontsize',fs,'interpreter','none',params,vals);
+set(gcf,'nextplot',np);
+set(gcf,'currentAxes',haold);
+if nargout,
+	hout=ht;
+end
+
+
+

.gitignore

+# Windows default autosave extension
+*.asv
+
+# OSX / *nix default autosave extension
+*.m~
+
+# Compiled MEX binaries (all platforms)
+*.mex*
+
+# Packaged app and toolbox files
+*.mlappinstall
+*.mltbx
+
+# Generated helpsearch folders
+helpsearch*/
+
+# Simulink code generation folders
+slprj/
+sccprj/
+
+# Matlab code generation folders
+codegen/
+
+# Simulink autosave extension
+*.autosave
+
+# Simulink cache files
+*.slxc
+
+# Octave session info
+octave-workspace
+
+# Mat files
+*.mat
+
+# Mat figures
+*.eps
+*.jpg
+*.png
+*.fig
+
+# Folders
+/MeasurementData/
\ No newline at end of file

@MadgwickAHRS/MadgwickAHRS.m

+classdef MadgwickAHRS < handle
+%MADGWICKAHRS Implementation of Madgwick's IMU and AHRS algorithms
+%
+%   For more information see:
+%   http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+%
+%   Date          Author          Notes
+%   28/09/2011    SOH Madgwick    Initial release
+
+    %% Public properties
+    properties (Access = public)
+        SamplePeriod = 1/256;
+        Quaternion = [1 0 0 0];     % output quaternion describing the Earth relative to the sensor
+        Beta = 1;               	% algorithm gain
+    end
+
+    %% Public methods
+    methods (Access = public)
+        function obj = MadgwickAHRS(varargin)
+            for i = 1:2:nargin
+                if  strcmp(varargin{i}, 'SamplePeriod'), obj.SamplePeriod = varargin{i+1};
+                elseif  strcmp(varargin{i}, 'Quaternion'), obj.Quaternion = varargin{i+1};
+                elseif  strcmp(varargin{i}, 'Beta'), obj.Beta = varargin{i+1};
+                else error('Invalid argument');
+                end
+            end;
+        end
+        function obj = Update(obj, Gyroscope, Accelerometer, Magnetometer)
+            q = obj.Quaternion; % short name local variable for readability
+
+            % Normalise accelerometer measurement
+            if(norm(Accelerometer) == 0), return; end	% handle NaN
+            Accelerometer = Accelerometer / norm(Accelerometer);	% normalise magnitude
+
+            % Normalise magnetometer measurement
+            if(norm(Magnetometer) == 0), return; end	% handle NaN
+            Magnetometer = Magnetometer / norm(Magnetometer);	% normalise magnitude
+
+            % Reference direction of Earth's magnetic feild
+            h = quaternProd(q, quaternProd([0 Magnetometer], quaternConj(q)));
+            b = [0 norm([h(2) h(3)]) 0 h(4)];
+
+            % Gradient decent algorithm corrective step
+            F = [2*(q(2)*q(4) - q(1)*q(3)) - Accelerometer(1)
+                2*(q(1)*q(2) + q(3)*q(4)) - Accelerometer(2)
+                2*(0.5 - q(2)^2 - q(3)^2) - Accelerometer(3)
+                2*b(2)*(0.5 - q(3)^2 - q(4)^2) + 2*b(4)*(q(2)*q(4) - q(1)*q(3)) - Magnetometer(1)
+                2*b(2)*(q(2)*q(3) - q(1)*q(4)) + 2*b(4)*(q(1)*q(2) + q(3)*q(4)) - Magnetometer(2)
+                2*b(2)*(q(1)*q(3) + q(2)*q(4)) + 2*b(4)*(0.5 - q(2)^2 - q(3)^2) - Magnetometer(3)];
+            J = [-2*q(3),                 	2*q(4),                    -2*q(1),                         2*q(2)
+                2*q(2),                 	2*q(1),                    	2*q(4),                         2*q(3)
+                0,                         -4*q(2),                    -4*q(3),                         0
+                -2*b(4)*q(3),               2*b(4)*q(4),               -4*b(2)*q(3)-2*b(4)*q(1),       -4*b(2)*q(4)+2*b(4)*q(2)
+                -2*b(2)*q(4)+2*b(4)*q(2),	2*b(2)*q(3)+2*b(4)*q(1),	2*b(2)*q(2)+2*b(4)*q(4),       -2*b(2)*q(1)+2*b(4)*q(3)
+                2*b(2)*q(3),                2*b(2)*q(4)-4*b(4)*q(2),	2*b(2)*q(1)-4*b(4)*q(3),        2*b(2)*q(2)];
+            step = (J'*F);
+            step = step / norm(step);	% normalise step magnitude
+
+            % Compute rate of change of quaternion
+            qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]) - obj.Beta * step';
+
+            % Integrate to yield quaternion
+            q = q + qDot * obj.SamplePeriod;
+            obj.Quaternion = q / norm(q); % normalise quaternion
+        end
+        function obj = UpdateIMU(obj, Gyroscope, Accelerometer)
+            q = obj.Quaternion; % short name local variable for readability
+
+            % Normalise accelerometer measurement
+            if(norm(Accelerometer) == 0), return; end	% handle NaN
+            Accelerometer = Accelerometer / norm(Accelerometer);	% normalise magnitude
+
+            % Gradient decent algorithm corrective step
+            F = [2*(q(2)*q(4) - q(1)*q(3)) - Accelerometer(1)
+                2*(q(1)*q(2) + q(3)*q(4)) - Accelerometer(2)
+                2*(0.5 - q(2)^2 - q(3)^2) - Accelerometer(3)];
+            J = [-2*q(3),	2*q(4),    -2*q(1),	2*q(2)
+                2*q(2),     2*q(1),     2*q(4),	2*q(3)
+                0,         -4*q(2),    -4*q(3),	0    ];
+            step = (J'*F);
+            step = step / norm(step);	% normalise step magnitude
+
+            % Compute rate of change of quaternion
+            qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]) - obj.Beta * step';
+
+            % Integrate to yield quaternion
+            q = q + qDot * obj.SamplePeriod;
+            obj.Quaternion = q / norm(q); % normalise quaternion
+        end
+    end
+end
\ No newline at end of file

@MahonyAHRS/MahonyAHRS.m

+classdef MahonyAHRS < handle
+%MAYHONYAHRS Madgwick's implementation of Mayhony's AHRS algorithm
+%
+%   For more information see:
+%   http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+%
+%   Date          Author          Notes
+%   28/09/2011    SOH Madgwick    Initial release
+ 
+    %% Public properties
+    properties (Access = public)
+        SamplePeriod = 1/256;
+        Quaternion = [1 0 0 0];     % output quaternion describing the Earth relative to the sensor
+        Kp = 1;                     % algorithm proportional gain
+        Ki = 0;                     % algorithm integral gain
+    end
+    
+    %% Public properties
+    properties (Access = private)
+        eInt = [0 0 0];             % integral error
+    end    
+ 
+    %% Public methods
+    methods (Access = public)
+        function obj = MahonyAHRS(varargin)
+            for i = 1:2:nargin
+                if  strcmp(varargin{i}, 'SamplePeriod'), obj.SamplePeriod = varargin{i+1};
+                elseif  strcmp(varargin{i}, 'Quaternion'), obj.Quaternion = varargin{i+1};
+                elseif  strcmp(varargin{i}, 'Kp'), obj.Kp = varargin{i+1};
+                elseif  strcmp(varargin{i}, 'Ki'), obj.Ki = varargin{i+1};
+                else error('Invalid argument');
+                end
+            end;
+        end
+        function obj = Update(obj, Gyroscope, Accelerometer, Magnetometer)
+            q = obj.Quaternion; % short name local variable for readability
+ 
+            % Normalise accelerometer measurement
+            if(norm(Accelerometer) == 0), return; end   % handle NaN
+            Accelerometer = Accelerometer / norm(Accelerometer);    % normalise magnitude
+ 
+            % Normalise magnetometer measurement
+            if(norm(Magnetometer) == 0), return; end    % handle NaN
+            Magnetometer = Magnetometer / norm(Magnetometer);   % normalise magnitude
+ 
+            % Reference direction of Earth's magnetic feild
+            h = quaternProd(q, quaternProd([0 Magnetometer], quaternConj(q)));
+            b = [0 norm([h(2) h(3)]) 0 h(4)];
+            
+            % Estimated direction of gravity and magnetic field
+            v = [2*(q(2)*q(4) - q(1)*q(3))
+                 2*(q(1)*q(2) + q(3)*q(4))
+                 q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2];
+            w = [2*b(2)*(0.5 - q(3)^2 - q(4)^2) + 2*b(4)*(q(2)*q(4) - q(1)*q(3))
+                 2*b(2)*(q(2)*q(3) - q(1)*q(4)) + 2*b(4)*(q(1)*q(2) + q(3)*q(4))
+                 2*b(2)*(q(1)*q(3) + q(2)*q(4)) + 2*b(4)*(0.5 - q(2)^2 - q(3)^2)]; 
+ 
+            % Error is sum of cross product between estimated direction and measured direction of fields
+            e = cross(Accelerometer, v) + cross(Magnetometer, w); 
+            if(obj.Ki > 0)
+                obj.eInt = obj.eInt + e * obj.SamplePeriod;   
+            else
+                obj.eInt = [0 0 0];
+            end
+            
+            % Apply feedback terms
+            Gyroscope = Gyroscope + obj.Kp * e + obj.Ki * obj.eInt;            
+            
+            % Compute rate of change of quaternion
+            qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]);
+ 
+            % Integrate to yield quaternion
+            q = q + qDot * obj.SamplePeriod;
+            obj.Quaternion = q / norm(q); % normalise quaternion
+        end
+        function obj = UpdateIMU(obj, Gyroscope, Accelerometer)
+            q = obj.Quaternion; % short name local variable for readability
+ 
+            % Normalise accelerometer measurement
+            if(norm(Accelerometer) == 0), return; end   % handle NaN
+            Accelerometer = Accelerometer / norm(Accelerometer);	% normalise magnitude
+ 
+            % Estimated direction of gravity and magnetic flux
+            v = [2*(q(2)*q(4) - q(1)*q(3))
+                 2*(q(1)*q(2) + q(3)*q(4))
+                 q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2];
+ 
+            % Error is sum of cross product between estimated direction and measured direction of field
+            e = cross(Accelerometer, v); 
+            if(obj.Ki > 0)
+                obj.eInt = obj.eInt + e * obj.SamplePeriod;   
+            else
+                obj.eInt = [0 0 0];
+            end
+            
+            % Apply feedback terms
+            Gyroscope = Gyroscope + obj.Kp * e + obj.Ki * obj.eInt;            
+            
+            % Compute rate of change of quaternion
+            qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]);
+ 
+            % Integrate to yield quaternion
+            q = q + qDot * obj.SamplePeriod;
+            obj.Quaternion = q / norm(q); % normalise quaternion
+        end
+    end
+end
\ No newline at end of file

DataProcessing.m

+%% Do the data processing
+clear all; close all; clc
+
+% folder = 'E:\MiniSens\Git\FPA-est\FPA-measurements\S09\Calibration\';
+% folder = 'E:\MiniSens\Git\FPA-est\FPA-measurements\S09\Normal\';
+% folder = 'E:\MiniSens\Git\FPA-est\FPA-measurements\S09\Toe-in\';
+folder = 'E:\MiniSens\Git\FPA-est\FPA-measurements\S09\Toe-out\';
+
+C3Dfiles = dir([folder,'\*.C3D']);
+
+FileInd = 12;
+C3Ddata = database.prepareC3Ddataset(C3Dfiles(FileInd).name,folder);
+   
+%%
+% Store the sampling frequencies.
+C3Ddata.MarkerFs = VideoFrameRate;
+C3Ddata.MarkerPosition = MarkerPos;
+
+save([FullFileName(1:end-4) '-C3D.mat'],'C3Ddata')
+clearvars -except folder C3Dfiles FileInd
\ No newline at end of file

InitOrien.m

+function [rt, rmy, rmz, accOffsetR]=InitOrien(data, startIndex)
+% initpos.m determines the initial rotation matrix to map everything in the
+% reference frame. 
+%
+%input is gyroscope data, accelerometer data, sample frequency and number
+%of samples
+%it returns the rotation matrix, the error with y axis and the error with
+%the z-axis
+f_co =2;   % cut-off to 1 hz, so it is easier to find thresh holds
+%% filter
+% [Bf, Af] = butter(2, (2 * f_co / fs));% 2nd order low-pass butterworth filter
+% acc =filter(Bf, Af, dataInput.acc);
+% gyr =filter(Bf, Af, dataInput.gyr);
+% f_co=12; % to filter out unwanted vibration made by muscles at the heel lift 
+
+accSig = data.acc(startIndex:end,:);
+gyrSig = data.gyr(startIndex:end,:);
+
+[Bf, Af] = butter(2, (2 * f_co / data.fs));% 2nd order low-pass butterworth filter
+datacc =filtfilt(Bf, Af, accSig); %since the calibration data is not real time filtfilt is used, as this would give a better result.
+datgyr =filtfilt(Bf, Af, gyrSig);
+%% The threshold norm
+thr=0.25; %threshold waarde
+%% To find the steps.
+[tb te k] = zvt(datacc,datgyr,data.N-startIndex+1);
+
+% Find the Static and Dynamic parts of the calibration trial.
+StaticPart = floor(0.2*tb):floor(0.8*tb);
+Steps = find(diff(k));
+if k(1) == 1
+    Steps(1) = [];
+end
+DynamicPart1 = Steps(1):Steps(6);
+% DynamicPart1 = Steps(1):Steps(2);
+% DynamicPart2 = Steps(3):Steps(4);
+% DynamicPart3 = Steps(5):Steps(6);
+
+% Determine bias
+bd=mean(datgyr(StaticPart,:));
+datgyr=datgyr-bd;
+
+co1=pca((datgyr(DynamicPart1,:)-mean(datgyr(DynamicPart1,:))));
+m=co1(:,1)/norm(co1(:,1)); %bepaling via PCA
+
+% co2=pca((datgyr(DynamicPart2,:)-mean(datgyr(DynamicPart2,:))));
+% m2=co2(:,2)/norm(co2(:,2)); %bepaling via PCA
+% 
+% co3=pca((datgyr(DynamicPart3,:)-mean(datgyr(DynamicPart3,:))));
+% m3=co3(:,3)/norm(co3(:,3)); %bepaling via PCA
+% 
+% %% determining the average rotation axis
+% m=-(m1+m2+m3)/3;
+
+%% determination of the z-axis, average of 3 heel lifts is taken.
+zax=mean(datacc(StaticPart,:));
+zaxis=zax/norm(zax); 
+%% cross product between the rotation axis and z-axis
+xas=cross(m',zaxis)/norm(cross(m',zaxis));
+tzt=cross(xas,m)/norm(cross(xas,m));
+rmz=sqrt((tzt-zaxis).^2);
+txt=cross(zaxis,xas)/norm(cross(zaxis,xas));
+rmy=sqrt((txt-m').^2);
+%% the rotation matrix 
+rt=inv([xas',m,tzt']);
+
+% Determine acc offset
+accOffset = mean(datacc(StaticPart,:));
+
+% Rotate acc offset to foot frame.
+accOffsetR = (rt*accOffset')';
+end
\ No newline at end of file

MainCalc.m

+close all; clearvars; clc
+
+addpath('quaternion_library');  
+
+%% Define the paths.
+folderCalib = '\Calibration\';
+folderMeas = 'E:\MiniSens\Git\FPA-est\MeasurementData\';
+Optical = 'Optical\';
+Inertial = 'Inertial\';
+
+GaitType = {'\Normal'; '\Toe-in'; '\Toe-Out'};
+GaitAnalysis = 3;
+
+OptFiles = dir([folderMeas Optical]);
+IntFiles = dir([folderMeas Inertial]);
+
+dirFlags = [OptFiles.isdir];
+SubjectFolders = OptFiles(dirFlags);
+
+dirFlagsI = [IntFiles.isdir];
+SubjectFoldersI = IntFiles(dirFlagsI);
+
+LeftFoot = {'00B408F3', '00B408F9','00B408F9', '00B408F9', '00B408D5', ...
+    '00B408D8', '00B408F9', '00B408F9', '00B408F9', '00B408F9'};
+RightFoot = {'00B408F9', '00B408F3', '00B408F3', '00B408F3', '00B408D8', ...
+    '00B408D5', '00B408F3', '00B408F3', '00B408F3', '00B408F3'};
+
+fco = 10;
+isync = 41;
+osync = 8;
+maxTime = 300;
+
+gravity = 9.81;
+
+Results.FPA.Optical = nan(10,10,12);
+Results.FPA.Inertial = nan(10,10,12);
+CalibData.FPA.Optical = nan(10,10);
+CalibData.FPA.Inertial = nan(10,10);
+Results.Sync = nan(10,12);
+
+for i = 3:length(SubjectFolders)-1
+    
+    %% Perform the callibration steps.
+    CalibFilesI = dir([folderMeas Inertial SubjectFoldersI(i).name folderCalib, '*.txt']);
+    CalibFilesO = dir([folderMeas Optical SubjectFolders(i).name folderCalib, '*.mat']);
+    
+    for k = 1:length(CalibFilesI)
+       Rfeet(k) = isempty(strfind(CalibFilesI(k).name, RightFoot{i-2}));
+       Lfeet(k) = isempty(strfind(CalibFilesI(k).name, LeftFoot{i-2}));
+    end
+    
+    calibIndex = find(Rfeet,1);
+    
+    CalibData.(SubjectFolders(i).name).Istart = [];
+    
+    calIndex = 1;
+    
+    while isempty(CalibData.(SubjectFolders(i).name).Istart)
+    
+        CalibData.(SubjectFolders(i).name).inertial = ...
+            readCalibratedMTwLog([folderMeas Inertial SubjectFolders(i).name ...
+            folderCalib CalibFilesI(calibIndex+(calIndex-1)*2).name]);
+        CalibData.(SubjectFolders(i).name).Optical = ...
+            load([folderMeas Optical SubjectFolders(i).name folderCalib CalibFilesO(1+calIndex-1).name]);
+
+        [CalibData.(SubjectFolders(i).name).Istart, ...
+            CalibData.(SubjectFolders(i).name).Ostart] = ...
+            math.FindSync(CalibData.(SubjectFolders(i).name).Optical.C3Ddata.MarkerPosition.RFootF,...
+            CalibData.(SubjectFolders(i).name).inertial.acc,osync,isync,maxTime);
+
+        calIndex = calIndex + 1;
+    end
+    tic
+    %% Determine the calibration orientation.
+    [CalibData.(SubjectFolders(i).name).inertial.Rcal, rmy, rmz, ...
+        CalibData.(SubjectFolders(i).name).inertial.AccOffsetR] = ...
+        InitOrien(CalibData.(SubjectFolders(i).name).inertial,CalibData.(SubjectFolders(i).name).Istart);
+    
+    fs = CalibData.(SubjectFolders(i).name).inertial.fs;
+    
+    [Bf, Af] = butter(2, (2 * fco / fs));% 2nd order low-pass butterworth filter
+    
+    CalibData.(SubjectFolders(i).name).inertial.dacc = filter(Bf, Af, CalibData.(SubjectFolders(i).name).inertial.acc);
+    CalibData.(SubjectFolders(i).name).inertial.dgyr = filter(Bf, Af, CalibData.(SubjectFolders(i).name).inertial.gyr);
+    
+    % Calculate the sync correction between the different systems.
+    CalibData.(SubjectFolders(i).name).Optical.C3Ddata.syncCorr = ...
+        CalibData.(SubjectFolders(i).name).Ostart - ...
+        CalibData.(SubjectFolders(i).name).Istart;
+        
+    %% Determination of the start and end time of the steps. 
+
+    %refers to the zerovelocity update function
+    [CalibData.(SubjectFolders(i).name).inertial.tb, ...
+        CalibData.(SubjectFolders(i).name).inertial.te, ...
+        CalibData.(SubjectFolders(i).name).inertial.k] = ...
+        zvt(CalibData.(SubjectFolders(i).name).inertial.dacc,...
+        CalibData.(SubjectFolders(i).name).inertial.dgyr,...
+        CalibData.(SubjectFolders(i).name).inertial.N);
+    
+    % Correct for wrong first step.
+    if CalibData.(SubjectFolders(i).name).inertial.te(1) < ...
+            CalibData.(SubjectFolders(i).name).inertial.tb(1)
+        CalibData.(SubjectFolders(i).name).inertial.te(1) = [];
+    end
+        
+    % End with a full step.
+    if CalibData.(SubjectFolders(i).name).inertial.te(end) < ...
+            CalibData.(SubjectFolders(i).name).inertial.tb(end)
+        CalibData.(SubjectFolders(i).name).inertial.tb(end) = [];
+    end
+        
+    CalibData.(SubjectFolders(i).name).inertial.tm = ...
+        floor((CalibData.(SubjectFolders(i).name).inertial.tb(2:end) + ...
+        CalibData.(SubjectFolders(i).name).inertial.te(1:end-1))/2);
+        
+    % Number of steps is 1 smaller as it should end with foot contact.
+    Steps = 1:(length(CalibData.(SubjectFolders(i).name).inertial.tm)-1);
+        
+    for iStep = Steps
+
+        if iStep == 1
+            CalibResults.(SubjectFolders(i).name).FPA = nan(length(Steps),1);
+        end
+
+        CalibData.(SubjectFolders(i).name).inertial = ...
+            math.IntSignals(CalibData.(SubjectFolders(i).name).inertial, iStep);
+
+        CalibData.(SubjectFolders(i).name).Optical.C3Ddata = ...
+            math.OptFPA(CalibData.(SubjectFolders(i).name).Optical.C3Ddata, ...
+            CalibData.(SubjectFolders(i).name).inertial, iStep);
+
+    end
+    
+    if ~isempty(Steps) && ~isempty(CalibData.(SubjectFolders(i).name).Ostart)
+        CalibData.FPA.Inertial(i-2, 1:length(CalibData.(SubjectFolders(...
+            i).name).inertial.FPA)) = ...
+            CalibData.(SubjectFolders(i).name).inertial.FPA;
+        
+        CalibData.FPA.Optical(i-2, 1:length(CalibData.(SubjectFolders(...
+            i).name).Optical.C3Ddata.FPA)) = ...
+            CalibData.(SubjectFolders(i).name).Optical.C3Ddata.FPA;
+    end
+
+    %% Analyse the different trials.
+    MeasFiles = dir([folderMeas Inertial SubjectFolders(i).name GaitType{GaitAnalysis} '\', '*.txt']);
+    MeasFilesO = dir([folderMeas Optical SubjectFolders(i).name GaitType{GaitAnalysis} '\', '*.mat']);
+    
+    for j = 1:2:length(MeasFiles)
+        
+        for k = 1:2
+            RfeetI(k) = isempty(strfind(MeasFiles(j+k-1).name, RightFoot{i-2}));
+            LfeetI(k) = isempty(strfind(MeasFiles(j+k-1).name, LeftFoot{i-2}));
+        end
+        
+        cRightFoot = find(Rfeet,1);
+        
+        % Read optical data.
+        Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]) = ...
+            load([MeasFilesO((j+1)/2).folder '\' MeasFilesO((j+1)/2).name]);
+        
+        % Read inertial data.
+        Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial = ...
+            readCalibratedMTwLog([MeasFiles(j+cRightFoot-1).folder '\'...
+            MeasFiles(j+cRightFoot-1).name]);
+        
+        % Store all the callibration data.
+        Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.Rcal = CalibData.(SubjectFolders(i).name).inertial.Rcal;
+        Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.rmy = rmy;
+        Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.rmz = rmz;
+        Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.AccOffsetR = CalibData.(SubjectFolders(i).name).inertial.AccOffsetR;
+        
+        if j == 15 && i == 7
+            maxTime = 800;
+        elseif j == 5 && i == 11
+            maxTime = 800;
+        else
+            maxTime = 300;
+        end
+        
+        % Synchronize both systems.        
+        [Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.Istart, ...
+            Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).C3Ddata.Ostart] = ...
+            math.FindSync(Results.(SubjectFolders(i).name).(...
+            ['Trial' num2str((j+1)/2,'%02d')]).C3Ddata.MarkerPosition.RFootF,...
+            Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]...
+            ).inertial.acc,osync,isync,maxTime);
+        
+        % Calculate the sync correction between the different systems.
+        Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).C3Ddata.syncCorr = ...
+            Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).C3Ddata.Ostart - ...
+            Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.Istart;
+        
+        if ~isempty(Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).C3Ddata.syncCorr)
+            Results.Sync(i-2,(j+1)/2) = Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).C3Ddata.syncCorr;
+        end
+        
+        fs = Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.fs;
+        
+        [Bf, Af] = butter(2, (2 * fco / fs));% 2nd order low-pass butterworth filter
+        Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.dacc = ...
+            filter(Bf, Af, Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.acc);
+        Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.dgyr = ...
+            filter(Bf, Af, Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.gyr);
+        
+        %% Determination of the start and end time of the steps. 
+
+        %refers to the zerovelocity update function
+        [Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.tb, ...
+            Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.te, ...
+            Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.k]=...
+            zvt(Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.dacc,...
+            Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.dgyr,...
+            Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.N);
+        
+%         figure; 
+%         plot(Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]...
+%             ).inertial.dacc);
+%         hold on; 
+%         plot(Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]...
+%             ).inertial.k*50)
+%         
+%         close all
+        
+        % Correct for wrong first step.
+        if Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.te(1) < ...
+                Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.tb(1)
+            Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.te(1) = [];
+        end
+        
+        % End with a full step.
+        if Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.te(end) < ...
+                Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.tb(end)
+            Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.tb(end) = [];
+        end
+        
+        Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.tm = ...
+            floor((Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.tb(2:end) + ...
+            Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.te(1:end-1))/2);
+        
+        % Number of steps is 1 smaller as it should end with foot contact.
+        Steps = 1:(length(Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.tm)-1);
+        
+        for iStep = Steps
+            
+            if iStep == 1
+                Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.FPA = ...
+                    nan(length(Steps),1);
+                Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).C3Ddata.FPA = ...
+                    nan(length(Steps),1);
+            end
+            
+            Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial = ...
+                math.IntSignals(Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial, iStep);
+            
+            Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).C3Ddata = ...
+                math.OptFPA(Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).C3Ddata, ...
+                Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial, iStep);
+            
+        end
+        if ~isempty(Steps)
+            Results.FPA.Inertial(i-2, 1:length(Results.(SubjectFolders(i).name).(...
+                ['Trial' num2str((j+1)/2,'%02d')]).inertial.FPA),(j+1)/2) = ...
+                Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).inertial.FPA;
+            Results.FPA.Optical(i-2, 1:length(Results.(SubjectFolders(i).name).(...
+                ['Trial' num2str((j+1)/2,'%02d')]).C3Ddata.FPA),(j+1)/2) = ...
+                Results.(SubjectFolders(i).name).(['Trial' num2str((j+1)/2,'%02d')]).C3Ddata.FPA;
+        end
+        toc
+    end
+end
+
+save(['Results-' GaitType{GaitAnalysis}(2:end) '.mat'],'Results','CalibData')
\ No newline at end of file

PlotFPAopt.m

+clearvars; close all; clc
+
+load('Results-Normal.mat')
+
+chosenSubject = 'S01';
+
+Trials = fieldnames(Results.(chosenSubject));
+
+figure;
+set(gcf, 'Position', get(0, 'Screensize'));
+
+for i = 1:length(Trials)
+    
+    times = Results.(chosenSubject).(Trials{i}).inertial.tm + ...
+        Results.(chosenSubject).(Trials{i}).C3Ddata.syncCorr;
+    
+    times = times(times < length(Results.(chosenSubject).(Trials{i}).C3Ddata.MarkerPosition.RFootM));
+    
+    FootContact = Results.(chosenSubject).(Trials{i}).C3Ddata.MarkerPosition.RFootM(times,:);
+    FootDir = Results.(chosenSubject).(Trials{i}).C3Ddata.MarkerPosition.RFootF(times,:) - ...
+        Results.(chosenSubject).(Trials{i}).C3Ddata.MarkerPosition.RFootB(times,:);
+    
+    subplot(3,4,i)
+    plot(FootContact(:,1),FootContact(:,2))
+    hold on; 
+
+    for j = 1:size(FootDir,1)
+        plot([FootContact(j,1) FootContact(j,1) + FootDir(j,1)],...
+            [FootContact(j,2) FootContact(j,2) + FootDir(j,2)])
+    end
+
+    axis equal
+
+end
\ No newline at end of file

PlotFPAresults.m

+clear all; close all; clc
+
+load('SyncInfo.mat')
+
+outlierValue = 10;
+
+ylimitsNormal = [-5 25];
+ylimitsToeIn = [-25 10];
+ylimitsToeOut = [0 60];
+selectedSubjects = [1, 2, 4, 5, 7];
+
+labels = {'S01', 'S02', 'S03', 'S04', 'S05'};
+
+TI = load('Results-Toe-In.mat');
+TO = load('Results-Toe-Out.mat');
+NO = load('Results-Normal.mat');
+
+GoodSteps.Normal.Optical = nan(10,10,12);
+GoodSteps.Normal.Inertial = nan(10,10,12);
+GoodSteps.ToeOut.Optical = nan(10,10,12);
+GoodSteps.ToeOut.Inertial = nan(10,10,12);
+GoodSteps.ToeIn.Optical = nan(10,10,12);
+GoodSteps.ToeIn.Inertial = nan(10,10,12);
+
+for iSubject = 1:10
+    for iTrial = 1:12
+        
+        if SyncInfo.Normal(iSubject,iTrial) == 1
+            GoodSteps.Normal.Optical(iSubject,:,iTrial) = ...
+                NO.Results.FPA.Optical(iSubject,:,iTrial);
+            GoodSteps.Normal.Inertial(iSubject,:,iTrial) = ...
+                NO.Results.FPA.Inertial(iSubject,:,iTrial);
+        end
+        
+        if SyncInfo.ToeIn(iSubject,iTrial) == 1
+            GoodSteps.ToeIn.Optical(iSubject,:,iTrial) = ...
+                TI.Results.FPA.Optical(iSubject,:,iTrial);
+            GoodSteps.ToeIn.Inertial(iSubject,:,iTrial) = ...
+                TI.Results.FPA.Inertial(iSubject,:,iTrial);
+        end
+        
+        if SyncInfo.ToeOut(iSubject,iTrial) == 1
+            GoodSteps.ToeOut.Optical(iSubject,:,iTrial) = ...
+                TO.Results.FPA.Optical(iSubject,:,iTrial);
+            GoodSteps.ToeOut.Inertial(iSubject,:,iTrial) = ...
+                TO.Results.FPA.Inertial(iSubject,:,iTrial);
+        end
+        
+    end
+end
+
+GoodSteps.Callibration.Optical = NO.CalibData.FPA.Optical;
+GoodSteps.Callibration.Inertial = NO.CalibData.FPA.Inertial;
+
+RMSE.ToeOut = sqrt((GoodSteps.ToeOut.Optical-GoodSteps.ToeOut.Inertial).^2);
+RMSE.ToeIn = sqrt((GoodSteps.ToeIn.Optical-GoodSteps.ToeIn.Inertial).^2);
+RMSE.Normal = sqrt((GoodSteps.Normal.Optical-GoodSteps.Normal.Inertial).^2);
+
+GoodSteps.Normal.indexes = RMSE.Normal < outlierValue;
+GoodSteps.ToeIn.indexes = RMSE.ToeIn < outlierValue;
+GoodSteps.ToeOut.indexes = RMSE.ToeOut < outlierValue;
+
+GoodSteps.Normal.indexes(GoodSteps.Normal.Optical > 30) = 0;
+GoodSteps.Normal.indexes(GoodSteps.Normal.Inertial > 30) = 0;
+
+GoodSteps.ToeOut.indexes(GoodSteps.Normal.Optical < 0) = 0;
+GoodSteps.ToeOut.indexes(GoodSteps.Normal.Inertial < 0) = 0;
+
+Diff.Normal = GoodSteps.Normal.Optical-GoodSteps.Normal.Inertial;
+Diff.Normal(~GoodSteps.Normal.indexes) = nan;
+Diff.Normal = reshape(Diff.Normal,[10,120]);
+Diff.ToeIn = GoodSteps.ToeIn.Optical-GoodSteps.ToeIn.Inertial;
+Diff.ToeIn(~GoodSteps.ToeIn.indexes) = nan;
+Diff.ToeIn = reshape(Diff.ToeIn,[10,120]);
+Diff.ToeOut = GoodSteps.ToeOut.Optical-GoodSteps.ToeOut.Inertial;
+Diff.ToeOut(~GoodSteps.ToeOut.indexes) = nan;
+Diff.ToeOut = reshape(Diff.ToeOut,[10,120]);
+
+
+Diff.Mean.Normal = nanmean(Diff.Normal(selectedSubjects,:),2);
+Diff.Mean.ToeIn = nanmean(Diff.ToeIn(selectedSubjects,:),2);
+Diff.Mean.ToeOut = nanmean(Diff.ToeOut(selectedSubjects,:),2);
+
+Diff.Std.Normal = nanstd(Diff.Normal(selectedSubjects,:),0,2);
+Diff.Std.ToeIn = nanstd(Diff.ToeIn(selectedSubjects,:),0,2);
+Diff.Std.ToeOut = nanstd(Diff.ToeOut(selectedSubjects,:),0,2);
+
+RMSE.Callibration = sqrt((GoodSteps.Callibration.Optical-GoodSteps.Callibration.Inertial).^2);
+
+GoodSteps.Callibration.indexes = RMSE.Callibration < outlierValue-3;
+
+%% Calculate the callibration offset.
+OpticalCaliSub = GoodSteps.Callibration.Optical;
+OpticalCaliSub(~GoodSteps.Callibration.indexes) = nan;
+
+InertialCaliSub = GoodSteps.Callibration.Inertial;
+InertialCaliSub(~GoodSteps.Callibration.indexes) = nan;
+
+
+BarValues.CallibrationOffset = nanmean(OpticalCaliSub(selectedSubjects,:) - ...
+    InertialCaliSub(selectedSubjects,:),2);
+
+Diff.Calibration = OpticalCaliSub-InertialCaliSub;
+Diff.Mean.Calibration = nanmean(Diff.Calibration(selectedSubjects,:),2);
+Diff.Std.Calibration = nanstd(Diff.Calibration(selectedSubjects,:),0,2);
+
+%% Be sure to use the good normal steps.
+
+OpticalSub = GoodSteps.Normal.Optical;
+OpticalSub(~GoodSteps.Normal.indexes) = nan;
+BarValues.Normal.Optical.All = reshape(OpticalSub,[10,120]);
+InertialSub = GoodSteps.Normal.Inertial;
+InertialSub(~GoodSteps.Normal.indexes) = nan;
+BarValues.Normal.Inertial.All = reshape(InertialSub,[10,120]);
+
+BarValues.Normal.Optical.Mean = nanmean(BarValues.Normal.Optical.All(selectedSubjects,:,:),2);
+BarValues.Normal.Optical.Std = nanstd(BarValues.Normal.Optical.All(selectedSubjects,:,:),0,2);
+BarValues.Normal.Inertial.Mean = nanmean(BarValues.Normal.Inertial.All(selectedSubjects,:,:),2);
+BarValues.Normal.Inertial.Std = nanstd(BarValues.Normal.Inertial.All(selectedSubjects,:,:),0,2);
+
+BarValues.Normal.Optical.CorrectSteps = sum(~isnan(BarValues.Normal.Optical.All(selectedSubjects,:)),2);
+BarValues.Normal.Inertial.CorrectSteps = sum(~isnan(BarValues.Normal.Inertial.All(selectedSubjects,:)),2);
+
+%% Be sure to use the good ToeIn steps.
+
+OpticalSub = GoodSteps.ToeIn.Optical;
+OpticalSub(~GoodSteps.ToeIn.indexes) = nan;
+BarValues.ToeIn.Optical.All = reshape(OpticalSub,[10,120]);
+InertialSub = GoodSteps.ToeIn.Inertial;
+InertialSub(~GoodSteps.ToeIn.indexes) = nan;
+BarValues.ToeIn.Inertial.All = reshape(InertialSub,[10,120]);
+
+BarValues.ToeIn.Optical.Mean = nanmean(BarValues.ToeIn.Optical.All(selectedSubjects,:,:),2);
+BarValues.ToeIn.Optical.Std = nanstd(BarValues.ToeIn.Optical.All(selectedSubjects,:,:),0,2);
+BarValues.ToeIn.Inertial.Mean = nanmean(BarValues.ToeIn.Inertial.All(selectedSubjects,:,:),2);
+BarValues.ToeIn.Inertial.Std = nanstd(BarValues.ToeIn.Inertial.All(selectedSubjects,:,:),0,2);
+
+BarValues.ToeIn.Optical.CorrectSteps = sum(~isnan(BarValues.Normal.Optical.All(selectedSubjects,:)),2);
+BarValues.ToeIn.Inertial.CorrectSteps = sum(~isnan(BarValues.ToeIn.Inertial.All(selectedSubjects,:)),2);
+
+%% Be sure to use the good ToeOut steps.
+
+OpticalSub = GoodSteps.ToeOut.Optical;
+OpticalSub(~GoodSteps.ToeOut.indexes) = nan;
+BarValues.ToeOut.Optical.All = reshape(OpticalSub,[10,120]);
+InertialSub = GoodSteps.ToeOut.Inertial;
+InertialSub(~GoodSteps.ToeOut.indexes) = nan;
+BarValues.ToeOut.Inertial.All = reshape(InertialSub,[10,120]);
+
+BarValues.ToeOut.Optical.Mean = nanmean(BarValues.ToeOut.Optical.All(selectedSubjects,:,:),2);
+BarValues.ToeOut.Optical.Std = nanstd(BarValues.ToeOut.Optical.All(selectedSubjects,:,:),0,2);
+BarValues.ToeOut.Inertial.Mean = nanmean(BarValues.ToeOut.Inertial.All(selectedSubjects,:,:),2);
+BarValues.ToeOut.Inertial.Std = nanstd(BarValues.ToeOut.Inertial.All(selectedSubjects,:,:),0,2);
+
+BarValues.ToeOut.Optical.CorrectSteps = sum(~isnan(BarValues.Normal.Optical.All(selectedSubjects,:)),2);
+BarValues.ToeOut.Inertial.CorrectSteps = sum(~isnan(BarValues.ToeOut.Inertial.All(selectedSubjects,:)),2);
+
+%% Start plotting the results.
+
+figure('units','normalized','outerposition',[0 0 1 1])
+subplot(3,3,1)
+plotting.PlotBars(BarValues.Normal,ylimitsNormal,labels)
+title('Normal Gait')
+subplot(3,3,4)
+plotting.PlotBars(BarValues.ToeIn,ylimitsToeIn,labels)
+title('Toe-In Gait')
+subplot(3,3,7)
+plotting.PlotBars(BarValues.ToeOut,ylimitsToeOut,labels)
+title('Toe-Out Gait')
+
+Opt.Normal = nan(5,60);
+Int.Normal = nan(5,60);
+
+Opt.ToeIn = nan(5,50);
+Int.ToeIn = nan(5,50);
+
+Opt.ToeOut = nan(5,50);
+Int.ToeOut = nan(5,50);
+
+for i = 1:length(selectedSubjects)
+    
+    Val.Optical.Normal{i} = BarValues.Normal.Optical.All(selectedSubjects(i),:);
+    Val.Optical.Normal{i}(isnan(Val.Optical.Normal{i})) = [];
+    
+    Opt.Normal(i,1:length(Val.Optical.Normal{i})) = Val.Optical.Normal{i};
+
+    Val.Inertial.Normal{i} = BarValues.Normal.Inertial.All(selectedSubjects(i),:);
+    Val.Inertial.Normal{i}(isnan(Val.Inertial.Normal{i})) = [];
+    
+    Int.Normal(i,1:length(Val.Inertial.Normal{i})) = Val.Inertial.Normal{i};
+    
+    [Val.ttestC.Normal.h(i), Val.ttestC.Normal.p(i), Val.ttestC.Normal.ci(i,:), ...
+        Val.ttestC.Normal.stats(i)] = ttest(Val.Optical.Normal{i},Val.Inertial.Normal{i} + Diff.Mean.Calibration(i));
+    
+    [Val.ttest.Normal.h(i), Val.ttest.Normal.p(i), Val.ttest.Normal.ci(i,:), ...
+        Val.ttest.Normal.stats(i)] = ttest(Val.Optical.Normal{i},Val.Inertial.Normal{i});
+    
+    Val.Optical.ToeIn{i} = BarValues.ToeIn.Optical.All(selectedSubjects(i),:);
+    Val.Optical.ToeIn{i}(isnan(Val.Optical.ToeIn{i})) = [];
+    
+    Opt.ToeIn(i,1:length(Val.Optical.ToeIn{i})) = Val.Optical.ToeIn{i};
+
+    Val.Inertial.ToeIn{i} = BarValues.ToeIn.Inertial.All(selectedSubjects(i),:);
+    Val.Inertial.ToeIn{i}(isnan(Val.Inertial.ToeIn{i})) = [];
+    
+    Int.ToeIn(i,1:length(Val.Inertial.ToeIn{i})) = Val.Inertial.ToeIn{i};
+    
+    [Val.ttestC.ToeIn.h(i), Val.ttestC.ToeIn.p(i), Val.ttestC.ToeIn.ci(i,:), ...
+        Val.ttestC.ToeIn.stats(i)] = ttest(Val.Optical.ToeIn{i},Val.Inertial.ToeIn{i} + Diff.Mean.Calibration(i));
+    
+    [Val.ttest.ToeIn.h(i), Val.ttest.ToeIn.p(i), Val.ttest.ToeIn.ci(i,:), ...
+        Val.ttest.ToeIn.stats(i)] = ttest(Val.Optical.ToeIn{i},Val.Inertial.ToeIn{i});
+    
+    Val.Optical.ToeOut{i} = BarValues.ToeOut.Optical.All(selectedSubjects(i),:);
+    Val.Optical.ToeOut{i}(isnan(Val.Optical.ToeOut{i})) = [];
+    
+    Opt.ToeOut(i,1:length(Val.Optical.ToeOut{i})) = Val.Optical.ToeOut{i};
+
+    Val.Inertial.ToeOut{i} = BarValues.ToeOut.Inertial.All(selectedSubjects(i),:);
+    Val.Inertial.ToeOut{i}(isnan(Val.Inertial.ToeOut{i})) = [];
+    
+    Int.ToeOut(i,1:length(Val.Inertial.ToeOut{i})) = Val.Inertial.ToeOut{i};
+    
+    [Val.ttestC.ToeOut.h(i), Val.ttestC.ToeOut.p(i), Val.ttestC.ToeOut.ci(i,:), ...
+        Val.ttestC.ToeOut.stats(i)] = ttest(Val.Optical.ToeOut{i},Val.Inertial.ToeOut{i} + Diff.Mean.Calibration(i));
+    
+    [Val.ttest.ToeOut.h(i), Val.ttest.ToeOut.p(i), Val.ttest.ToeOut.ci(i,:), ...
+        Val.ttest.ToeOut.stats(i)] = ttest(Val.Optical.ToeOut{i},Val.Inertial.ToeOut{i});
+     
+end
+
+%% Bland Altman part.
+
+groups = {'S01'; 'S02'; 'S03'; 'S04'; 'S05'};
+corrinfo = {'r2','eq'};
+BAinfo = {'RPC(%)'};
+limits = 'auto';
+colors = parula(length(selectedSubjects));
+symbols = ['s','o','c','.','x'];
+
+tit = 'Normal Gait';
+label = {'Optical reference','Inertial estimate','degrees'};
+[MaxK.cr, MaxK.fig, MaxK.statsStruct] = plotting.BlandAltman(Opt.Normal',Int.Normal',label,tit,groups,'corrInfo',corrinfo,...
+    'baInfo',BAinfo,'axesLimits',limits,'colors',colors,'showFitCI',' on','symbols',symbols);
+
+tit = 'Toe-In Gait';
+[MaxK.cr, MaxK.fig, MaxK.statsStruct] = plotting.BlandAltman(Opt.ToeIn',Int.ToeIn',label,tit,groups,'corrInfo',corrinfo,...
+    'baInfo',BAinfo,'axesLimits',limits,'colors',colors,'showFitCI',' on','symbols',symbols);
+
+tit = 'Toe-Out Gait';
+[MaxK.cr, MaxK.fig, MaxK.statsStruct] = plotting.BlandAltman(Opt.ToeOut',Int.ToeOut',label,tit,groups,'corrInfo',corrinfo,...
+    'baInfo',BAinfo,'axesLimits',limits,'colors',colors,'showFitCI',' on','symbols',symbols);
+
+%% Bland Altman combined
+
+groups = {'S01'; 'S02'; 'S03'; 'S04'; 'S05'};
+corrinfo = {'r2','eq'};
+BAinfo = {'RPC(%)'};
+limits = 'auto';
+colors = parula(length(selectedSubjects));
+
+calibErrorMat = repmat(Diff.Mean.Calibration,[1,360]);
+allErrorMat = [Diff.Normal, Diff.ToeIn, Diff.ToeOut];
+symbols = ['s','o','c','.','x'];
+
+baFig = figure('units','normalized','outerposition',[0 0 1 1]);
+baPlot1 = subplot(3,4,1:2);
+baPlot2 = subplot(3,4,5:6);
+baPlot3 = subplot(3,4,9:10);
+
+tit = ' ';
+label = {'Optical reference','Inertial estimate','degrees'};
+[MaxK.cr, MaxK.fig, MaxK.statsStruct] = plotting.BlandAltman(baPlot1,Opt.Normal',Int.Normal',label,tit,groups,'corrInfo',corrinfo,...
+    'baInfo',BAinfo,'axesLimits',limits,'colors',colors,'showFitCI',' on','symbols',symbols);
+
+[MaxK.cr, MaxK.fig, MaxK.statsStruct] = plotting.BlandAltman(baPlot2,Opt.ToeIn',Int.ToeIn',label,tit,groups,'corrInfo',corrinfo,...
+    'baInfo',BAinfo,'axesLimits',limits,'colors',colors,'showFitCI',' on','symbols',symbols);
+
+[MaxK.cr, MaxK.fig, MaxK.statsStruct] = plotting.BlandAltman(baPlot3,Opt.ToeOut',Int.ToeOut',label,tit,groups,'corrInfo',corrinfo,...
+    'baInfo',BAinfo,'axesLimits',limits,'colors',colors,'showFitCI',' on','symbols',symbols);
+
+%% Bland Altman errors.
+
+groups = {'S01'; 'S02'; 'S03'; 'S04'; 'S05'};
+corrinfo = {'r2','eq'};
+BAinfo = {'RPC(%)'};
+limits = 'auto';
+colors = parula(length(selectedSubjects));
+symbols = ['s','o','c','.','x'];
+baFig2 = figure('units','normalized','outerposition',[0 0 1 1]);
+
+calibErrorMat = repmat(Diff.Mean.Calibration,[1,360]);
+allErrorMat = [Diff.Normal(selectedSubjects,:), Diff.ToeIn(selectedSubjects,:), ...
+    Diff.ToeOut(selectedSubjects,:)];
+
+tit = 'Comparing Errors with the estimation calibration error';
+label = {'Calibration error','FPA error','degrees'};
+[MaxK.cr, MaxK.fig, MaxK.statsStruct] = plotting.BlandAltman(calibErrorMat',...
+    allErrorMat',label,tit,groups,'corrInfo',corrinfo,'baInfo',BAinfo,...
+    'axesLimits',limits,'colors',colors,'showFitCI',' on','symbols',symbols,...
+    'MarkerSize',6);

PlotSync.m

+clearvars; close all; clc
+
+load('Results-Toe-In.mat')
+
+chosenSubject = 'S09';
+
+Trials = fieldnames(Results.(chosenSubject));
+
+figure;
+set(gcf, 'Position', get(0, 'Screensize'));
+
+for i = 1:length(Trials)
+    
+    syncTime = Results.(chosenSubject).(Trials{i}).C3Ddata.syncCorr;
+    OptPos = Results.(chosenSubject).(Trials{i}).C3Ddata.MarkerPosition.RFootF;
+    IntAcc = Results.(chosenSubject).(Trials{i}).inertial.dacc;
+    
+    OptAcc = diff(diff(sqrt(sum(OptPos.^2,2))));
+
+    
+    subplot(3,4,i)
+    
+    if syncTime > 0
+        plot(OptAcc(syncTime:end))
+        hold on;
+        plot(IntAcc)
+    else
+        plot(OptAcc)
+        hold on;
+        plot(IntAcc(-syncTime:end))
+    end
+        
+    
+
+end
\ No newline at end of file

RepairNames.m

+close all; clearvars; clc
+
+addpath('quaternion_library');  
+
+%% Define the paths.
+folderCalib = '\Calibration\';
+folderMeas = 'E:\MiniSens\Git\FPA-est\MeasurementData\';
+Optical = 'Optical\';
+Inertial = 'Inertial\';
+
+GaitType = {'\Normal'; '\Toe-in'; '\Toe-Out'};
+GaitAnalysis = 3;
+
+OptFiles = dir([folderMeas Optical]);
+
+dirFlags = [OptFiles.isdir];
+SubjectFolders = OptFiles(dirFlags);
+
+for i = 3:length(SubjectFolders)
+    
+    %% Perform the callibration steps.
+    CalibFilesO = dir([folderMeas Optical SubjectFolders(i).name folderCalib, '*.mat']);
+    
+    for cali = 1:length(CalibFilesO)
+        load([folderMeas Optical SubjectFolders(i).name folderCalib CalibFilesO(cali).name]);
+        
+        if ~isfield(C3Ddata.MarkerPosition,'RFootB')
+            if isfield(C3Ddata.MarkerPosition,'RfootB')
+                C3Ddata.MarkerPosition.RFootB = C3Ddata.MarkerPosition.RfootB;
+                C3Ddata.MarkerPosition = rmfield(C3Ddata.MarkerPosition,'RfootB');
+                save([folderMeas Optical SubjectFolders(i).name folderCalib CalibFilesO(cali).name],'C3Ddata')
+            else
+                test = 1;
+            end    
+        end
+    end
+    
+    MeasFilesO = dir([folderMeas Optical SubjectFolders(i).name GaitType{GaitAnalysis} '\', '*.mat']);
+    MeasFiles = dir([folderMeas Inertial SubjectFolders(i).name GaitType{GaitAnalysis} '\', '*.txt']);
+
+    
+    for j = 1:2:length(MeasFiles)
+        
+        load([MeasFilesO((j+1)/2).folder '\' MeasFilesO((j+1)/2).name]);
+        
+        if ~isfield(C3Ddata.MarkerPosition,'RFootB')
+            if isfield(C3Ddata.MarkerPosition,'RfootB')
+                C3Ddata.MarkerPosition.RFootB = C3Ddata.MarkerPosition.RfootB;
+                C3Ddata.MarkerPosition = rmfield(C3Ddata.MarkerPosition,'RfootB');
+                save([MeasFilesO((j+1)/2).folder '\' MeasFilesO((j+1)/2).name],'C3Ddata')
+            else
+                test = 1;
+            end  
+        end
+        
+        
+    end
+    
+end
\ No newline at end of file