Started renaming variables and deleting unnecessary operations. to be tested with DOT data!

+math/initOrien.m

+%% This function calculates the orientation of the sensor with respect to
+% the walking direction.
+
+% inputs:
+% data - structure containing acc, gyr, mag, time, fs fields of data.
+
+% outputs:
+% rt - rotation matrix to link sensor orientation to foot orientation.
+% rmy - error of the rotation around y axis.
+% rmz - error of the rotation around x axis.
+% accOffsetR - Offset of the acceleration data.
+
+function [rotMatrix, rmy, rmz, accOffsetR]=initOrien(data)
+
+% cut-off frequency of the filter.
+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);
+
+%% Step detection using the zero velocity update.
+[tStart, tEnd, contactVector] = math.zvt(filtAcc,filtGyr);
+
+% 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
+    steps(1) = [];
+end
+dynamicPart = steps(1):steps(6);
+
+% Determine bias and subtract it.
+gyroBias = mean(data.gyr(staticPart,:));
+gyroNoBias = data.gyr - gyroBias;
+
+% Obtain the perpendicular walking direction principal component.
+co1 = pca((gyroNoBias(dynamicPart,:)-mean(gyroNoBias(dynamicPart,:))));
+
+% Normalize that direction to get one component of the calibration matrix.
+m = co1(:,1)/norm(co1(:,1));
+
+% determination of the 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);
+
+% the rotation matrix  is obtained by adding the three directions together.
+rotMatrix=inv([xas',m,tzt']);
+
+% Determine acc offset (gravity)
+accOffset = mean(data.acc(staticPart,:));
+
+% Rotate acc offset to foot frame.
+accOffsetR = (rotMatrix*accOffset')';
+end
\ No newline at end of file

+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
+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;
+    end
+end
+x=diff(k); % derivative is needed for the last condition check
+dx=find(abs(x)==1);
+for i=1:length(dx)-1
+    if (dx(i+1)-dx(i))<10
+        k(dx(i):dx(i+1))=k(dx(i)-1);
+    end
+end
+dx=(diff(k));
+tb=find(dx==1);
+te=find(dx==-1);
+end
+

InitOrien.m

-function [rt, rmy, rmz, accOffsetR]=InitOrien(data, startIndex)
-% initpos.m determines the initial rotation matrix to map everything in the
-% reference frame. 
-%
-%input is gyroscope data, accelerometer data, sample frequency and number
-%of samples
-%it returns the rotation matrix, the error with y axis and the error with
-%the z-axis
-f_co =2;   % cut-off to 1 hz, so it is easier to find thresh holds
-%% filter
-% [Bf, Af] = butter(2, (2 * f_co / fs));% 2nd order low-pass butterworth filter
-% acc =filter(Bf, Af, dataInput.acc);
-% gyr =filter(Bf, Af, dataInput.gyr);
-% f_co=12; % to filter out unwanted vibration made by muscles at the heel lift 
-
-accSig = data.acc(startIndex:end,:);
-gyrSig = data.gyr(startIndex:end,:);
-
-[Bf, Af] = butter(2, (2 * f_co / data.fs));% 2nd order low-pass butterworth filter
-datacc =filtfilt(Bf, Af, accSig); %since the calibration data is not real time filtfilt is used, as this would give a better result.
-datgyr =filtfilt(Bf, Af, gyrSig);
-%% The threshold norm
-thr=0.25; %threshold waarde
-%% To find the steps.
-[tb te k] = zvt(datacc,datgyr,data.N-startIndex+1);
-
-% Find the Static and Dynamic parts of the calibration trial.
-StaticPart = floor(0.2*tb):floor(0.8*tb);
-Steps = find(diff(k));
-if k(1) == 1
-    Steps(1) = [];
-end
-DynamicPart1 = Steps(1):Steps(6);
-% DynamicPart1 = Steps(1):Steps(2);
-% DynamicPart2 = Steps(3):Steps(4);
-% DynamicPart3 = Steps(5):Steps(6);
-
-% Determine bias
-bd=mean(datgyr(StaticPart,:));
-datgyr=datgyr-bd;
-
-co1=pca((datgyr(DynamicPart1,:)-mean(datgyr(DynamicPart1,:))));
-m=co1(:,1)/norm(co1(:,1)); %bepaling via PCA
-
-% co2=pca((datgyr(DynamicPart2,:)-mean(datgyr(DynamicPart2,:))));
-% m2=co2(:,2)/norm(co2(:,2)); %bepaling via PCA
-% 
-% co3=pca((datgyr(DynamicPart3,:)-mean(datgyr(DynamicPart3,:))));
-% m3=co3(:,3)/norm(co3(:,3)); %bepaling via PCA
-% 
-% %% determining the average rotation axis
-% m=-(m1+m2+m3)/3;
-
-%% determination of the z-axis, average of 3 heel lifts is taken.
-zax=mean(datacc(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);
-%% the rotation matrix 
-rt=inv([xas',m,tzt']);
-
-% Determine acc offset
-accOffset = mean(datacc(StaticPart,:));
-
-% Rotate acc offset to foot frame.
-accOffsetR = (rt*accOffset')';
-end
\ No newline at end of file

Main.m

+%% This is the main file that runs on IMU data in csv format to obtain 
+% foot progression angle.
+
+% Run the settings file.
+settingsFile
+
+% Allocate space for storing the data.
+dataStruct = struct();
+
+% Loop over the subject folders.
+for i = 3:length(subjectFolders)
+    subjDir = dir([subjectFolders(i).folder '\' subjectFolders(i).name]);
+    subjDirFlag = [subjDir.isdir];
+    trialFiles = subjDir(~subjDirFlag);
+
+    % Create structure for current subject.
+    dataStruct.(subjectFolders(i).name) = struct();
+    
+    dataStruct.(subjectFolders(i).name).calibration = getCalibration( ...
+        join([subjectFolders(i).folder '\' subjectFolders(i).name '\' calibrationFolder],''));
+
+    for j = 1:length(trialFiles)
+        currentPath = [trialFiles(j).folder '\' trialFiles(j).name];
+
+        trialName = ['trial_' extractBefore(trialFiles(j).name, '.csv')];
+
+        % Store the data in a useable format.
+        dataStruct.(subjectFolders(i).name).(trialName) = readDotCsv(currentPath);
+    end
+end
\ No newline at end of file

getCalibration.m

+%% This function reads in data from Movella DOT csv files of the calibration folder.
+% filePath - full path to where calibration files are stored.
+function dataStruct = getCalibration(filePath)
+    
+    % Only select the files that are relevant.
+    calibDir = dir(filePath);
+    calibDirFlag = [calibDir.isdir];
+    calibFiles = calibDir(~calibDirFlag);
+
+    % Pre-allocate the data structure.
+    dataStruct = struct();
+
+    % Loop over all the files in the calibration folder, such that results
+    % could be compared.
+    for i = 1:length(calibFiles)
+
+        % Get the current file path and trial name.
+        currFilePath = [calibFiles(i).folder '\' calibFiles(i).name];
+        trialName = ['trial_' extractBefore(calibFiles(i).name, '.csv')];
+
+        % Load in the actual Movella Dot CSV file.
+        dataStruct.(trialName) = readDotCsv(currFilePath);
+
+        % Pre-allocate a field for the calibration information.
+        if i ==1
+            dataStruct.(trialName).calibration = struct();    
+        end
+
+        % Determine the calibration orientation.
+        [dataStruct.(trialName).calibration.Rcal, ...
+            dataStruct.(trialName).calibration.rmy, ...
+            dataStruct.(trialName).calibration.rmz, ...
+            dataStruct.(trialName).AccOffsetR] = ...
+        math.initOrien(dataStruct.(trialName));
+
+
+
+    end
+
+end
\ No newline at end of file

readDotCsv.m

+%% This function reads in data from Movella DOT csv files.
+
+function dataStruct = readDotCsv(filePath)
+
+    % Load in the actual Movella Dot CSV file.
+    currentData = readtable(filePath, 'Delimiter', ',');
+
+    % Pre-allocate a structure.
+    dataStruct = struct();
+
+    % Store the data in a useable format.
+    dataStruct.acc = table2array(currentData(:,{'Acc_X', 'Acc_Y', 'Acc_Z'}));
+    dataStruct.gyr = table2array(currentData(:,{'Gyr_X', 'Gyr_Y', 'Gyr_Z'}));
+    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)));
+
+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";
+
+% Specify where the calibration folder is, has to be placed inside the 
+% subject folder.
+calibrationFolder = "calibration";
+
+% Extract all folders in that directory to know where to loop over.
+allFiles = dir(fileFolder);
+dirFlags = [allFiles.isdir];
+
+% Create a structure containing all folders.
+subjectFolders = allFiles(dirFlags);
+
+% gravity constant is defined.
+gravity = 9.81;
\ No newline at end of file

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));
+%% 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));
-k=0; % just giving a random start value
+
+% 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
-     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)
+        contactVector(i-10)=0;
+    else
+        contactVector(i-10)=1;
     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(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
-        k(dx(i):dx(i+1))=k(dx(i)-1);
+        contactVector(dx(i):dx(i+1))=contactVector(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(contactVector));
+tStart=find(dx==1);
+tEnd=find(dx==-1);
 end