Skip to content
Snippets Groups Projects
Commit a9be8de5 authored by Wouda, Frank (UT-EEMCS)'s avatar Wouda, Frank (UT-EEMCS)
Browse files

Renamed variables to make thngs clearer. TODO: inSignals should be updated.

parent efdebb26
No related branches found
No related tags found
No related merge requests found
function data = intSignals(data,index) function data = intSignals(data,index)
% Pre-allocate structures for rotated signals.
if index == 1 if index == 1
data.rG = nan(data.N,3); data.rG = nan(data.N,3);
data.vG = nan(data.N,3); data.vG = nan(data.N,3);
...@@ -10,7 +11,7 @@ end ...@@ -10,7 +11,7 @@ end
rt=eye(3); rt=eye(3);
% Current part of analysis. % Current part of analysis.
indexes = data.tm(index):data.tm(index+1); indexes = data.tMiddle(index):data.tMiddle(index+1);
% Rotate the gyroscope data to Foot-frame. % Rotate the gyroscope data to Foot-frame.
gF = (data.Rcal*data.dgyr(indexes,:)'); gF = (data.Rcal*data.dgyr(indexes,:)');
......
...@@ -6,10 +6,34 @@ ...@@ -6,10 +6,34 @@
% output: % output:
% dataStruct - containing calculated FPA. % dataStruct - containing calculated FPA.
function dataStruct = calculateSteps(dataStruct) function dataStruct = calculateSteps(dataStruct, trialName)
% Get the trial data.
data = dataStruct.(trialName);
% Get all calibration files.
calibrationFields = fieldnames(dataStruct.calibration);
% By default take the first calibration trial.
calibration = dataStruct.calibration.(calibrationFields{1});
% 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));
data.filtAcc = filtfilt(Bf, Af, data.acc);
data.filtGyr = filtfilt(Bf, Af, data.gyr);
% Step detection using the zero velocity update.
data = math.zvt(data);
% Correct the steps.
data = math.correctSteps(data);
% Number of steps is 1 smaller as it should end with foot contact. % Number of steps is 1 smaller as it should end with foot contact.
steps = 1:(length(dataStruct.tMiddle)-1); steps = 1:(length(data.tMiddle)-1);
% Loop over all steps. % Loop over all steps.
for iStep = steps for iStep = steps
......
...@@ -9,19 +9,22 @@ ...@@ -9,19 +9,22 @@
function dataStruct = correctSteps(dataStruct) function dataStruct = correctSteps(dataStruct)
if ~isempty(dataStruct.tStart)
% Correct for wrong first step. % Correct for wrong first step.
if dataStruct.tEnd(1) < dataStruct.tStart(1) if dataStruct.tEnd(1) < dataStruct.tStart(1)
dataStruct.tEnd(1) = []; dataStruct.tEnd(1) = [];
end end
% End with a full step. % End with a full step.
if dataStruct.tEnd(end) < dataStruct.tStart(end) if dataStruct.tEnd(end) < dataStruct.tStart(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);
else
disp("NO steps detected.")
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 end
\ No newline at end of file
...@@ -13,37 +13,34 @@ ...@@ -13,37 +13,34 @@
% tEnd - end time of static parts. % tEnd - end time of static parts.
% contactVector - boolean vector containing indicators of contact. % contactVector - boolean vector containing indicators of contact.
function outStruct = initOrien(data) function data = initOrien(data)
% Pre-allocate structure
outStruct = struct();
% cut-off frequency of the filter. % cut-off frequency of the filter.
fCutoff = 2; data.fCutoff = 2;
% 2nd order low-pass butterworth filter to get the relevant motion data. % 2nd order low-pass butterworth filter to get the relevant motion data.
% Implement as filtfilt due to no realtime constraints. % Implement as filtfilt due to no realtime constraints.
[Bf, Af] = butter(2, (2 * fCutoff / data.fs)); [Bf, Af] = butter(2, (2 * data.fCutoff / data.fs));
outStruct.filtAcc = filtfilt(Bf, Af, data.acc); data.filtAcc = filtfilt(Bf, Af, data.acc);
outStruct.filtGyr = filtfilt(Bf, Af, data.gyr); data.filtGyr = filtfilt(Bf, Af, data.gyr);
%% Step detection using the zero velocity update. %% Step detection using the zero velocity update.
[outStruct.tStart, outStruct.tEnd, outStruct.contactVector] = math.zvt(outStruct.filtAcc,outStruct.filtGyr, data.fs); data = math.zvt(data);
% Correct the calculated steps. % Correct the calculated steps.
outStruct = math.correctSteps(outStruct); data = math.correctSteps(data);
% Find the Static and Dynamic parts of the calibration trial. % Find the Static and Dynamic parts of the calibration trial.
staticPart = floor(0.2*outStruct.tStart):floor(0.8*outStruct.tStart); staticPart = floor(0.2*data.tStart):floor(0.8*data.tStart);
steps = find(diff(outStruct.contactVector)); steps = find(diff(data.contactVector));
if outStruct.contactVector(1) == 1 if data.contactVector(1) == 1
steps(1) = []; steps(1) = [];
end end
dynamicPart = steps(1):steps(6); dynamicPart = steps(1):steps(6);
% Determine bias and subtract it. % Determine bias and subtract it.
outStruct.gyroBias = mean(data.gyr(staticPart,:)); data.gyroBias = mean(data.gyr(staticPart,:));
gyroNoBias = data.gyr - outStruct.gyroBias; gyroNoBias = data.gyr - data.gyroBias;
% Obtain the perpendicular walking direction principal component. % Obtain the perpendicular walking direction principal component.
yAxis = pca((gyroNoBias(dynamicPart,:)-mean(gyroNoBias(dynamicPart,:)))); yAxis = pca((gyroNoBias(dynamicPart,:)-mean(gyroNoBias(dynamicPart,:))));
...@@ -60,16 +57,16 @@ xas = cross(normYaxis',zaxis)/norm(cross(normYaxis',zaxis)); ...@@ -60,16 +57,16 @@ xas = cross(normYaxis',zaxis)/norm(cross(normYaxis',zaxis));
tzt = cross(xas,normYaxis)/norm(cross(xas,normYaxis)); tzt = cross(xas,normYaxis)/norm(cross(xas,normYaxis));
% Calculate the errors of the axes with respect to being perpendicular. % Calculate the errors of the axes with respect to being perpendicular.
outStruct.rmz = sqrt((tzt-zaxis).^2); data.rmz = sqrt((tzt-zaxis).^2);
txt = cross(zaxis,xas)/norm(cross(zaxis,xas)); txt = cross(zaxis,xas)/norm(cross(zaxis,xas));
outStruct.rmy = sqrt((txt-normYaxis').^2); data.rmy = sqrt((txt-normYaxis').^2);
% the rotation matrix is obtained by adding the three directions together. % the rotation matrix is obtained by adding the three directions together.
outStruct.rotMatrix = inv([xas',normYaxis,tzt']); data.rotMatrix = inv([xas',normYaxis,tzt']);
% Determine acc offset (gravity) % Determine acc offset (gravity)
accOffset = mean(data.acc(staticPart,:)); accOffset = mean(data.acc(staticPart,:));
% Rotate acc offset to foot frame. % Rotate acc offset to foot frame.
outStruct.accOffsetR = (outStruct.rotMatrix*accOffset')'; data.accOffsetR = (data.rotMatrix*accOffset')';
end end
\ No newline at end of file
...@@ -8,19 +8,19 @@ ...@@ -8,19 +8,19 @@
% tEnd - indexes of toe-off. % tEnd - indexes of toe-off.
% contactVector - vector with indicators of contact. % contactVector - vector with indicators of contact.
function [tStart, tEnd, dynamicsVector] = zvt(acc,gyr,fs) function data = zvt(data)
% Obtain the magnitudes of acceleration and angular velocity. % Obtain the magnitudes of acceleration and angular velocity.
mGyr=sqrt(sum(gyr.^2,2)); mGyr=sqrt(sum(data.filtGyr.^2,2));
mAcc=sqrt(sum(acc.^2,2)); mAcc=sqrt(sum(data.filtAcc.^2,2));
% Initialize N, k and v. % Initialize N, k and v.
N = length(mGyr); data.N = length(mGyr);
dynamicsVector = ones(length(mGyr),1); data.contactVector = ones(length(mGyr),1);
varianceWindow = zeros(length(mGyr),1); varianceWindow = zeros(length(mGyr),1);
% loop over all samples starting at 11 for the first window. % loop over all samples starting at 11 for the first window.
for i=11:N for i=11:data.N
% Take a snapshot of the acceleration data and average it. % Take a snapshot of the acceleration data and average it.
mAccWindow=mAcc(i-10:i-1); mAccWindow=mAcc(i-10:i-1);
avgmAccWindow=sum(mAccWindow)/10; avgmAccWindow=sum(mAccWindow)/10;
...@@ -33,25 +33,25 @@ for i=11:N ...@@ -33,25 +33,25 @@ for i=11:N
% stationary. For detailed explanation of the conditions see the % stationary. For detailed explanation of the conditions see the
% publication: https://doi.org/10.1186/s12984-021-00816-4 % 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) if varianceWindow(i-10)<0.5 && mGyr(i-10)<50 && (9<mAcc(i-10) && mAcc(i-10)<11)
dynamicsVector(i-10)=0; data.contactVector(i-10)=0;
end end
end end
% Differentiate the contact vector to find indices of the contacts and % Differentiate the contact vector to find indices of the contacts and
% double check that the duration of contact is sufficient (10 samples). % double check that the duration of contact is sufficient (10 samples).
x=diff(dynamicsVector); x=diff(data.contactVector);
dx=find(abs(x)==1); dx=find(abs(x)==1);
% Correct the contact windows shorter than 100 ms to non-contact. % Correct the contact windows shorter than 100 ms to non-contact.
for i=1:length(dx)-1 for i=1:length(dx)-1
if (dx(i+1)-dx(i))/fs <= 0.1 if (dx(i+1)-dx(i))/data.fs <= 0.1
dynamicsVector(dx(i):dx(i+1))=dynamicsVector(dx(i)-1); data.contactVector(dx(i):dx(i+1))=data.contactVector(dx(i)-1);
end end
end end
% Recalculate the moments of contact to get indices. % Recalculate the moments of contact to get indices.
dx=(diff(dynamicsVector)); dx=(diff(data.contactVector));
tStart=find(dx==1); data.tStart=find(dx==1);
tEnd=find(dx==-1); data.tEnd=find(dx==-1);
end end
...@@ -29,7 +29,7 @@ for i = 3:length(subjectFolders) ...@@ -29,7 +29,7 @@ for i = 3:length(subjectFolders)
% calculate all values for each step. % calculate all values for each step.
dataStruct.(subjectFolders(i).name).(trialName) = ... dataStruct.(subjectFolders(i).name).(trialName) = ...
math.calculateSteps(dataStruct.(subjectFolders(i).name).(trialName)); math.calculateSteps(dataStruct.(subjectFolders(i).name), trialName);
end end
end end
\ No newline at end of file
...@@ -27,8 +27,7 @@ function dataStruct = getCalibration(filePath) ...@@ -27,8 +27,7 @@ function dataStruct = getCalibration(filePath)
end end
% Determine the calibration orientation. % Determine the calibration orientation.
dataStruct.(trialName).calibration = ... dataStruct.(trialName) = math.initOrien(dataStruct.(trialName));
math.initOrien(dataStruct.(trialName));
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment