Updated zvt to work with DOT data.

+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,:));

+math/zvt.m

-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
 

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);

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

settingsFile.m

 %% 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.