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)
% Pre-allocate structures for rotated signals.
if index == 1
data.rG = nan(data.N,3);
data.vG = nan(data.N,3);
......@@ -10,7 +11,7 @@ end
rt=eye(3);
% 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.
gF = (data.Rcal*data.dgyr(indexes,:)');
......
......@@ -6,10 +6,34 @@
% output:
% 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.
steps = 1:(length(dataStruct.tMiddle)-1);
steps = 1:(length(data.tMiddle)-1);
% Loop over all steps.
for iStep = steps
......
......@@ -9,19 +9,22 @@
function dataStruct = correctSteps(dataStruct)
% Correct for wrong first step.
if dataStruct.tEnd(1) < dataStruct.tStart(1)
dataStruct.tEnd(1) = [];
end
% End with a full step.
if dataStruct.tEnd(end) < dataStruct.tStart(end)
dataStruct.tStart(end) = [];
if ~isempty(dataStruct.tStart)
% Correct for wrong first step.
if dataStruct.tEnd(1) < dataStruct.tStart(1)
dataStruct.tEnd(1) = [];
end
% End with a full step.
if dataStruct.tEnd(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
% Get the middle of contact phases for later calculations.
dataStruct.tMiddle = floor((dataStruct.tStart(2:end) + ...
dataStruct.tEnd(1:end-1))/2);
end
\ No newline at end of file
......@@ -13,37 +13,34 @@
% tEnd - end time of static parts.
% contactVector - boolean vector containing indicators of contact.
function outStruct = initOrien(data)
% Pre-allocate structure
outStruct = struct();
function data = initOrien(data)
% cut-off frequency of the filter.
fCutoff = 2;
data.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));
outStruct.filtAcc = filtfilt(Bf, Af, data.acc);
outStruct.filtGyr = filtfilt(Bf, Af, data.gyr);
[Bf, Af] = butter(2, (2 * data.fCutoff / data.fs));
data.filtAcc = filtfilt(Bf, Af, data.acc);
data.filtGyr = filtfilt(Bf, Af, data.gyr);
%% 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.
outStruct = math.correctSteps(outStruct);
data = math.correctSteps(data);
% Find the Static and Dynamic parts of the calibration trial.
staticPart = floor(0.2*outStruct.tStart):floor(0.8*outStruct.tStart);
steps = find(diff(outStruct.contactVector));
if outStruct.contactVector(1) == 1
staticPart = floor(0.2*data.tStart):floor(0.8*data.tStart);
steps = find(diff(data.contactVector));
if data.contactVector(1) == 1
steps(1) = [];
end
dynamicPart = steps(1):steps(6);
% Determine bias and subtract it.
outStruct.gyroBias = mean(data.gyr(staticPart,:));
gyroNoBias = data.gyr - outStruct.gyroBias;
data.gyroBias = mean(data.gyr(staticPart,:));
gyroNoBias = data.gyr - data.gyroBias;
% Obtain the perpendicular walking direction principal component.
yAxis = pca((gyroNoBias(dynamicPart,:)-mean(gyroNoBias(dynamicPart,:))));
......@@ -60,16 +57,16 @@ 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.
outStruct.rmz = sqrt((tzt-zaxis).^2);
data.rmz = sqrt((tzt-zaxis).^2);
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.
outStruct.rotMatrix = inv([xas',normYaxis,tzt']);
data.rotMatrix = inv([xas',normYaxis,tzt']);
% Determine acc offset (gravity)
accOffset = mean(data.acc(staticPart,:));
% Rotate acc offset to foot frame.
outStruct.accOffsetR = (outStruct.rotMatrix*accOffset')';
data.accOffsetR = (data.rotMatrix*accOffset')';
end
\ No newline at end of file
......@@ -8,19 +8,19 @@
% tEnd - indexes of toe-off.
% 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.
mGyr=sqrt(sum(gyr.^2,2));
mAcc=sqrt(sum(acc.^2,2));
mGyr=sqrt(sum(data.filtGyr.^2,2));
mAcc=sqrt(sum(data.filtAcc.^2,2));
% Initialize N, k and v.
N = length(mGyr);
dynamicsVector = ones(length(mGyr),1);
data.N = length(mGyr);
data.contactVector = ones(length(mGyr),1);
varianceWindow = zeros(length(mGyr),1);
% 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.
mAccWindow=mAcc(i-10:i-1);
avgmAccWindow=sum(mAccWindow)/10;
......@@ -33,25 +33,25 @@ for i=11:N
% 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;
data.contactVector(i-10)=0;
end
end
% 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);
x=diff(data.contactVector);
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))/fs <= 0.1
dynamicsVector(dx(i):dx(i+1))=dynamicsVector(dx(i)-1);
if (dx(i+1)-dx(i))/data.fs <= 0.1
data.contactVector(dx(i):dx(i+1))=data.contactVector(dx(i)-1);
end
end
% Recalculate the moments of contact to get indices.
dx=(diff(dynamicsVector));
tStart=find(dx==1);
tEnd=find(dx==-1);
dx=(diff(data.contactVector));
data.tStart=find(dx==1);
data.tEnd=find(dx==-1);
end
......@@ -29,7 +29,7 @@ for i = 3:length(subjectFolders)
% calculate all values for each step.
dataStruct.(subjectFolders(i).name).(trialName) = ...
math.calculateSteps(dataStruct.(subjectFolders(i).name).(trialName));
math.calculateSteps(dataStruct.(subjectFolders(i).name), trialName);
end
end
\ No newline at end of file
......@@ -27,8 +27,7 @@ function dataStruct = getCalibration(filePath)
end
% Determine the calibration orientation.
dataStruct.(trialName).calibration = ...
math.initOrien(dataStruct.(trialName));
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