exp_SonEMS_1.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;
 
 
%% Sensor Data
datapath = getDataPath();
datapath = fullfile(datapath, "ETH_Data/SonE_36/RawData/ZM");
data = jumpReadData(fullfile(datapath, "13167000.BIN"));
dt = 1/data.mpu_rate;
 
% manually find start and stop movement phases
idx_tug_post = [999486, 1001570];
idx_walk = [605943, 731541];
 
%% Zero velocity detection
zv = jumpZeroVelocityDetection(data, "threshold_acc", 1, "threshold_gyr", 10, "windowSize", 20, "plot",false);
zv_tug_post = zv(idx_tug_post(1):idx_tug_post(2));
zv_walk = zv(idx_walk(1):idx_walk(2));
 
%% Filter TUG post
 
% instance of filter
ekf = EKF1;
ekf.sigma_w_p = ekf.sigma_w_p * 5; % as not many zv points are available, trust in accel measurements must be bigger.
 
% measurements
ekf.zv = zv_tug_post;
ekf.z_acc = data.tt.acc(idx_tug_post(1):idx_tug_post(2),:)';
ekf.z_vel = zeros(size(ekf.z_acc));
ekf.z_gyr = data.tt.gyr(idx_tug_post(1):idx_tug_post(2),:)' * pi/180; % convert to rad/s
ekf.z_pos = zeros(size(ekf.z_acc));
ekf.z_pos(1:2,:) = nan;
ekf.z_pos(1:2,end-100:end) = 0;
ekf.z_pos(1:2,1:100) = 0;
ekf.z_yaw = nan(1, size(ekf.z_acc,2));
 
 
% initial orientation
 
% init orientation
a_x = mean(data.tt.acc(idx_tug_post(1):idx_tug_post(1)+200,1));
a_y = mean(data.tt.acc(idx_tug_post(1):idx_tug_post(1)+200,2));
a_z = mean(data.tt.acc(idx_tug_post(1):idx_tug_post(1)+200,3));
 
% calculate initial orientation
roll = atan2(a_y, a_z);
pitch = atan2(-a_x, sqrt(a_y^2 + a_z^2));
ekf.q_0 = quaternion([0, pitch, roll], "eulerd", "ZYX", "frame").normalize.compact;
 
% Run filter
smoothed = ekf.run(plot=true);
 
%% compare TUG post
 
% load ground truth
load("C:\Users\moss\OneDrive - ZHAW\MT\Measurements\ETH_Data\SonE_36\Processed\Vicon\matfiles\TUG_Post.mat")
GT = VD.Marker.LTO3 / 1000;
%GT = GT(240:2941,:); % trim away the jump
 
% align trajectories
smoothed_cld = pointCloud(smoothed(8:10,:)');
tform = rigidtform3d([180,0,0], [0,0,0]);
smoothed_cld = pctransform(smoothed_cld, tform);
 
GT_cld = pointCloud(GT);
[~, GT_cld] = pcregistericp(GT_cld, smoothed_cld, "Metric","pointToPoint", "MaxIterations", 200, "Verbose",false);
 
% format
GT_cld.Color = [0,1,0];
GT_cld.Intensity = 10 * ones(GT_cld.Count,1);
smoothed_cld.Color = [1,0,0];
 
% display
figure
pcshow(GT_cld,"BackgroundColor","white");
hold on;
pcshow(smoothed_cld, "MarkerSize", 5);
hold off;
 
 
%% Filter walking
 
close all
 
% instance of filter
ekf = EKF1;
 
% initial gyroscope bias
ekf.b_w_0 = [mean(data.tt.gyr(zv_walk(1:4000),1));mean(data.tt.gyr(zv_walk(1:4000),2));mean(data.tt.gyr(zv_walk(1:4000),3))] * pi / 180;
 
 
ekf.P_0_b_w = 1e-6; % original 5e-3, trust the initial bias much
ekf.P_0_q = 1e-3;
ekf.var_gyr = 1e-4; % opriginal 12e-4, less trust in zero rotation
 
% tune measurement noise
%ekf.var_vel_ZUPT = 1e-1;
ekf.var_acc_xy = 0.5e-3; %original: 45e-3
ekf.var_acc_z = 1e-3; %original: 70e-3
ekf.var_pos_pseudo = 1e-3; % very low trust, only a slight nudging
ekf.var_b_w = 1e-7;
 
% tune process noise
ekf.sigma_w_p = ekf.sigma_w_p * 2; % as not many zv points are available, trust in accel measurements must be bigger.
 
% measurements
ekf.zv = zv_walk;
ekf.z_acc = data.tt.acc(idx_walk(1):idx_walk(2),:)';
ekf.z_vel = zeros(size(ekf.z_acc));
ekf.z_gyr = data.tt.gyr(idx_walk(1):idx_walk(2),:)' * pi/180; % convert to rad/s
ekf.z_pos = zeros(size(ekf.z_acc));
speed = 3.2/3.6; % m/s;
 
% pseudo position update
ekf.z_pos(1,92:end) = cumsum(speed * dt * ones(size(ekf.z_pos(1,92:end))));
%ekf.z_pos(1:2,:) = nan;
 
% pseudo yaw update
ekf.z_yaw = deg2rad(10) * ones(1, size(ekf.z_acc,2));
%ekf.z_yaw = nan(1, size(ekf.z_acc,2));
 
 
% initial orientation
 
% mean accelerations during stance
a_x = mean(data.tt.acc(zv_walk,1));
a_y = mean(data.tt.acc(zv_walk,2));
a_z = mean(data.tt.acc(zv_walk,3));
 
% calculate initial orientation
roll = atan2(a_y, a_z);
pitch = atan2(-a_x, sqrt(a_y^2 + a_z^2));
yaw = deg2rad(130);
ekf.q_0 = quaternion([yaw, pitch, roll], "euler", "ZYX", "frame").normalize.compact;
 
% Run filter
smoothed = ekf.run(plot=true);
%% compare walking
 
% load ground truth
load("C:\Users\moss\OneDrive - ZHAW\MT\Measurements\ETH_Data\SonE_36\Processed\Vicon\matfiles\WN35.mat")
GT = VD.Marker.LTO3 / 1000;
 
v = diff(GT(:,2))/0.005;
figure
plot(v);
 
GT(:,2) = GT(:,2) - cumsum(speed * dt * ones(1,height(GT)))';
 
plot(-GT(:,2));
hold on
plot(smoothed(8,:));
hold off
%% show the Problem to Yong
figure
plot(data.tt.t, vecnorm(data.tt.acc,2,2))
title("SonEMS 36 - TUG Pre (partially)")
grid on
ylabel("||a||_2 [m/s^2]", Interpreter="tex")
ylim([8,12])
xlim([datetime("01.11.2022 10:25:38"), datetime("01.11.2022 10:25:43")])
pause;
title("SonEMS 36 - Treadmill (partially)")
xlim([datetime("01.11.2022 10:47:00"), datetime("01.11.2022 10:47:05")])
 
test = vecnorm(data.tt.acc,2,2);
TUG_test = test(254555:256260);
WALK_test= test(649824:651544);