diff --git a/+math/IntSignals.m b/+math/IntSignals.m
index c4890df5634049a668dc293c7aa60b5ebfccf219..fcbeba5540b413a9ea41344bac1677b9e871493b 100644
--- a/+math/IntSignals.m
+++ b/+math/IntSignals.m
@@ -1,63 +1,62 @@
-function data = intSignals(data,index)
+function data = intSignals(data, calibration, index)
% Pre-allocate structures for rotated signals.
if index == 1
- data.rG = nan(data.N,3);
- data.vG = nan(data.N,3);
- data.aG = nan(data.N,3);
+ data.posLocal = nan(data.N,3);
+ data.velLocal = nan(data.N,3);
+ data.accLocal = nan(data.N,3);
end
% start with initial orientation of unit rotation matrix.
-rt=eye(3);
+unitRotMat=eye(3);
% Current part of analysis.
indexes = data.tMiddle(index):data.tMiddle(index+1);
% Rotate the gyroscope data to Foot-frame.
-gF = (data.Rcal*data.dgyr(indexes,:)');
+gyroLocal = (calibration.rotMatrix*data.filtGyr(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);
+rotMatHat = math.intOmega(unitRotMat,gyroLocal',1/data.fs);
-aR = (data.Rcal*data.dacc(indexes,:)')';
+accLocal = (calibration.rotMatrix*data.filtAcc(indexes,:)')';
% Pre-allocate space.
-aG = nan(length(indexes),3);
+accFreeLocal = nan(length(indexes),3);
% Gravity.
-g = data.AccOffsetR;
+g = calibration.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';
+ accFreeLocal(i-indexes(1)+1,:) = permute(rotMatHat(i-indexes(1)+1,:,:),[2 3 1])*...
+ accLocal(i-indexes(1)+1,:)'-g';
end
%% Integrate the free acceleration to obtain velocity.
-vG = nan(length(indexes),3);
-vG(1,:) = zeros(1,3);
+velLocal = nan(length(indexes),3);
+velLocal(1,:) = zeros(1,3);
for i = 2:length(indexes)
- vG(i,:) = vG(i-1,:) + aG(i,:)./data.fs;
+ velLocal(i,:) = velLocal(i-1,:) + accFreeLocal(i,:)./data.fs;
end
-timeAxis=(0:length(vG)-1)/data.fs;
+timeAxis=(0:length(velLocal)-1)/data.fs;
%% Linear velocity drift compensation.
for i = 2:length(indexes)
- vG(i,:) = vG(i,:) - timeAxis(i)/timeAxis(end) * vG(end,:);
+ velLocal(i,:) = velLocal(i,:) - timeAxis(i)/timeAxis(end) * velLocal(end,:);
end
%% Strapdown integration: obtain position.
-rG = nan(length(indexes),3);
-rG(1,:) = zeros(1,3);
+posLocal = nan(length(indexes),3);
+posLocal(1,:) = zeros(1,3);
for i = 2:length(indexes)
- rG(i,:) = rG(i-1,:) + vG(i,:)./data.fs;
+ posLocal(i,:) = posLocal(i-1,:) + velLocal(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;
+data.FPA(index) = atand(posLocal(end,2)/posLocal(end,1));
+data.posLocal(indexes,:) = posLocal;
+data.velLocal(indexes,:) = velLocal;
+data.accLocal(indexes,:) = accFreeLocal;
end
\ No newline at end of file
diff --git a/+math/calculateSteps.m b/+math/calculateSteps.m
index 43da98e04e00f030c15244d1b09981c4ed74d457..bb695b68b7bcba7ed58928e0637f647ec47e3ac8 100644
--- a/+math/calculateSteps.m
+++ b/+math/calculateSteps.m
@@ -40,11 +40,11 @@ for iStep = steps
% Pre-allocate space for the FPA values.
if iStep == 1
- dataStruct.FPA = nan(length(steps),1);
+ data.FPA = nan(length(steps),1);
end
% Integrate the signals to obtain the FPA within a step.
- dataStruct = math.intSignals(dataStruct, iStep);
+ data = math.intSignals(data, calibration, iStep);
end
end
\ No newline at end of file
diff --git a/+math/intOmega.m b/+math/intOmega.m
new file mode 100644
index 0000000000000000000000000000000000000000..28fa1257bb70f65d508dae80f16310238cf526c3
--- /dev/null
+++ b/+math/intOmega.m
@@ -0,0 +1,36 @@
+function rotMat = intOmega(initRotMat, omega, ts)
+% intOmega.m - Calculates orientation matrix by integrating gyroscope signal
+%
+% Inputs:
+% initRotMat Initial orientation matrix
+% omega Angular velocity (rad/s)
+% ts Sample time
+%
+% Outputs:
+% rotMat Orientation matrix [Nsamples x 3 x 3]
+%
+% Martin Schepers, July 2005
+% Henk Kortier, Sep 2011
+% Frank Wouda, Aug 2023
+
+N = size(omega,1);
+omegaMatrix = zeros(N,3,3);
+
+for i=1:N
+ omegaMatrix(i,:,:) = [0 -omega(i,3) omega(i,2);
+ omega(i,3) 0 -omega(i,1);
+ -omega(i,2) omega(i,1) 0];
+end
+
+rotMat = zeros(N,3,3);
+rotMat(1,:,:) = initRotMat; % initial value
+
+for i=2:N
+ R = permute(rotMat(i-1, :, :),[2 3 1]);
+
+ Rdot = R*permute(omegaMatrix(i,:,:),[2 3 1]); % Integrate R
+ R = R + ts.*Rdot;
+
+ [A,~]=qr(R); % create proper rotation matrix
+ rotMat(i,:,:) = sqrt(A.*A).*sign(R); % get correct sign
+end
\ No newline at end of file
diff --git a/Main.m b/Main.m
index 2b08b77ffa09b02d792bd1546e19adf79c6e9b40..53dcd0aa0957955e8d533e47b3d2ebdbeaf210fa 100644
--- a/Main.m
+++ b/Main.m
@@ -1,6 +1,8 @@
%% This is the main file that runs on IMU data in csv format to obtain
% foot progression angle.
+clear all; close all; clc
+
% Run the settings file.
settingsFile
diff --git a/intOmega.m b/intOmega.m
deleted file mode 100644
index a3a5f992f8ff5f9b7d7e18c79fe4831b7c9d36fa..0000000000000000000000000000000000000000
--- a/intOmega.m
+++ /dev/null
@@ -1,35 +0,0 @@
-function [Rgs Rdot] = intOmega(Rgs_init, omega, t_sample)
-% intOmega.m - Calculates orientation matrix by integrating gyroscope signal
-%
-% Inputs:
-% Rgs_init Initial orientation matrix
-% omega Angular velocity (rad/s)
-% t_sample Sample time
-%
-% Outputs:
-% Rgs Orientation matrix [Nsamples x 3 x 3]
-%
-% Martin Schepers, July 2005
-% Henk Kortier, Sep 2011
-
-N = size(omega,1);
-omega_sg_s_tilde = zeros(N,3,3);
-
-for i=1:N
- omega_sg_s_tilde(i,:,:) = [0 -omega(i,3) omega(i,2);
- omega(i,3) 0 -omega(i,1);
- -omega(i,2) omega(i,1) 0];
-end
-
-Rgs = zeros(N,3,3);
-Rgs(1,:,:) = Rgs_init; % initial value
-
-for i=2:N
- R = permute(Rgs(i-1, :, :),[2 3 1]);
-
- Rdot = R*permute(omega_sg_s_tilde(i,:,:),[2 3 1]); % Integrate R
- R = R + t_sample.*Rdot;
-
- [A,~]=qr(R); % create proper rotation matrix
- Rgs(i,:,:) = sqrt(A.*A).*sign(R); % get correct sign
-end
\ No newline at end of file