diff --git a/+math/initOrien.m b/+math/initOrien.m
index a8aff634fa6f397b8e58658f955bf9b9427df7e4..91209ae98bf4dc410c86f3f0acbdc24c94562ae3 100644
--- a/+math/initOrien.m
+++ b/+math/initOrien.m
@@ -22,7 +22,7 @@ filtAcc = filtfilt(Bf, Af, data.acc);
filtGyr = filtfilt(Bf, Af, data.gyr);
%% Step detection using the zero velocity update.
-[tStart, tEnd, contactVector] = math.zvt(filtAcc,filtGyr);
+[tStart, tEnd, contactVector] = math.zvt(filtAcc,filtGyr, data.fs);
% Find the Static and Dynamic parts of the calibration trial.
staticPart = floor(0.2*tStart):floor(0.8*tStart);
@@ -37,24 +37,26 @@ gyroBias = mean(data.gyr(staticPart,:));
gyroNoBias = data.gyr - gyroBias;
% Obtain the perpendicular walking direction principal component.
-co1 = pca((gyroNoBias(dynamicPart,:)-mean(gyroNoBias(dynamicPart,:))));
+yAxis = pca((gyroNoBias(dynamicPart,:)-mean(gyroNoBias(dynamicPart,:))));
% Normalize that direction to get one component of the calibration matrix.
-m = co1(:,1)/norm(co1(:,1));
+normYaxis = yAxis(:,1)/norm(yAxis(:,1));
-% determination of the z-axis, average of 3 heel lifts is taken.
+% determination of the (normalized) z-axis, average of 3 heel lifts is taken.
zax = mean(data.acc(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);
+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);
+txt = cross(zaxis,xas)/norm(cross(zaxis,xas));
+rmy = sqrt((txt-normYaxis').^2);
% the rotation matrix is obtained by adding the three directions together.
-rotMatrix=inv([xas',m,tzt']);
+rotMatrix = inv([xas',normYaxis,tzt']);
% Determine acc offset (gravity)
accOffset = mean(data.acc(staticPart,:));
diff --git a/+math/zvt.m b/+math/zvt.m
index 91b1791a58a3fb550593fb816b6be818fd66edfa..8b376463623b7402181fe91895a23a0deaea9a34 100644
--- a/+math/zvt.m
+++ b/+math/zvt.m
@@ -1,27 +1,57 @@
-function [tb te k] = zvt(acc,gyr,N)
-gyr=gyr*(180/pi); %tranform gyroscope data in degrees/second.
-mgyr=sqrt(sum(gyr.^2,2));
-macc=sqrt(sum(acc.^2,2));
-k=0; % just giving a random start value
+%% 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, dynamicsVector] = zvt(acc,gyr,fs)
+
+% Obtain the magnitudes of acceleration and angular velocity.
+mGyr=sqrt(sum(gyr.^2,2));
+mAcc=sqrt(sum(acc.^2,2));
+
+% Initialize N, k and v.
+N = length(mGyr);
+dynamicsVector = ones(length(mGyr),1);
+varianceWindow = zeros(length(mGyr),1);
+
+% loop over all samples starting at 11 for the first window.
for i=11:N
- da=macc(i-10:i-1); %local data.
- avda=sum(da)/10; % the average of the local data
- v(i-10)=sqrt(sum((da-avda).^2)/10); % variance of the local data
- if v(i-10)<0.5 && mgyr(i-10)<50 && (9<macc(i-10) && macc(i-10)<11) % the three conditions
- k(i-10)=0;
- else
- k(i-10)=1;
+ % 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)
+ dynamicsVector(i-10)=0;
end
end
-x=diff(k); % derivative is needed for the last condition check
+
+% Differentiate the contact vector to find indices of the contacts and
+% double check that the duration of contact is sufficient (10 samples).
+x=diff(dynamicsVector);
dx=find(abs(x)==1);
+
+% Correct the contact windows shorter than 100 ms to non-contact.
for i=1:length(dx)-1
- if (dx(i+1)-dx(i))<10
- k(dx(i):dx(i+1))=k(dx(i)-1);
+ if (dx(i+1)-dx(i))/fs <= 0.1
+ dynamicsVector(dx(i):dx(i+1))=dynamicsVector(dx(i)-1);
end
end
-dx=(diff(k));
-tb=find(dx==1);
-te=find(dx==-1);
+
+% Recalculate the moments of contact to get indices.
+dx=(diff(dynamicsVector));
+tStart=find(dx==1);
+tEnd=find(dx==-1);
end
diff --git a/getCalibration.m b/getCalibration.m
index 667abaf3cbb8c0eeac7fdfc23c251cfa9f811369..2cc405b740830b4ca72c48967ee1847b0734155a 100644
--- a/getCalibration.m
+++ b/getCalibration.m
@@ -16,7 +16,7 @@ function dataStruct = getCalibration(filePath)
% Get the current file path and trial name.
currFilePath = [calibFiles(i).folder '\' calibFiles(i).name];
- trialName = ['trial_' extractBefore(calibFiles(i).name, '.csv')];
+ trialName = ['trial_' strrep(extractBefore(calibFiles(i).name, '.csv'),' ', '_')];
% Load in the actual Movella Dot CSV file.
dataStruct.(trialName) = readDotCsv(currFilePath);
diff --git a/readDotCsv.m b/readDotCsv.m
index 4cb08ee56b10e276c731a34a843afe64e36a5436..3004a82fcc6255f1ba4f6279a4956e8acc1c0fba 100644
--- a/readDotCsv.m
+++ b/readDotCsv.m
@@ -14,6 +14,6 @@ function dataStruct = readDotCsv(filePath)
dataStruct.mag = table2array(currentData(:,{'Mag_X', 'Mag_Y', 'Mag_Z'}));
dataStruct.time = table2array(currentData(:,'SampleTimeFine')- ...
currentData(1,'SampleTimeFine'))/1000000;
- dataStruct.fs = round(1/(dataStruct.time(1) - dataStruct.time(2)));
+ dataStruct.fs = round(1/(dataStruct.time(2) - dataStruct.time(1)));
end
\ No newline at end of file
diff --git a/settingsFile.m b/settingsFile.m
index 346ba49a652fff7109eb0e0c5bcfc7505ed01a13..40b32a1e53469e232f229d68b89977e696002134 100644
--- a/settingsFile.m
+++ b/settingsFile.m
@@ -1,7 +1,7 @@
%% This file contains all the required settings.
% Specify the main direction for the subject data files.
-fileFolder = "C:\Measurements\FPA-measurements";
+fileFolder = "C:\Measurements\FPA-measurements\Subject2";
% Specify where the calibration folder is, has to be placed inside the
% subject folder.