From 4a235cc64e72f823d3b620638e37b5787e007f09 Mon Sep 17 00:00:00 2001
From: "AD\\WoudaFJ" <f.j.wouda@utwente.nl>
Date: Mon, 21 Aug 2023 12:25:42 +0200
Subject: [PATCH] Removed old files.
---
+database/prepareC3Ddataset.m | 70 -----
+database/readC3D.m | 22 --
+database/readC3D_All_data_GTV121_v2_42.p | Bin 2316 -> 0 bytes
+math/FindSync.m | 33 ---
+math/OptFPA.m | 47 ----
@MadgwickAHRS/MadgwickAHRS.m | 91 -------
@MahonyAHRS/MahonyAHRS.m | 107 --------
DataProcessing.m | 20 --
MainCalc.m | 273 --------------------
PlotFPAopt.m | 34 ---
PlotFPAresults.m | 301 ----------------------
PlotSync.m | 35 ---
RepairNames.m | 59 -----
findmov.m | 7 -
madgwick_algorithm_matlab/ExampleScript.m | 103 --------
quaternion_library/TestScript.m | 81 ------
quaternion_library/axisAngle2quatern.m | 21 --
quaternion_library/axisAngle2rotMat.m | 35 ---
quaternion_library/euler2rotMat.m | 27 --
quaternion_library/quatern2euler.m | 27 --
quaternion_library/quatern2rotMat.m | 24 --
quaternion_library/quaternConj.m | 15 --
quaternion_library/quaternProd.m | 19 --
quaternion_library/rotMat2euler.m | 21 --
quaternion_library/rotMat2quatern.m | 40 ---
readCalibratedMTwLog.m | 122 ---------
zvt.m | 62 -----
27 files changed, 1696 deletions(-)
delete mode 100644 +database/prepareC3Ddataset.m
delete mode 100644 +database/readC3D.m
delete mode 100644 +database/readC3D_All_data_GTV121_v2_42.p
delete mode 100644 +math/FindSync.m
delete mode 100644 +math/OptFPA.m
delete mode 100644 @MadgwickAHRS/MadgwickAHRS.m
delete mode 100644 @MahonyAHRS/MahonyAHRS.m
delete mode 100644 DataProcessing.m
delete mode 100644 MainCalc.m
delete mode 100644 PlotFPAopt.m
delete mode 100644 PlotFPAresults.m
delete mode 100644 PlotSync.m
delete mode 100644 RepairNames.m
delete mode 100644 findmov.m
delete mode 100644 madgwick_algorithm_matlab/ExampleScript.m
delete mode 100644 quaternion_library/TestScript.m
delete mode 100644 quaternion_library/axisAngle2quatern.m
delete mode 100644 quaternion_library/axisAngle2rotMat.m
delete mode 100644 quaternion_library/euler2rotMat.m
delete mode 100644 quaternion_library/quatern2euler.m
delete mode 100644 quaternion_library/quatern2rotMat.m
delete mode 100644 quaternion_library/quaternConj.m
delete mode 100644 quaternion_library/quaternProd.m
delete mode 100644 quaternion_library/rotMat2euler.m
delete mode 100644 quaternion_library/rotMat2quatern.m
delete mode 100644 readCalibratedMTwLog.m
delete mode 100644 zvt.m
diff --git a/+database/prepareC3Ddataset.m b/+database/prepareC3Ddataset.m
deleted file mode 100644
index cee79c6..0000000
--- a/+database/prepareC3Ddataset.m
+++ /dev/null
@@ -1,70 +0,0 @@
-% 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;
-
diff --git a/+database/readC3D.m b/+database/readC3D.m
deleted file mode 100644
index 2812a19..0000000
--- a/+database/readC3D.m
+++ /dev/null
@@ -1,22 +0,0 @@
-% 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
diff --git a/+database/readC3D_All_data_GTV121_v2_42.p b/+database/readC3D_All_data_GTV121_v2_42.p
deleted file mode 100644
index e6f2d53fa7a359b40bd16e5d5608e576ccae66fb..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 2316
zcmV+n3G?=LFfcAKFm^C7E-)|v3s4+*e*d)q019>h00`^=02Cm_`LpSoZnmv6f356t
zC8G?Eo7VO~UX`cjW&xUa$IQdXMOv-7xocjOPPD?)n-*g+TU!}!V^xY90n1=f(DF(&
zmZB93_bB>bep(tT?xnh3e^B2<cIB|NiHovROD+~kgm-Y-|G!yFEv%0KdOSGBTmJ97
zVa2uG84X=g=oe}OVcA6G#nEvv$JGH%3!T52cYq$34l({Zu4QHi5Tou3E`&(3<|pqn
zJNcIRR}W)~YZGIX_`M6`kf4AxN#T8i?bfi0{+wZ2<xPe9%PTIft$JZf9aE1b=*Vv1
zWC3$KSVpx&4#qrd7Aql^b=Ep|z*n3SS+#9*qc<F2vDD8FTFbk}%}kun1zoq<Sd#z#
z?~8(tZUDqrFKe(omBE@Z8Pp8yD2+#v#NQaJVM@0>7vX5do8nXr%BVwXZ$Z!vohN{{
zSJ9NDhi}Ps2tuprHb%}?F_7n>_#oXDdajK@xPuX=laf}k7h0?_G~H3;-BYBGE0(X@
zwU=01+nK$~Ubw$Kxi8mIx=nG)3>2usrf`OR>d%;{{;y+?>M!Z6p4}Vb#Nf>`G&(T)
zOzghL#y(z7*QCYBd?OSCO|3=3W%D9uG#xQ*v=h+QJbQVA2Cy~TdR$KL3y2JeJ<Bfz
zl+Mrtapo2eUdsQcQPU`r99|;(E_z3&GiFlUlme_HcVNC4!U;7%eA!m?x#=H_8a~~x
zSYt)AwExpuSufD2-k$^O_y-Y}C?(B9l?dDlby@!pg7P6q%a^EDH$M&haVGL&S1r46
zFYfKzjkfiNptbH_U9d5jPrrpRn;PVaq;4+wWqc6xwe3<gTb6$SZSd(>^8n6VUHh=i
z_O@uV0Li~Y03YkAx;FjmM{}0cqyS{%4irg{V=EJ3GP$a3WR}~u@9U%xJR*;U6+Hhz
zeS36j4T6qOJf8@M*GiNO1`5<LP&SPspqRuEY-=TVll>agwmFfAB12DFMkpD!450Zj
z+lD+D8k3e+LVQ)!Bdm#SYD%@7qw4WznNoiLa3BJSoZ45wc`U6dVt<XFl?Ku)lru1a
zK&2922OR2`OAwy9_d-k<M^s-l*q*`Ak9LRP+tqSeF)9e2YQ=yshrgyj#Fx$_Io*k)
z%jvJ<7+D}v@dl+)%3y1imqDQ|ZC;+U>1jFT^RbdiBI0A{EFUx|)9LUcO%ijiUOj;W
z%wCeFl|FTca0mlhkDeVAqY37k)XwD)t2DdIsRMbc(va6Ba4AtO0fu)8(Qdk1OUfYB
z+0HM`E0UfAPG|1P5NB<wSC1Sxh$>*I?f-!rfQ^MIeHn1W6QKVk5I*J1S5G-{IcE_<
z#-S5;v_3NlDV!opR5#xqz5L(7Hs&a%5bnH>;pF{#xzrt=J|B&k%jg&8ZW~DKf=b_W
zFMLgg=3pOZeB%0WWad#BMOR3(Pm(}^C%;f(MJxNp6TaacvXnj-s*95vVGWJmACJ6t
ztalT~a!Q5bIrvItmt}*cxnKzry)DCNbmbKEVAWxDd7YB~nnq1@u11RtT$=V*1!<rb
z4yw`}A821w{vw56&DJaRK@cpm901v8?E|Vskb(Z}9O6<mI0=*fy-!9Z1{?6|5N-v$
z4s6UFmP6vVfsnBXT2Ayfc15#PqIzTmI`15_3-=DxVE`6&a@6VK4Aps+W|}PBK&<|F
zQX@wKjq8Zjm1yt`hUIKox=hxF<@%)PQ*7;Is&&}YMks14EZG0T-jPi>vNB9o7n|Nj
z0Yhrq!JFz4HY~w<eb&KDL|`)?24k!sDDYZD*YvOpExS=MB*!Z{)8$NSMp!xH_P|p;
zt(fAO<CI79tsV+~xxOe)!4P|W6b*`t8uhokR@_3krXeA&j33%oX5@&D$b{>o-|ocE
z#2v2CcuKxQLoMMdnpIuzC5!HJzv=s!s1b4GkSafP2xg?{FQ@zROwCi3Y;2H?Wa%;S
zXkcUu_Le9!!77-qR6Wmn1g?GW!hwYBbRf%R6${U#>`#<H*Ce29B2_iN-4_@}ciKe;
zx|o{!TJgl{z_|h)mEr?O_CocjNSl6yP^%lu5n$sRP<rr&F)ra=3<8cVMHHzxu5pJt
zd7J9hXp{vL885CDr?g|P5y22c$(kgBiYG!s*p6GJE)_UDjagA|h#k>>VzANU+;B@|
zxBmcWF!Ii9mXO_pL9<>kOL*r%C`h-)H5tBLU~D^g3!73qQ5Zc_hWPJ)jMa~GeTCm<
zFr1rFn>~>I(A%|nQ|63CRVm&3R1knxS`<xsg0==sW}5NVvV{2FA_wJbX|97SXKh7l
zO`MJPiv#ri?B`UHm+8r_n9vsk?5}Qz_Z3$Q6O8zF_g`7gttla)$CHlO(Hikiw<9ZC
z)U`wpb-}{-GQQ#5k?CI;w^LJ;nKfI#N&f2*b}Vm@uLMWSt{kvVG_eLo^f`+~jx8*M
z$kL86*#fH8iPwnJgX;&BxntXzNj23mMA7CLdFvLLfar)UwzoI?#Olb-8cs*wb&?T~
zr*N3vLQA7~SDUaT!0LhxFn`{fC%0rJ8^i-I-GCNC*#0)}6o;W=lTX#i0MuUu>xNZQ
z0K?oIPmlIaMCYJ@YN%^$Du2Yu7XM_?m~gkrvZ0b3r=;_&XjGa2uU|G~%cbXFgt;7Z
zRKo?|)Ty5DUVkHxW0J{1J>^tI$%K_VtfN&k{A(5!)F^NkK3N%yrpAmIVs_16;qT!{
z5V?(ehDE9y&)vjP7*taqr;UULi%f~Ta#x?+i{MN83DgKvKra{7FtCTgef%yWPGLty
zhzZ+T4wD6K<`BGZNoBW}#6FYw9aS%l?#tLc^k3*EU~>JPNH_<M4SWR3JbUo#gL%X9
zZ_fb!?l9_q>KT1{X|dZSMn3i58>);GDBu{?arx6gn=Kw-_ksVk$1kQds+!r?{4)ia
z94G6aD7oKWtsoc;t@5J_3Q~~kUSu&<h^DvPo<rmUMU?G4MF0#ntiAmdsu^5mDVhsW
mtTY%nH0{zWe+XsFf**E)bfuksyme)@dJmWDi)`vT&tt6K7-vHO
diff --git a/+math/FindSync.m b/+math/FindSync.m
deleted file mode 100644
index f87fc63..0000000
--- a/+math/FindSync.m
+++ /dev/null
@@ -1,33 +0,0 @@
-%% 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
diff --git a/+math/OptFPA.m b/+math/OptFPA.m
deleted file mode 100644
index 0c4b69b..0000000
--- a/+math/OptFPA.m
+++ /dev/null
@@ -1,47 +0,0 @@
-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
diff --git a/@MadgwickAHRS/MadgwickAHRS.m b/@MadgwickAHRS/MadgwickAHRS.m
deleted file mode 100644
index 3a5cd07..0000000
--- a/@MadgwickAHRS/MadgwickAHRS.m
+++ /dev/null
@@ -1,91 +0,0 @@
-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
diff --git a/@MahonyAHRS/MahonyAHRS.m b/@MahonyAHRS/MahonyAHRS.m
deleted file mode 100644
index fc99d53..0000000
--- a/@MahonyAHRS/MahonyAHRS.m
+++ /dev/null
@@ -1,107 +0,0 @@
-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
diff --git a/DataProcessing.m b/DataProcessing.m
deleted file mode 100644
index a8565f3..0000000
--- a/DataProcessing.m
+++ /dev/null
@@ -1,20 +0,0 @@
-%% 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
diff --git a/MainCalc.m b/MainCalc.m
deleted file mode 100644
index 4f8ec99..0000000
--- a/MainCalc.m
+++ /dev/null
@@ -1,273 +0,0 @@
-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
diff --git a/PlotFPAopt.m b/PlotFPAopt.m
deleted file mode 100644
index e1837e1..0000000
--- a/PlotFPAopt.m
+++ /dev/null
@@ -1,34 +0,0 @@
-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
diff --git a/PlotFPAresults.m b/PlotFPAresults.m
deleted file mode 100644
index 7a68463..0000000
--- a/PlotFPAresults.m
+++ /dev/null
@@ -1,301 +0,0 @@
-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);
diff --git a/PlotSync.m b/PlotSync.m
deleted file mode 100644
index 1d4b061..0000000
--- a/PlotSync.m
+++ /dev/null
@@ -1,35 +0,0 @@
-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
diff --git a/RepairNames.m b/RepairNames.m
deleted file mode 100644
index 906a7c9..0000000
--- a/RepairNames.m
+++ /dev/null
@@ -1,59 +0,0 @@
-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
diff --git a/findmov.m b/findmov.m
deleted file mode 100644
index 5e1336f..0000000
--- a/findmov.m
+++ /dev/null
@@ -1,7 +0,0 @@
-function [Tb Tend norms]=findmov(data,thr)
-N=length(data);
-norms=sqrt(sum(data.^2,2)); % taking the magnitude of the data
-Tb=find(norms>thr,1,'first'); %searching first point data is bigger then threshold
-Tsend=find(norms(Tb:N)<thr,1,'first'); %searching first point data is smaller then thresh hold
-Tend=Tb+Tsend-1;
-end
\ No newline at end of file
diff --git a/madgwick_algorithm_matlab/ExampleScript.m b/madgwick_algorithm_matlab/ExampleScript.m
deleted file mode 100644
index f977b7c..0000000
--- a/madgwick_algorithm_matlab/ExampleScript.m
+++ /dev/null
@@ -1,103 +0,0 @@
-% ExampleScript.m
-%
-% This script demonstrates use of the MadgwickAHRS and MahonyAHRS algorithm
-% classes with example data. ExampleData.mat contains calibrated gyroscope,
-% accelerometer and magnetometer data logged from an AHRS device (x-IMU)
-% while it was sequentially rotated from 0 degrees, to +90 degree and then
-% to -90 degrees around the X, Y and Z axis. The script first plots the
-% example sensor data, then processes the data through the algorithm and
-% plots the output as Euler angles.
-%
-% Note that the Euler angle plot shows erratic behaviour in phi and psi
-% when theta approaches ±90 degrees. This due to a singularity in the Euler
-% angle sequence known as 'Gimbal lock'. This issue does not exist for a
-% quaternion or rotation matrix representation.
-%
-% Date Author Notes
-% 28/09/2011 SOH Madgwick Initial release
-% 13/04/2012 SOH Madgwick deg2rad function no longer used
-% 06/11/2012 Seb Madgwick radian to degrees calculation corrected
-
-%% Start of script
-
-addpath('quaternion_library'); % include quaternion library
-close all; % close all figures
-clear; % clear all variables
-clc; % clear the command terminal
-
-%% Import and plot sensor data
-
-load('ExampleData.mat');
-
-figure('Name', 'Sensor Data');
-axis(1) = subplot(3,1,1);
-hold on;
-plot(time, Gyroscope(:,1), 'r');
-plot(time, Gyroscope(:,2), 'g');
-plot(time, Gyroscope(:,3), 'b');
-legend('X', 'Y', 'Z');
-xlabel('Time (s)');
-ylabel('Angular rate (deg/s)');
-title('Gyroscope');
-hold off;
-axis(2) = subplot(3,1,2);
-hold on;
-plot(time, Accelerometer(:,1), 'r');
-plot(time, Accelerometer(:,2), 'g');
-plot(time, Accelerometer(:,3), 'b');
-legend('X', 'Y', 'Z');
-xlabel('Time (s)');
-ylabel('Acceleration (g)');
-title('Accelerometer');
-hold off;
-axis(3) = subplot(3,1,3);
-hold on;
-plot(time, Magnetometer(:,1), 'r');
-plot(time, Magnetometer(:,2), 'g');
-plot(time, Magnetometer(:,3), 'b');
-legend('X', 'Y', 'Z');
-xlabel('Time (s)');
-ylabel('Flux (G)');
-title('Magnetometer');
-hold off;
-linkaxes(axis, 'x');
-
-%% Process sensor data through algorithm
-
-AHRS = MadgwickAHRS('SamplePeriod', 1/256, 'Beta', 0.1);
-AHRS2 = MadgwickAHRS('SamplePeriod', 1/256, 'Beta', 0.1);
-% AHRS = MahonyAHRS('SamplePeriod', 1/256, 'Kp', 0.5);
-
-quaternion = zeros(length(time), 4);
-for t = 1:length(time)
- AHRS.Update(Gyroscope(t,:) * (pi/180), Accelerometer(t,:), Magnetometer(t,:)); % gyroscope units must be radians
- AHRS2.UpdateIMU(Gyroscope(t,:) * (pi/180), Accelerometer(t,:)); % gyroscope units must be radians
- quaternion(t, :) = AHRS.Quaternion;
- quaternion2(t, :) = AHRS2.Quaternion;
-end
-
-%% Plot algorithm output as Euler angles
-% The first and third Euler angles in the sequence (phi and psi) become
-% unreliable when the middle angles of the sequence (theta) approaches ±90
-% degrees. This problem commonly referred to as Gimbal Lock.
-% See: http://en.wikipedia.org/wiki/Gimbal_lock
-
-euler = quatern2euler(quaternConj(quaternion)) * (180/pi); % use conjugate for sensor frame relative to Earth and convert to degrees.
-euler2 = quatern2euler(quaternConj(quaternion2)) * (180/pi); % use conjugate for sensor frame relative to Earth and convert to degrees.
-
-figure('Name', 'Euler Angles');
-hold on;
-plot(time, euler(:,1), 'r');
-plot(time, euler(:,2), 'g');
-plot(time, euler(:,3), 'b');
-
-plot(time, euler2(:,1), '-r');
-plot(time, euler2(:,2), '-g');
-plot(time, euler2(:,3), '-b');
-title('Euler angles');
-xlabel('Time (s)');
-ylabel('Angle (deg)');
-legend('\phi', '\theta', '\psi');
-hold off;
-
-%% End of script
\ No newline at end of file
diff --git a/quaternion_library/TestScript.m b/quaternion_library/TestScript.m
deleted file mode 100644
index c98487a..0000000
--- a/quaternion_library/TestScript.m
+++ /dev/null
@@ -1,81 +0,0 @@
-% TestScript.m
-%
-% This script tests the quaternion library functions to ensure that each
-% function output is consistent.
-%
-% Date Author Notes
-% 27/09/2011 SOH Madgwick Initial release
-
-%% Start of script
-
-close all; % close all figures
-clear; % clear all variables
-clc; % clear the command terminal
-
-%% Axis-angle to rotation matrix
-
-axis = [1 2 3];
-axis = axis / norm(axis);
-angle = pi/2;
-
-R = axisAngle2rotMat(axis, angle);
-num = ' % 1.5f';
-a = sprintf('\rAxis-angle to rotation matrix:');
-b = sprintf(strcat('\r', num, '\t', num, '\t', num), R(1,:));
-c = sprintf(strcat('\r', num, '\t', num, '\t', num), R(2,:));
-d = sprintf(strcat('\r', num, '\t', num, '\t', num), R(3,:));
-disp(strcat(a,b,c,d));
-
-%% Axis-angle to quaternion
-
-q = axisAngle2quatern(axis, angle);
-num = ' % 1.5f';
-a = sprintf('\rAxis-angle to quaternion:');
-b = sprintf(strcat('\r', num, '\t', num, '\t', num, '\t', num), q);
-disp(strcat(a,b));
-
-%% Quaternion to rotation matrix
-
-R = quatern2rotMat(q);
-num = ' % 1.5f';
-a = sprintf('\rQuaternion to rotation matrix:');
-b = sprintf(strcat('\r', num, '\t', num, '\t', num), R(1,:));
-c = sprintf(strcat('\r', num, '\t', num, '\t', num), R(2,:));
-d = sprintf(strcat('\r', num, '\t', num, '\t', num), R(3,:));
-disp(strcat(a,b,c,d));
-
-%% Rotation matrix to quaternion
-
-q = rotMat2quatern(R);
-num = ' % 1.5f';
-a = sprintf('\rRotation matrix to quaternion:');
-b = sprintf(strcat('\r', num, '\t', num, '\t', num, '\t', num), q);
-disp(strcat(a,b));
-
-%% Rotation matrix to ZYX Euler angles
-
-euler = rotMat2euler(R);
-num = ' % 1.5f';
-a = sprintf('\rRotation matrix to ZYX Euler angles:');
-b = sprintf(strcat('\r', num, '\t', num, '\t', num), euler);
-disp(strcat(a,b));
-
-%% Quaternion to ZYX Euler angles
-
-euler = quatern2euler(q);
-num = ' % 1.5f';
-a = sprintf('\rQuaternion to ZYX Euler angles:');
-b = sprintf(strcat('\r', num, '\t', num, '\t', num), euler);
-disp(strcat(a,b));
-
-%% ZYX Euler angles to rotation matrix
-
-R = euler2rotMat(euler(1), euler(2), euler(3));
-num = ' % 1.5f';
-a = sprintf('\rZYX Euler angles to rotation matrix:');
-b = sprintf(strcat('\r', num, '\t', num, '\t', num), R(1,:));
-c = sprintf(strcat('\r', num, '\t', num, '\t', num), R(2,:));
-d = sprintf(strcat('\r', num, '\t', num, '\t', num), R(3,:));
-disp(strcat(a,b,c,d));
-
-%% End of script
\ No newline at end of file
diff --git a/quaternion_library/axisAngle2quatern.m b/quaternion_library/axisAngle2quatern.m
deleted file mode 100644
index 002a8ab..0000000
--- a/quaternion_library/axisAngle2quatern.m
+++ /dev/null
@@ -1,21 +0,0 @@
-function q = axisAngle2quatern(axis, angle)
-%AXISANGLE2QUATERN Converts an axis-angle orientation to a quaternion
-%
-% q = axisAngle2quatern(axis, angle)
-%
-% Converts and axis-angle orientation to a quaternion where a 3D rotation
-% is described by an angular rotation around axis defined by a vector.
-%
-% For more information see:
-% http://www.x-io.co.uk/node/8#quaternions
-%
-% Date Author Notes
-% 27/09/2011 SOH Madgwick Initial release
-
- q0 = cos(angle./2);
- q1 = -axis(:,1)*sin(angle./2);
- q2 = -axis(:,2)*sin(angle./2);
- q3 = -axis(:,3)*sin(angle./2);
- q = [q0 q1 q2 q3];
-end
-
diff --git a/quaternion_library/axisAngle2rotMat.m b/quaternion_library/axisAngle2rotMat.m
deleted file mode 100644
index 54c5653..0000000
--- a/quaternion_library/axisAngle2rotMat.m
+++ /dev/null
@@ -1,35 +0,0 @@
-function R = axisAngle2rotMat(axis, angle)
-%AXISANGLE2ROTMAT Converts an axis-angle orientation to a rotation matrix
-%
-% q = axisAngle2rotMat(axis, angle)
-%
-% Converts and axis-angle orientation to a rotation matrix where a 3D
-% rotation is described by an angular rotation around axis defined by a
-% vector.
-%
-% For more information see:
-% http://www.x-io.co.uk/node/8#quaternions
-%
-% Date Author Notes
-% 27/09/2011 SOH Madgwick Initial release
-
- kx = axis(:,1);
- ky = axis(:,2);
- kz = axis(:,3);
- cT = cos(angle);
- sT = sin(angle);
- vT = 1 - cos(angle);
-
- R(1,1,:) = kx.*kx.*vT + cT;
- R(1,2,:) = kx.*ky.*vT - kz.*sT;
- R(1,3,:) = kx.*kz.*vT + ky.*sT;
-
- R(2,1,:) = kx.*ky.*vT + kz.*sT;
- R(2,2,:) = ky.*ky.*vT + cT;
- R(2,3,:) = ky.*kz.*vT - kx.*sT;
-
- R(3,1,:) = kx.*kz.*vT - ky.*sT;
- R(3,2,:) = ky.*kz.*vT + kx.*sT;
- R(3,3,:) = kz.*kz.*vT + cT;
-end
-
diff --git a/quaternion_library/euler2rotMat.m b/quaternion_library/euler2rotMat.m
deleted file mode 100644
index 7e726c0..0000000
--- a/quaternion_library/euler2rotMat.m
+++ /dev/null
@@ -1,27 +0,0 @@
-function R = euler2rotMat(phi, theta, psi)
-%EULER2ROTMAT Converts a ZYX Euler angle orientation to a rotation matrix
-%
-% q = euler2rotMat(axis, angle)
-%
-% Converts ZYX Euler angle orientation to a rotation matrix where phi is
-% a rotation around X, theta around Y and psi around Z.
-%
-% For more information see:
-% http://www.x-io.co.uk/node/8#quaternions
-%
-% Date Author Notes
-% 27/09/2011 SOH Madgwick Initial release
-
- R(1,1,:) = cos(psi).*cos(theta);
- R(1,2,:) = -sin(psi).*cos(phi) + cos(psi).*sin(theta).*sin(phi);
- R(1,3,:) = sin(psi).*sin(phi) + cos(psi).*sin(theta).*cos(phi);
-
- R(2,1,:) = sin(psi).*cos(theta);
- R(2,2,:) = cos(psi).*cos(phi) + sin(psi).*sin(theta).*sin(phi);
- R(2,3,:) = -cos(psi).*sin(phi) + sin(psi).*sin(theta).*cos(phi);
-
- R(3,1,:) = -sin(theta);
- R(3,2,:) = cos(theta).*sin(phi);
- R(3,3,:) = cos(theta).*cos(phi);
-end
-
diff --git a/quaternion_library/quatern2euler.m b/quaternion_library/quatern2euler.m
deleted file mode 100644
index 22995ee..0000000
--- a/quaternion_library/quatern2euler.m
+++ /dev/null
@@ -1,27 +0,0 @@
-function euler = quatern2euler(q)
-%QUATERN2EULER Converts a quaternion orientation to ZYX Euler angles
-%
-% q = quatern2euler(q)
-%
-% Converts a quaternion orientation to ZYX Euler angles where phi is a
-% rotation around X, theta around Y and psi around Z.
-%
-% For more information see:
-% http://www.x-io.co.uk/node/8#quaternions
-%
-% Date Author Notes
-% 27/09/2011 SOH Madgwick Initial release
-
- R(1,1,:) = 2.*q(:,1).^2-1+2.*q(:,2).^2;
- R(2,1,:) = 2.*(q(:,2).*q(:,3)-q(:,1).*q(:,4));
- R(3,1,:) = 2.*(q(:,2).*q(:,4)+q(:,1).*q(:,3));
- R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2));
- R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2;
-
- phi = atan2(R(3,2,:), R(3,3,:) );
- theta = -atan(R(3,1,:) ./ sqrt(1-R(3,1,:).^2) );
- psi = atan2(R(2,1,:), R(1,1,:) );
-
- euler = [phi(1,:)' theta(1,:)' psi(1,:)'];
-end
-
diff --git a/quaternion_library/quatern2rotMat.m b/quaternion_library/quatern2rotMat.m
deleted file mode 100644
index 38dc945..0000000
--- a/quaternion_library/quatern2rotMat.m
+++ /dev/null
@@ -1,24 +0,0 @@
-function R = quatern2rotMat(q)
-%QUATERN2ROTMAT Converts a quaternion orientation to a rotation matrix
-%
-% R = quatern2rotMat(q)
-%
-% Converts a quaternion orientation to a rotation matrix.
-%
-% For more information see:
-% http://www.x-io.co.uk/node/8#quaternions
-%
-% Date Author Notes
-% 27/09/2011 SOH Madgwick Initial release
-
- R(1,1,:) = 2.*q(:,1).^2-1+2.*q(:,2).^2;
- R(1,2,:) = 2.*(q(:,2).*q(:,3)+q(:,1).*q(:,4));
- R(1,3,:) = 2.*(q(:,2).*q(:,4)-q(:,1).*q(:,3));
- R(2,1,:) = 2.*(q(:,2).*q(:,3)-q(:,1).*q(:,4));
- R(2,2,:) = 2.*q(:,1).^2-1+2.*q(:,3).^2;
- R(2,3,:) = 2.*(q(:,3).*q(:,4)+q(:,1).*q(:,2));
- R(3,1,:) = 2.*(q(:,2).*q(:,4)+q(:,1).*q(:,3));
- R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2));
- R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2;
-end
-
diff --git a/quaternion_library/quaternConj.m b/quaternion_library/quaternConj.m
deleted file mode 100644
index 5709fcd..0000000
--- a/quaternion_library/quaternConj.m
+++ /dev/null
@@ -1,15 +0,0 @@
-function qConj = quaternConj(q)
-%QUATERN2ROTMAT Converts a quaternion to its conjugate
-%
-% qConj = quaternConj(q)
-%
-% Converts a quaternion to its conjugate.
-%
-% For more information see:
-% http://www.x-io.co.uk/node/8#quaternions
-%
-% Date Author Notes
-% 27/09/2011 SOH Madgwick Initial release
-
- qConj = [q(:,1) -q(:,2) -q(:,3) -q(:,4)];
-end
diff --git a/quaternion_library/quaternProd.m b/quaternion_library/quaternProd.m
deleted file mode 100644
index 9c3d921..0000000
--- a/quaternion_library/quaternProd.m
+++ /dev/null
@@ -1,19 +0,0 @@
-function ab = quaternProd(a, b)
-%QUATERNPROD Calculates the quaternion product
-%
-% ab = quaternProd(a, b)
-%
-% Calculates the quaternion product of quaternion a and b.
-%
-% For more information see:
-% http://www.x-io.co.uk/node/8#quaternions
-%
-% Date Author Notes
-% 27/09/2011 SOH Madgwick Initial release
-
- ab(:,1) = a(:,1).*b(:,1)-a(:,2).*b(:,2)-a(:,3).*b(:,3)-a(:,4).*b(:,4);
- ab(:,2) = a(:,1).*b(:,2)+a(:,2).*b(:,1)+a(:,3).*b(:,4)-a(:,4).*b(:,3);
- ab(:,3) = a(:,1).*b(:,3)-a(:,2).*b(:,4)+a(:,3).*b(:,1)+a(:,4).*b(:,2);
- ab(:,4) = a(:,1).*b(:,4)+a(:,2).*b(:,3)-a(:,3).*b(:,2)+a(:,4).*b(:,1);
-end
-
diff --git a/quaternion_library/rotMat2euler.m b/quaternion_library/rotMat2euler.m
deleted file mode 100644
index c08dfd0..0000000
--- a/quaternion_library/rotMat2euler.m
+++ /dev/null
@@ -1,21 +0,0 @@
-function euler = rotMat2euler(R)
-%ROTMAT2EULER Converts a rotation matrix orientation to ZYX Euler angles
-%
-% euler = rotMat2euler(R)
-%
-% Converts a rotation matrix orientation to ZYX Euler angles where phi is
-% a rotation around X, theta around Y and psi around Z.
-%
-% For more information see:
-% http://www.x-io.co.uk/node/8#quaternions
-%
-% Date Author Notes
-% 27/09/2011 SOH Madgwick Initial release
-
- phi = atan2(R(3,2,:), R(3,3,:) );
- theta = -atan(R(3,1,:) ./ sqrt(1-R(3,1,:).^2) );
- psi = atan2(R(2,1,:), R(1,1,:) );
-
- euler = [phi(1,:)' theta(1,:)' psi(1,:)'];
-end
-
diff --git a/quaternion_library/rotMat2quatern.m b/quaternion_library/rotMat2quatern.m
deleted file mode 100644
index 9ec5bdf..0000000
--- a/quaternion_library/rotMat2quatern.m
+++ /dev/null
@@ -1,40 +0,0 @@
-function q = rotMat2quatern(R)
-%ROTMAT2QUATERN Converts a rotation matrix orientation to a quaternion
-%
-% q = axisAngle2quatern(axis, angle)
-%
-% Converts a rotation matrix orientation to a quaternion.
-%
-% For more information see:
-% http://www.x-io.co.uk/node/8#quaternions
-%
-% Date Author Notes
-% 27/09/2011 SOH Madgwick Initial release
-
- [row col numR] = size(R);
- q = zeros(numR, 4);
- K = zeros(4,4);
- for i = 1:numR
- K(1,1) = (1/3) * (R(1,1,i) - R(2,2,i) - R(3,3,i));
- K(1,2) = (1/3) * (R(2,1,i) + R(1,2,i));
- K(1,3) = (1/3) * (R(3,1,i) + R(1,3,i));
- K(1,4) = (1/3) * (R(2,3,i) - R(3,2,i));
- K(2,1) = (1/3) * (R(2,1,i) + R(1,2,i));
- K(2,2) = (1/3) * (R(2,2,i) - R(1,1,i) - R(3,3,i));
- K(2,3) = (1/3) * (R(3,2,i) + R(2,3,i));
- K(2,4) = (1/3) * (R(3,1,i) - R(1,3,i));
- K(3,1) = (1/3) * (R(3,1,i) + R(1,3,i));
- K(3,2) = (1/3) * (R(3,2,i) + R(2,3,i));
- K(3,3) = (1/3) * (R(3,3,i) - R(1,1,i) - R(2,2,i));
- K(3,4) = (1/3) * (R(1,2,i) - R(2,1,i));
- K(4,1) = (1/3) * (R(2,3,i) - R(3,2,i));
- K(4,2) = (1/3) * (R(3,1,i) - R(1,3,i));
- K(4,3) = (1/3) * (R(1,2,i) - R(2,1,i));
- K(4,4) = (1/3) * (R(1,1,i) + R(2,2,i) + R(3,3,i));
- [V,D] = eig(K);
- %p = find(max(D));
- %q = V(:,p)';
- q(i,:) = V(:,4)';
- q(i,:) = [q(i,4) q(i,1) q(i,2) q(i,3)];
- end
-end
\ No newline at end of file
diff --git a/readCalibratedMTwLog.m b/readCalibratedMTwLog.m
deleted file mode 100644
index ab51519..0000000
--- a/readCalibratedMTwLog.m
+++ /dev/null
@@ -1,122 +0,0 @@
-function data = readCalibratedMTwLog(filename)
-% data = readCalibratedMTwLog(filename)
-% read mtw exported txt file
-
-fid = fopen([filename]);
-
-if fid == -1
- disp('invalid mtw file');
-
-else
-
-data.StartTime = str2double(regexp(fgetl(fid),'[0-9]+','match'));
-data.fs = str2double(regexp(fgetl(fid),'[0-9]+\.[0-9]+','match'));
-data.Scenario = regexp(fgetl(fid),'[0-9]+\.[0-9]','match');
-empty_line = fgetl(fid);
-data.FW = regexp(fgetl(fid),'[0-9]+\.[0-9]+\.[0-9]+','match');
-
-tline = fgetl(fid);
-while isempty(regexp(tline,'Counter', 'once'))
- tline = fgetl(fid);
- header = regexp(tline,'\w+\s\w+\:','match');
-
-% if (strcmp(header,'Sample rate:'))
- data = regexp(tline,'(?<fs>\d+\.\d+)','names');
-% end
-end
-
-header = regexp(tline,'\t','split');
-numcols = length(header);
-
-values = textscan(fid, repmat('%f',1,numcols), 'delimiter', '\t');%, 'HeaderLines',4);
-
-fclose(fid);
-
-data.N = length(values{1});
-data.counter = values{1};
-
-%GIVES ERROR IF Nsamples>16 bits
-%if (data.counter(end)-data.counter(1)+1 ~= data.N)
-% error('Samples missing');
-%end
-
-for i = 1:length(values)
- %Remove first 0.25s at start and end samples
- ncut = round(0.5*data.fs);
- values{i} = values{i}(ncut:end-ncut);
-
- isnanValues = isnan(values{i});
- if sum(isnanValues)>0
- values{i}(isnanValues) = 0;
- %disp('NaN values set to zero
- end
-end
-
-data.N = length(values{1});
-data.counter = values{1};
-data.header{1} = 'counter';
-
-for i = 1:numcols
- if strcmp(header{i},'Acc_X')
- data.acc = [values{i} values{i+1} values{i+2}];
- data.header{end+1} = 'acc';
- end
-
- if strcmp(header{i},'Gyr_X')
- data.gyr = [values{i} values{i+1} values{i+2}];
- data.header{end+1} = 'gyr';
- end
-
- if strcmp(header{i},'Mag_X')
- data.mag = [values{i} values{i+1} values{i+2}];
- data.header{end+1} = 'mag';
- end
-
- if strcmp(header{i},'Quat_w')
- data.qgs = [values{i} values{i+1} values{i+2} values{i+3}];
- data.header{end+1} = 'qgs';
- end
-
- if strcmp(header{i},'Mat[1][1]')
-% warning('check if first column or rows')
- data.Rgs=zeros(data.N,3,3);
- data.Rsg=zeros(data.N,3,3);
- data.Rsg(:,:,1)=[values{i} values{i+1} values{i+2} ];
- data.Rsg(:,:,2)=[values{i+3} values{i+4} values{i+5} ];
- data.Rsg(:,:,3)=[values{i+6} values{i+7} values{i+8} ];
- data.Rgs(:,1,:)=[values{i} values{i+1} values{i+2} ];
- data.Rgs(:,2,:)=[values{i+3} values{i+4} values{i+5} ];
- data.Rgs(:,3,:)=[values{i+6} values{i+7} values{i+8} ];
- data.header{end+1} = 'RgsXsens';
- end
-
- if strcmp(header{i},'OriInc_w')
- data.dqgs = [values{i} values{i+1} values{i+2} values{i+3}];
- data.header{end+1} = 'dqgs';
- end
-
- if strcmp(header{i},'VelInc_X')
- data.dv = [values{i} values{i+1} values{i+2}];
- data.header{end+1} = 'dv';
- end
-
- if strcmp(header{i},'Temperature')
- data.temp = values{i};
- data.header{end+1} = 'temp';
- end
-
- if strcmp(header{i},'Pressure')
- data.pres = values{i};
- data.header{end+1} = 'pres';
- end
-
-end
-
-end
-%data.gyr = qrot(-data.gyr,data.qsg);
-%data.gyr = -data.gyr;
-%data.Acc = [values{9} values{10} values{11}];
-%data.Gyr = [values{12} values{13} values{14}];
-%data.Mag = [values{15} values{16} values{17}];
-%data.qsg = [values{19} values{20} values{21} values{22}];
-%data.P = [values{18}];
\ No newline at end of file
diff --git a/zvt.m b/zvt.m
deleted file mode 100644
index 822394b..0000000
--- a/zvt.m
+++ /dev/null
@@ -1,62 +0,0 @@
-%% Function to determine when the foot is stationary on the floor.
-% inputs:
-% acc - Nx3 matrix with acceleration values.
-% gyr - Nx# matrix with gyroscope values.
-
-% outputs:
-% tStart - indexes of initial contacts.
-% tEnd - indexes of toe-off.
-% contactVector - vector with indicators of contact.
-
-function [tStart, tEnd, contactVector] = zvt(acc,gyr)
-
-% Tranform gyroscope data in degrees/second.
-gyrDeg=gyr*(180/pi);
-
-% Obtain the magnitudes of acceleration and angular velocity.
-mgyr=sqrt(sum(gyrDeg.^2,2));
-macc=sqrt(sum(acc.^2,2));
-
-% Initialize N, k and v.
-N = length(mgyr);
-contactVector = zeros(1,length(mgyr));
-varianceWindow = zeros(1,length(mgyr));
-
-% loop over all samples starting at 11 for the first window.
-for i=11:N
- % Take a snapshot of the acceleration data and average it.
- maccWindow=macc(i-10:i-1);
- avgmaccWindow=sum(maccWindow)/10;
-
- % Calculate the variance within that window.
- varianceWindow(i-10)=sqrt(sum((maccWindow-avgmaccWindow).^2)/10);
-
- % If the variance is small, the gyroscope signal is small and the
- % acceleration magnitude is close to gravity the foot should be
- % stationary. For detailed explanation of the conditions see the
- % publication: https://doi.org/10.1186/s12984-021-00816-4
- if varianceWindow(i-10)<0.5 && mgyr(i-10)<50 && (9<macc(i-10) && macc(i-10)<11)
- contactVector(i-10)=0;
- else
- contactVector(i-10)=1;
- end
-end
-
-% Differentiate the contact vector to find indices of the contacts and
-% double check that the duration of contact is sufficient (10 samples).
-x=diff(contactVector);
-dx=find(abs(x)==1);
-
-% Correct the contact windows shorter than 10 samples to non-contact.
-for i=1:length(dx)-1
- if (dx(i+1)-dx(i))<10
- contactVector(dx(i):dx(i+1))=contactVector(dx(i)-1);
- end
-end
-
-% Recalculate the moments of contact to get indices.
-dx=(diff(contactVector));
-tStart=find(dx==1);
-tEnd=find(dx==-1);
-end
-
--
GitLab