exp_SonEMS_2.m

Go to the documentation of this file.

%
% For LICENSE / TERMS OF USE, see the file LICENSE in the root directory of the repository.
 
clear; close all; clc;
 
%% Data Variables
SonE = 80;
speed = 2.8 / 3.6; % m/s
trial = "WN44"; % PN38, 65; WN35, 38, 44, 50, 56, 65
%> @TODO introduce an automatic speed detection
%> @TODO make everything in SI units
 
%% Import Data Data
 
% path definitions
datapath = "D:\ETH_Data\SonE_" + SonE + "\";
vicon_path = fullfile(datapath, "Processed/Vicon/");
zm_raw_path = fullfile(datapath, "RawData/ZM/*.BIN");
summary_path = fullfile(datapath,"Result/GaitSummaryIMU_SonE_" + SonE + ".mat");
 
% load files
% vicon data position data
load(fullfile(vicon_path,"GaitSummary_YK","SonE_" + SonE + ".mat"));
vicon = GaitSummary.("SonE_" + SonE).(trial);
 
% zurich move, data ankle L
zm_raw_files = dir(zm_raw_path);
for i = 1:numel(zm_raw_files)
    this_header = jumpReadData(fullfile(zm_raw_files(i).folder, zm_raw_files(i).name), header_only=true);
    
    if strcmp(this_header.name, "ankle_L")
        zm_l = jumpReadData(fullfile(zm_raw_files(i).folder, zm_raw_files(i).name));
    elseif strcmp(this_header.name, "ankle_R")
        zm_r = jumpReadData(fullfile(zm_raw_files(i).folder, zm_raw_files(i).name));
    end
    
end
 
 
% gait summary
load(summary_path)
sum_zm_l = GaitSummaryIMU.ModulesData.ankle_L;
sum_zm_r = GaitSummaryIMU.ModulesData.ankle_R;
 
% delete unused variables
clearvars -except vicon sum_zm_l sum_zm_r zm_l zm_r speed trial SonE
 
%% create VICON ground truth from markers
% there are 4 markers on each foot:
% - heel (Calcaneus - RHEE, LHEE)
% - Toe 1 (Caput metatarsale I - RTO1, LTO1)
% - Toe 3 (Caput metatarsale III - RTO3, LTO3)
% - Toe 5 (Caput metatarsale V - RTO5, LTO5)
%
% the JumpSensor was placed on the metatarsal bones on the proximal side of the
% toe markers. The exact Position was not recorded. Therefore, we will use the
% mean position of the toe markers and transform this position with a fixed
% offset towards the heel marker. An offset of 5cm is used.
%
% note: the vicon data is in mm
 
% mean toe left
vicon.KinematicData.Marker.LTO = (vicon.KinematicData.Marker.LTO1 + vicon.KinematicData.Marker.LTO3 + vicon.KinematicData.Marker.LTO5) / 3;
% mean toe right
vicon.KinematicData.Marker.RTO = (vicon.KinematicData.Marker.RTO1 + vicon.KinematicData.Marker.RTO3 + vicon.KinematicData.Marker.RTO5) / 3;
 
% offset from toe to heel
offset = 50; % 5cm
 
% left foot
left_vec = vicon.KinematicData.Marker.LHEE - vicon.KinematicData.Marker.LTO;
left_vec = left_vec ./ vecnorm(left_vec,2,2);
vicon.KinematicData.Marker.LF = vicon.KinematicData.Marker.LTO + offset * left_vec;
 
% right foot
right_vec = vicon.KinematicData.Marker.RHEE - vicon.KinematicData.Marker.RTO;
right_vec = right_vec ./ vecnorm(right_vec,2,2);
vicon.KinematicData.Marker.RF = vicon.KinematicData.Marker.RTO + offset * right_vec;
 
% accumulate the distance based on the speed
 
% find the axis in walking direction
%> @TODO: introduce a detection of the walking axis and the axis direction. for
%> now, we assume that the negative y-axis is the walking direction
 
dist = cumsum(speed * 0.005 * ones(height(vicon.KinematicData.Marker.LF),1)) * 1000; %distance in millimeters
vicon.KinematicData.Marker.LF(:,2) = vicon.KinematicData.Marker.LF(:,2) - dist;
vicon.KinematicData.Marker.RF(:,2) = vicon.KinematicData.Marker.RF(:,2) - dist;
 
% delete unused variables
clearvars -except vicon sum_zm_l sum_zm_r zm_l zm_r trial speed SonE
 
%% find IMU data start and end
% in sum_zm_l.synced.trim(8).acc and sum_zm_r.synced.trim(8).acc the already
% trimmed and processed data of the IMU is stored. Since we do not want to use
% preprocessed data, we sync this preprocessed data to the raw data in zm_l and
% zm_r. This can be done with alignsignals.
 
% find the row where the trial is
trial_row = find(string({sum_zm_l.synced.trim.Trials}) == trial);
 
% left
[~, ~, idx_start_l_x] = alignsignals(sum_zm_l.synced.trim(trial_row).acc(:,1), zm_l.tt.acc(:,1), Method="xcorr");
[~, ~, idx_start_l_y] = alignsignals(sum_zm_l.synced.trim(trial_row).acc(:,2), zm_l.tt.acc(:,2), Method="xcorr");
[~, ~, idx_start_l_z] = alignsignals(sum_zm_l.synced.trim(trial_row).acc(:,3), zm_l.tt.acc(:,3), Method="xcorr");
 
% check if all left indices are the same
assert(abs(idx_start_l_x-idx_start_l_y) <= 1 && abs(idx_start_l_x-idx_start_l_z) <= 1, "Indices are not the same")
idx_start_l = round(mean([idx_start_l_x, idx_start_l_y, idx_start_l_z])) + 1;
 
% right
[~, ~, idx_start_r_x] = alignsignals(sum_zm_r.synced.trim(trial_row).acc(:,1), zm_r.tt.acc(:,1), Method="xcorr");
[~, ~, idx_start_r_y] = alignsignals(sum_zm_r.synced.trim(trial_row).acc(:,2), zm_r.tt.acc(:,2), Method="xcorr");
[~, ~, idx_start_r_z] = alignsignals(sum_zm_r.synced.trim(trial_row).acc(:,3), zm_r.tt.acc(:,3), Method="xcorr");
 
% check if all right indices are the same
assert(abs(idx_start_r_x-idx_start_r_y) <= 1 && abs(idx_start_r_x-idx_start_r_z) <= 1, "Indices are not the same")
idx_start_r = round(mean([idx_start_r_x, idx_start_r_y, idx_start_r_z])) + 1;
 
% clip zm data
zm_l.tt = zm_l.tt(idx_start_l:idx_start_l+height(sum_zm_l.synced.trim(trial_row).acc)-1,:);
zm_l.t_start = zm_l.tt.t(1);
zm_r.tt = zm_r.tt(idx_start_r:idx_start_r+height(sum_zm_r.synced.trim(trial_row).acc)-1,:);
zm_r.t_start = zm_r.tt.t(1);
 
% check if the data is properly aligned
assert(corr(sum_zm_l.synced.trim(trial_row).acc(:,1), zm_l.tt.acc(:,1)) > 0.99, "The x-axis of the left IMU data is not properly aligned")
assert(corr(sum_zm_l.synced.trim(trial_row).acc(:,2), zm_l.tt.acc(:,2)) > 0.99, "The y-axis of the left IMU data is not properly aligned")
assert(corr(sum_zm_l.synced.trim(trial_row).acc(:,3), zm_l.tt.acc(:,3)) > 0.99, "The z-axis of the left IMU data is not properly aligned")
assert(corr(sum_zm_r.synced.trim(trial_row).acc(:,1), zm_r.tt.acc(:,1)) > 0.99, "The x-axis of the right IMU data is not properly aligned")
assert(corr(sum_zm_r.synced.trim(trial_row).acc(:,2), zm_r.tt.acc(:,2)) > 0.99, "The y-axis of the right IMU data is not properly aligned")
assert(corr(sum_zm_r.synced.trim(trial_row).acc(:,3), zm_r.tt.acc(:,3)) > 0.99, "The z-axis of the right IMU data is not properly aligned")
 
% delete unused variables
clearvars -except vicon zm_l zm_r speed trial SonE
 
%% check the alignment of the VICON and IMU data
% the data should be aligned in time. also the number of samples should be the
% same. here, first the number of samples is checked and then the periods of
% movement are compared.
 
% check number of samples
assert(abs(height(vicon.KinematicData.Marker.LHEE) - height(zm_l.tt.acc)) <= 1, "The number of samples of the left IMU data and the VICON data is not the same")
assert(abs(height(vicon.KinematicData.Marker.RHEE) - height(zm_r.tt.acc)) <= 1, "The number of samples of the right IMU data and the VICON data is not the same")
 
% check periods of movement
% here the following is done:
% VICON:
% - the speed of movement is calculated from the position data
% - the speed is filtered with a lowpass filter
% - take the norm of the speed
% - the periods of movement are detected with a threshold
%
% IMU:
% - the magnitude of the angular velocity is calculated
% - the magnitude is filtered with a lowpass filter
% - the norm of the angular velocity is taken
% - the periods of movement are detected with a threshold
 
% VICON
% speed
v_vicon_l = diff(vicon.KinematicData.Marker.LF) / 0.005; % mm/s
v_vicon_r = diff(vicon.KinematicData.Marker.RF) / 0.005; % mm/s
 
% lowpass filter
[b, a] = butter(2, 0.1);
v_vicon_l = filtfilt(b, a, v_vicon_l);
v_vicon_r = filtfilt(b, a, v_vicon_r);
 
% norm
v_vicon_l = vecnorm(v_vicon_l,2,2);
v_vicon_r = vecnorm(v_vicon_r,2,2);
 
% threshold
th_vicon = 60; % mm/s
vicon_periods_l = v_vicon_l > th_vicon;
vicon_periods_r = v_vicon_r > th_vicon;
 
% IMU
% angular velocity
w_l = zm_l.tt.gyr - mean(zm_l.tt.gyr);
w_r = zm_r.tt.gyr - mean(zm_r.tt.gyr);
 
% lowpass filter
[b, a] = butter(2, 0.1);
w_l = filtfilt(b, a, w_l);
w_r = filtfilt(b, a, w_r);
 
% norm
w_l = vecnorm(w_l,2,2);
w_r = vecnorm(w_r,2,2);
 
% threshold
th_imu = 65; % deg/s
imu_periods_l = w_l > th_imu;
imu_periods_r = w_r > th_imu;
 
% check if the periods align
l_min = min([height(vicon_periods_l), height(imu_periods_l)]);
assert(corr(vicon_periods_l(1:l_min), imu_periods_l(1:l_min)) > 0.75, "The periods of movement of the left IMU data and the VICON data do not align")
assert(corr(vicon_periods_r(1:l_min), imu_periods_r(1:l_min)) > 0.75, "The periods of movement of the right IMU data and the VICON data do not align")
 
% transform vicon data
vic_l = vicon.KinematicData.Marker.LF;
vic_r = vicon.KinematicData.Marker.RF;
 
% delete unused variables
clearvars -except vicon vic_l vic_r zm_l zm_r speed trial SonE
 
%% filter the data of the left foot
 
close all
 
 
% dt and zero velocity detection
dt = 0.005;
zv_l = jumpZeroVelocityDetection(zm_l, "threshold_acc", 1.2, "threshold_gyr", 15, "windowSize", 30, "plot", false);
zv_r = jumpZeroVelocityDetection(zm_r, "threshold_acc", 1.2, "threshold_gyr", 15, "windowSize", 30, "plot", false);
 
% start and end during stance
idx_l(1) = find(zv_l, 1, "first");
idx_l(2) = find(zv_l, 1, "last");
idx_r(1) = find(zv_r, 1, "first");
idx_r(2) = find(zv_r, 1, "last");
 
% instance of filter
ekf = EKF1;
 
% initial values
% initial gyroscope bias
w_zv_l = zm_l.tt.gyr(zv_l,:);
w_zv_r = zm_r.tt.gyr(zv_r,:);
ekf.b_w_0 = [mean(w_zv_l(1:20,1)), mean(w_zv_l(1:20,2)), mean(w_zv_l(1:20,3))] * pi/180;
 
% initial orientation
% mean accelerations during stance
a_x = mean(zm_l.tt.acc(38:104,1)); % DEBUG: mean(zm_l.tt.acc(zv_l,1));
a_y = mean(zm_l.tt.acc(38:104,2)); % DEBUG: mean(zm_l.tt.acc(zv_l,2));
a_z = mean(zm_l.tt.acc(38:104,3)); % DEBUG: mean(zm_l.tt.acc(zv_l,3));
% calculate initial orientation
roll = atan2(a_y, a_z);
pitch = atan2(-a_x, sqrt(a_y^2 + a_z^2));
yaw = 0; deg2rad(60);
rot_vec = [roll, pitch, yaw];
ekf.q_0 = quaternion(rot_vec, "rotvec").normalize.compact';
 
% initial covariances
ekf.P_0_q = 1e-1; % original 5e-4
ekf.P_0_v = 1e-4; % original 1e-7
ekf.P_0_p = 1e-5; % original 1e-7
ekf.P_0_b_w = 1e-3; % original 5e-3
 
% measurement noise
ekf.var_acc_xy = ekf.var_acc_xy / 10; %original: 45e-3
ekf.var_acc_z  = ekf.var_acc_z  / 10; %original: 70e-3
ekf.var_vel_ZUPT = 1e-5; %original: 1e-6
ekf.var_gyr = ekf.var_gyr * 2;
ekf.var_pos_x = 1e-4; % original 1e-6
ekf.var_pos_y = 1e-4; % original 1e-6
ekf.var_pos_z = 1e-5; % original 1e-6
ekf.var_yaw_pseudo = 1e-4; % original 1e-4
 
% process noise
ekf.sigma_w_p = ekf.sigma_w_p * 5; % as not many zv points are available, trust in accel measurements must be bigger.
ekf.var_a = 0.045; % original 0.045
ekf.var_b_w = 1e-8; % original 1e-9
 
% measurements
ekf.zv = zv_l(idx_l(1):idx_l(2));
ekf.z_acc = zm_l.tt.acc(idx_l(1):idx_l(2),:)';
ekf.z_vel = zeros(size(ekf.z_acc));
ekf.z_gyr = zm_l.tt.gyr(idx_l(1):idx_l(2),:)' * pi/180; % convert to rad/s
ekf.z_pos = zeros(size(ekf.z_acc));
 
% pseudo position update
ekf.z_pos(1,92:end) = cumsum(speed * dt * ones(size(ekf.z_pos(1,92:end))));
ekf.z_pos(1,:) = nan; %this line turns off x-position pseudo updates
% ekf.z_pos(2,:) = nan; %this line turns off y-position pseudo updates
 
% pseudo yaw update
ekf.z_yaw = deg2rad(4.5) * ones(1, size(ekf.z_acc,2));
%ekf.z_yaw = nan(1, size(ekf.z_acc,2));
 
% Run left filter
smoothed_l = ekf.run(plot=true);
 
% right filter
% initial values
% initial gyroscope bias
ekf.b_w_0 = [mean(w_zv_r(1:20,1)), mean(w_zv_r(1:20,2)), mean(w_zv_r(1:20,3))] * pi/180;
 
% initial orientation
% mean accelerations during stance
a_x = mean(zm_r.tt.acc(zv_r,1));
a_y = mean(zm_r.tt.acc(zv_r,2));
a_z = mean(zm_r.tt.acc(zv_r,3));
% calculate initial orientation
roll = atan2(a_y, a_z);
pitch = atan2(-a_x, sqrt(a_y^2 + a_z^2));
yaw = 0; deg2rad(-60);
rot_vec = [roll, pitch, yaw];
ekf.q_0 = quaternion(rot_vec, "rotvec").normalize.compact';
 
% measurements
ekf.zv = zv_r(idx_r(1):idx_r(2));
ekf.z_acc = zm_r.tt.acc(idx_r(1):idx_r(2),:)';
ekf.z_vel = zeros(size(ekf.z_acc));
ekf.z_gyr = zm_r.tt.gyr(idx_r(1):idx_r(2),:)' * pi/180; % convert to rad/s
ekf.z_pos = zeros(size(ekf.z_acc));
 
% run right filter
%smoothed_r = ekf.run(plot=true);
 
% extract trajectories
traj_l = smoothed_l(8:10,:)';
% traj_r = smoothed_r(8:10,:)';
 
% add start and end (match original length)
traj_l = [nan(idx_l(1)-1,3); traj_l; nan(height(zm_l.tt)-idx_l(2),3)];
% traj_r = [nan(idx_r(1)-1,3); traj_r; nan(height(zm_r.tt)-idx_r(2),3)];
 
%% compare results
 
% gait events
TO_left = vicon.GaitEvents.TOleftlocs;
HS_left = vicon.GaitEvents.HSleftlocs;
 
% the amount of gait events might differ. the first TO must be before the first
% HS and the last TO must be before the last HS. the rest is cut off.
% while(TO_left(1) > HS_left(1))
%     HS_left(1) = [];
% end
% while(TO_left(end) > HS_left(end))
%     TO_left(end) = [];
% end
 
% stride length vicon left
stride_length_vicon = nan(length(HS_left),1);
stride_width_vicon = nan(length(HS_left),1);
for i = 1:numel(stride_length_vicon)-1
    stride_length_vicon(i) = norm(vic_l(HS_left(i),2) - vic_l(HS_left(i+1),2)) / 1000; % only y axis
    %stride_width_vicon(i) = -(vic_l(HS_left(i),1) - vic_l(TO_left(i),1)) / 1000; % only x axis
end
 
% stride length IMU left
stride_length_imu_l = nan(length(HS_left),1);
stride_width_imu_l = nan(length(HS_left),1);
for i = 1:numel(stride_length_imu_l)-1
    stride_length_imu_l(i) = norm(traj_l(HS_left(i),1) - traj_l(HS_left(i+1),1)); % only x-axis
    %stride_width_imu_l(i) = (traj_l(HS_left(i),2) - traj_l(TO_left(i),2)); % only x-axis
end
 
% plot of stride length
figure("Units","normalized","OuterPosition",[0,0,0.5,0.5])
subplt(1) = subplot(2,2,1:2);
plot(stride_length_vicon, '.-', 'DisplayName', 'VICON', "LineWidth", 0.5, "MarkerSize", 20)
hold on
plot(stride_length_imu_l, '.-', 'DisplayName', 'Jump Sensor', "LineWidth", 0.5, "MarkerSize", 20)
legend("Location","best")
title(sprintf("Stride Length (SonEMS %d, %s)", SonE, trial))
xlabel("Stride")
ylabel("Stride Length [m]")
grid on
 
% error over time
subplt(2) = subplot(223);
error = (stride_length_vicon - stride_length_imu_l)*100; % in cm
mean_squared_error = rmse(stride_length_vicon, stride_length_imu_l)*100;
stem(error, "LineWidth",1)
title("Error, mean squared error: " + mean_squared_error + " cm")
xlabel("Stride")
ylabel("Error [cm]")
grid on
 
% error distribution
subplt(3) = subplot(224);
histogram(error, 20)
title("Error Distribution")
xlabel("Error [cm]")
ylabel("Frequency")
grid on
 
% % plot of stride width
% figure("Units","normalized","OuterPosition",[0.5,0,0.5,0.5])
% subplt(1) = subplot(2,2,1:2);
% plot(stride_width_vicon*100, '.-', 'DisplayName', 'VICON', "LineWidth", 0.5, "MarkerSize", 20)
% hold on
% plot(stride_width_imu_l*100, '.-', 'DisplayName', 'Jump Sensor', "LineWidth", 0.5, "MarkerSize", 20)
% legend("Location","best")
% title(sprintf("Stride Width (SonEMS %d, %s)", SonE, trial))
% xlabel("Stride")
% ylabel("Stride Width [cm]")
% grid on
% 
% % error over time
% subplt(2) = subplot(223);
% error = (stride_width_vicon - stride_width_imu_l)*100; % in cm
% mean_squared_error = rmse(stride_width_vicon, stride_width_imu_l)*100;
% stem(error, "LineWidth",1)
% title("Error, RMSE: " + mean_squared_error + " cm")
% xlabel("Stride")
% ylabel("Error [cm]")
% grid on
% 
% % error distribution
% subplt(3) = subplot(224);
% histogram(error, 20)
% title("Error Distribution")
% xlabel("Error [cm]")
% ylabel("Frequency")
% grid on