From 92a5027f8ab92dd761f90cf823682341f5c13b04 Mon Sep 17 00:00:00 2001
From: "AD\\WoudaFJ" <f.j.wouda@utwente.nl>
Date: Fri, 11 Aug 2023 15:32:24 +0200
Subject: [PATCH] Started renaming variables and deleting unnecessary
operations. to be tested with DOT data!
---
+math/initOrien.m | 64 ++++++++++++++++++++++++++++++++++++++++++
+math/zvt.m | 27 ++++++++++++++++++
InitOrien.m | 71 -----------------------------------------------
Main.m | 30 ++++++++++++++++++++
getCalibration.m | 40 ++++++++++++++++++++++++++
readDotCsv.m | 19 +++++++++++++
settingsFile.m | 18 ++++++++++++
zvt.m | 67 +++++++++++++++++++++++++++++++++-----------
8 files changed, 249 insertions(+), 87 deletions(-)
create mode 100644 +math/initOrien.m
create mode 100644 +math/zvt.m
delete mode 100644 InitOrien.m
create mode 100644 Main.m
create mode 100644 getCalibration.m
create mode 100644 readDotCsv.m
create mode 100644 settingsFile.m
diff --git a/+math/initOrien.m b/+math/initOrien.m
new file mode 100644
index 0000000..a8aff63
--- /dev/null
+++ b/+math/initOrien.m
@@ -0,0 +1,64 @@
+%% 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
diff --git a/+math/zvt.m b/+math/zvt.m
new file mode 100644
index 0000000..91b1791
--- /dev/null
+++ b/+math/zvt.m
@@ -0,0 +1,27 @@
+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
+
diff --git a/InitOrien.m b/InitOrien.m
deleted file mode 100644
index 4e266d6..0000000
--- a/InitOrien.m
+++ /dev/null
@@ -1,71 +0,0 @@
-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
diff --git a/Main.m b/Main.m
new file mode 100644
index 0000000..373aedd
--- /dev/null
+++ b/Main.m
@@ -0,0 +1,30 @@
+%% 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
diff --git a/getCalibration.m b/getCalibration.m
new file mode 100644
index 0000000..667abaf
--- /dev/null
+++ b/getCalibration.m
@@ -0,0 +1,40 @@
+%% 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
diff --git a/readDotCsv.m b/readDotCsv.m
new file mode 100644
index 0000000..4cb08ee
--- /dev/null
+++ b/readDotCsv.m
@@ -0,0 +1,19 @@
+%% 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
diff --git a/settingsFile.m b/settingsFile.m
new file mode 100644
index 0000000..346ba49
--- /dev/null
+++ b/settingsFile.m
@@ -0,0 +1,18 @@
+%% 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
diff --git a/zvt.m b/zvt.m
index 91b1791..822394b 100644
--- a/zvt.m
+++ b/zvt.m
@@ -1,27 +1,62 @@
-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
--
GitLab