From efdebb2699ac7533ce4fa12acc124d0ff71c804d Mon Sep 17 00:00:00 2001
From: "AD\\WoudaFJ" <f.j.wouda@utwente.nl>
Date: Tue, 15 Aug 2023 16:34:54 +0200
Subject: [PATCH] Added functions to show dependencies more clearly. Added
descriptions. TODO: update intSignals function.
---
+math/IntSignals.m | 2 +-
+math/calculateSteps.m | 26 ++++++++++++++++++++++++++
+math/correctSteps.m | 27 +++++++++++++++++++++++++++
+math/initOrien.m | 35 ++++++++++++++++++++++-------------
+math/zvt.m | 4 ++--
Main.m | 7 ++++++-
getCalibration.m | 7 +++----
readDotCsv.m | 3 +++
8 files changed, 90 insertions(+), 21 deletions(-)
create mode 100644 +math/calculateSteps.m
create mode 100644 +math/correctSteps.m
diff --git a/+math/IntSignals.m b/+math/IntSignals.m
index 1caa6e5..3c226f3 100644
--- a/+math/IntSignals.m
+++ b/+math/IntSignals.m
@@ -1,4 +1,4 @@
-function data = IntSignals(data,index)
+function data = intSignals(data,index)
if index == 1
data.rG = nan(data.N,3);
diff --git a/+math/calculateSteps.m b/+math/calculateSteps.m
new file mode 100644
index 0000000..dfaeffd
--- /dev/null
+++ b/+math/calculateSteps.m
@@ -0,0 +1,26 @@
+%% This function calculates the step parameters of interest.
+
+% input:
+% dataStruct - containing identified steps and acc/gyro data.
+
+% output:
+% dataStruct - containing calculated FPA.
+
+function dataStruct = calculateSteps(dataStruct)
+
+% Number of steps is 1 smaller as it should end with foot contact.
+steps = 1:(length(dataStruct.tMiddle)-1);
+
+% Loop over all steps.
+for iStep = steps
+
+ % Pre-allocate space for the FPA values.
+ if iStep == 1
+ dataStruct.FPA = nan(length(steps),1);
+ end
+
+ % Integrate the signals to obtain the FPA within a step.
+ dataStruct = math.intSignals(dataStruct, iStep);
+
+end
+end
\ No newline at end of file
diff --git a/+math/correctSteps.m b/+math/correctSteps.m
new file mode 100644
index 0000000..f779408
--- /dev/null
+++ b/+math/correctSteps.m
@@ -0,0 +1,27 @@
+%% This function makes sure that the steps start with contact and end with
+% a full contact phase.
+
+% input:
+% dataStruct - containing tStart, tEnd vectors to correct.
+
+% output:
+% dataStruct - with correct start and end indexes.
+
+function dataStruct = correctSteps(dataStruct)
+
+
+% Correct for wrong first step.
+if dataStruct.tEnd(1) < dataStruct.tStart(1)
+ dataStruct.tEnd(1) = [];
+end
+
+% End with a full step.
+if dataStruct.tEnd(end) < dataStruct.tStart(end)
+ dataStruct.tStart(end) = [];
+end
+
+% Get the middle of contact phases for later calculations.
+dataStruct.tMiddle = floor((dataStruct.tStart(2:end) + ...
+ dataStruct.tEnd(1:end-1))/2);
+
+end
\ No newline at end of file
diff --git a/+math/initOrien.m b/+math/initOrien.m
index 91209ae..f2fa384 100644
--- a/+math/initOrien.m
+++ b/+math/initOrien.m
@@ -9,8 +9,14 @@
% rmy - error of the rotation around y axis.
% rmz - error of the rotation around x axis.
% accOffsetR - Offset of the acceleration data.
+% tStart - start time of static parts.
+% tEnd - end time of static parts.
+% contactVector - boolean vector containing indicators of contact.
-function [rotMatrix, rmy, rmz, accOffsetR]=initOrien(data)
+function outStruct = initOrien(data)
+
+% Pre-allocate structure
+outStruct = struct();
% cut-off frequency of the filter.
fCutoff = 2;
@@ -18,23 +24,26 @@ fCutoff = 2;
% 2nd order low-pass butterworth filter to get the relevant motion data.
% Implement as filtfilt due to no realtime constraints.
[Bf, Af] = butter(2, (2 * fCutoff / data.fs));
-filtAcc = filtfilt(Bf, Af, data.acc);
-filtGyr = filtfilt(Bf, Af, data.gyr);
+outStruct.filtAcc = filtfilt(Bf, Af, data.acc);
+outStruct.filtGyr = filtfilt(Bf, Af, data.gyr);
%% Step detection using the zero velocity update.
-[tStart, tEnd, contactVector] = math.zvt(filtAcc,filtGyr, data.fs);
+[outStruct.tStart, outStruct.tEnd, outStruct.contactVector] = math.zvt(outStruct.filtAcc,outStruct.filtGyr, data.fs);
+
+% Correct the calculated steps.
+outStruct = math.correctSteps(outStruct);
% Find the Static and Dynamic parts of the calibration trial.
-staticPart = floor(0.2*tStart):floor(0.8*tStart);
-steps = find(diff(contactVector));
-if contactVector(1) == 1
+staticPart = floor(0.2*outStruct.tStart):floor(0.8*outStruct.tStart);
+steps = find(diff(outStruct.contactVector));
+if outStruct.contactVector(1) == 1
steps(1) = [];
end
dynamicPart = steps(1):steps(6);
% Determine bias and subtract it.
-gyroBias = mean(data.gyr(staticPart,:));
-gyroNoBias = data.gyr - gyroBias;
+outStruct.gyroBias = mean(data.gyr(staticPart,:));
+gyroNoBias = data.gyr - outStruct.gyroBias;
% Obtain the perpendicular walking direction principal component.
yAxis = pca((gyroNoBias(dynamicPart,:)-mean(gyroNoBias(dynamicPart,:))));
@@ -51,16 +60,16 @@ xas = cross(normYaxis',zaxis)/norm(cross(normYaxis',zaxis));
tzt = cross(xas,normYaxis)/norm(cross(xas,normYaxis));
% Calculate the errors of the axes with respect to being perpendicular.
-rmz = sqrt((tzt-zaxis).^2);
+outStruct.rmz = sqrt((tzt-zaxis).^2);
txt = cross(zaxis,xas)/norm(cross(zaxis,xas));
-rmy = sqrt((txt-normYaxis').^2);
+outStruct.rmy = sqrt((txt-normYaxis').^2);
% the rotation matrix is obtained by adding the three directions together.
-rotMatrix = inv([xas',normYaxis,tzt']);
+outStruct.rotMatrix = inv([xas',normYaxis,tzt']);
% Determine acc offset (gravity)
accOffset = mean(data.acc(staticPart,:));
% Rotate acc offset to foot frame.
-accOffsetR = (rotMatrix*accOffset')';
+outStruct.accOffsetR = (outStruct.rotMatrix*accOffset')';
end
\ No newline at end of file
diff --git a/+math/zvt.m b/+math/zvt.m
index 8b37646..80e22ac 100644
--- a/+math/zvt.m
+++ b/+math/zvt.m
@@ -1,7 +1,7 @@
%% Function to determine when the foot is stationary on the floor.
% inputs:
-% acc - Nx3 matrix with acceleration values.
-% gyr - Nx# matrix with gyroscope values.
+% acc - Nx3 matrix with acceleration values (in m/s^2).
+% gyr - Nx# matrix with gyroscope values (in deg/s).
% outputs:
% tStart - indexes of initial contacts.
diff --git a/Main.m b/Main.m
index 373aedd..516837f 100644
--- a/Main.m
+++ b/Main.m
@@ -22,9 +22,14 @@ for i = 3:length(subjectFolders)
for j = 1:length(trialFiles)
currentPath = [trialFiles(j).folder '\' trialFiles(j).name];
- trialName = ['trial_' extractBefore(trialFiles(j).name, '.csv')];
+ trialName = strrep(['trial_' extractBefore(trialFiles(j).name, '.csv')], ' ','_');
% Store the data in a useable format.
dataStruct.(subjectFolders(i).name).(trialName) = readDotCsv(currentPath);
+
+ % calculate all values for each step.
+ dataStruct.(subjectFolders(i).name).(trialName) = ...
+ math.calculateSteps(dataStruct.(subjectFolders(i).name).(trialName));
+
end
end
\ No newline at end of file
diff --git a/getCalibration.m b/getCalibration.m
index 2cc405b..1bb3dde 100644
--- a/getCalibration.m
+++ b/getCalibration.m
@@ -27,13 +27,12 @@ function dataStruct = getCalibration(filePath)
end
% Determine the calibration orientation.
- [dataStruct.(trialName).calibration.Rcal, ...
- dataStruct.(trialName).calibration.rmy, ...
- dataStruct.(trialName).calibration.rmz, ...
- dataStruct.(trialName).AccOffsetR] = ...
+ dataStruct.(trialName).calibration = ...
math.initOrien(dataStruct.(trialName));
+
+
end
diff --git a/readDotCsv.m b/readDotCsv.m
index 3004a82..5377286 100644
--- a/readDotCsv.m
+++ b/readDotCsv.m
@@ -14,6 +14,9 @@ function dataStruct = readDotCsv(filePath)
dataStruct.mag = table2array(currentData(:,{'Mag_X', 'Mag_Y', 'Mag_Z'}));
dataStruct.time = table2array(currentData(:,'SampleTimeFine')- ...
currentData(1,'SampleTimeFine'))/1000000;
+
+ % Calculate the magnitic norm and sample frequency
+ dataStruct.magNorm = sum(dataStruct.mag.^2, 2);
dataStruct.fs = round(1/(dataStruct.time(2) - dataStruct.time(1)));
end
\ No newline at end of file
--
GitLab