derivation9Ekf6D.m

In this example, a 6D orientation and position estimation is performed.

In this example, a 6D orientation and position estimation is performed.

Note

a more detailed explanation can be found here: Part 9: 6D EKF-RTS

Details

% =========================================================================== %
%> @example derivation9Ekf6D.m
%>
%> In this example, a 6D orientation and position estimation is performed.
%>
%> @note a more detailed explanation can be found here: @ref derivation9Ekf6D
%>
%> <details>
% =========================================================================== %
clear; close all; clc;
 
%% Sensor Data
datapath = getDataPath();
datapath = fullfile(datapath, "self_captured/240429_walking_line_forward_backward");
data = jumpReadData(fullfile(datapath, "walking_line.BIN"));
dt = 1/data.mpu_rate;
 
% manually find start and stop of the three movement phases (see desc.txt in data folder)
idx_cal = [1, 10827];
idx_walking = [18179, 22580];
idx_initial = [18179, 18774] - idx_walking(1) + 1;
idx_at_turn = [20036, 20848] - idx_walking(1) + 1;
idx_end = [22205, 22582] - idx_walking(1) + 1;
 
%% Zero velocity detection
zv_cal = jumpZeroVelocityDetection(data);
zv_cal(idx_cal(2)+1:end) = 0;
zv_walk = jumpZeroVelocityDetection(data, "threshold_acc", 0.5, "threshold_gyr", 6, "windowSize", 20, "plot",false);
zv_walk = zv_walk(idx_walking(1):idx_walking(2));
 
params = jumpEstimateCalibrationParameters(data, zv_cal);
bias_acc = params.acc.bias;
sens_acc = params.acc.sensitivity;
bias_gyr = params.gyr.bias;
 
% init pos
p_0 = [0,0,0];
 
% init orientation
a_x = mean(data.tt.acc(idx_initial(1):idx_initial(2),1)); % DEBUG: - bias_acc(1);
a_y = mean(data.tt.acc(idx_initial(1):idx_initial(2),2)); % DEBUG: - bias_acc(2);
a_z = mean(data.tt.acc(idx_initial(1):idx_initial(2),3)); % DEBUG: - bias_acc(3);
 
% calculate initial orientation
roll = atan2(a_y, a_z);
pitch = atan2(-a_x, sqrt(a_y^2 + a_z^2));
q_0 = quaternion([0, pitch, roll], "eulerd", "ZYX", "frame").normalize.compact;
 
%% measurement vector
z = zeros(12, idx_walking(2) - idx_walking(1) + 1);
z(1:3,:) = data.tt.acc(idx_walking(1):idx_walking(2),:)'; % accelerometer measurements
% rows 4,5,6 are zero (transitional velocity during stance)
z(7:9,:) = data.tt.gyr(idx_walking(1):idx_walking(2),:)' * pi/180; % gyro measurements in radians/s
% x and y position in rows 10 and 11 are zero (other cases handled in the loop)
z(12,:) = 0; % z-position is zero during stance
 
%% filter Equations
% number of observations
N = size(z,2);
 
% number of states
% 4 q, 3 v, 3 p, 3 b_a, 3 b_w, 3 s_a
n_x = 19;
 
% state vector
x_fm = nan(n_x, N); % fm: forward, minus
x_fp = nan(n_x, N); % fp: forward, plus
x_bp = nan(n_x, N); % bp: backward, plus
x_0 =  zeros(n_x, 1);
 
% initial values
x_0(1:4 ) = q_0'; % initial orientation
x_0(8:10) = p_0; % initial position
x_0(11:13) = 0; % DEBUG: params.acc.bias; % initial acceleration bias
x_0(14:16) = 0; % DEBUG: params.gyr.bias * pi / 180; % initial gyro bias
x_0(17:19) = 1; % DEBUG:params.acc.sensitivity; % initial sensitivity
 
% covariance matrix
P_fm = nan(n_x, n_x, N);
P_fp = nan(n_x, n_x, N);
P_bp = nan(n_x, n_x, N);
 
% initial covariance matrix
p_0_q = 5e-4; % initial orientation std
p_0_v = 1e-7; % initial velocity std
p_0_p = 1e-7; % initial position std
p_0_b_a = 5e-3; % initial accel bias std
p_0_b_w = 5e-3; % initial gyro bias std
p_0_s = 0.3e-4; % initial sensitivity std
P_0 = diag([p_0_q * ones(1,4), p_0_v * ones(1,3), p_0_p * ones(1,3), p_0_b_a * ones(1,3), p_0_b_w * ones(1,3), p_0_s * ones(1,3)]).^2;
 
% process noise
% defined in a function
 
% measurement noise covariance matrix
var_acc_xy = 0.045;
var_acc_z = 0.07;
var_vel_ZUPT = 1e-6;
var_gyr = 0.07 * pi / 180;
var_pos_zupt = 1e-6;
var_pos_pseudo = 1e-6;
R = diag([var_acc_xy * ones(1,2), var_acc_z, var_vel_ZUPT * ones(1,3), var_gyr * ones(1,3), var_pos_pseudo * ones(1,2), var_pos_zupt]);
 
%% iterate over all observations (filtering)
tic;
 
bad_iterations = false(1,N);
warning("off");
for k = 1:N
    if k == 1
        % initial prediction
        [x_fm(:, k), w] = f_fun(x_0, z(:, k), z(:, k), zv_walk(k), dt);
        F = F_jac(w, dt);
        P_fm(:, :, k) = F * P_0 * F' + Q_cov(x_0, dt);
    else
        % prediction
        [x_fm(:, k), w]= f_fun(x_fp(:, k-1), z(:, k-1), z(:, k), zv_walk(k), dt);
        F = F_jac(w,dt);
        P_fm(:, :, k) = F * P_fp(:, :, k-1) * F' + Q_cov(x_fp(:, k-1), dt);
    end
    
    % normalize quaternion
    x_fm(1:4, k) = quaternion(x_fm(1:4, k)').normalize.compact';
    
    h = h_fun(x_fm(:, k));
    H = H_jac(x_fm(:, k));
    
    % handle measurements
    if ~zv_walk(k)
        % there are no updates during movement
        % TODO: the computing intensive part can be skipped completely instead of setting the H-Matrix to zero
        H(:) = 0;
    else
        % handly x,y measurements
        % initial position
        if k >= idx_initial(1) && k <= idx_initial(2)
            % leave the measurements at zero
            H(10:11,:) = 0; % leave off for now
        elseif k >= idx_at_turn(1) && k <= idx_at_turn(2)
            % set the measurements of x and y
            z(10:11,k) = [-9,-0];
            H(10:11,:) = 0; % leave off for now
        elseif k >= idx_end(1) && k <= idx_end(2)
            %leave the measurements at zero
            H(10:11,:) = 0; % leave off for now
        else
            % otherwise disable x,y measurements
            H(10:11,:) = 0; % no pseudo position measurement
        end
    end
    
    % kalman gain
    K = P_fm(:, :, k) * H' * pinv(H * P_fm(:, :, k) * H' + R);
    
    % update
    x_fp(:, k) = x_fm(:, k) + K * (z(:, k) - h);
    P_fp(:, :, k) = (eye(n_x) - K * H) * P_fm(:, :, k) * (eye(n_x) - K * H)' + K * R * K';
    
    % normalize quaternion
    x_fp(1:4, k) = quaternion(x_fp(1:4, k)').normalize.compact';
    
    % check if a warning occured
    [~, warnID] = lastwarn;
    if strcmp(warnID, 'MATLAB:nearlySingularMatrix') || strcmp(warnID, 'MATLAB:singularMatrix')
        bad_iterations(k) = true;
        lastwarn("",""); % reset warning
    end
end
warning("on");
 
%% smoothing
if 1
    % iterate over observations
    for k = N-1:-1:1
        if k == N-1
            % initialization
            x_bp(:, N) = x_fp(:, N);
            P_bp(:, :, N) = P_fp(:, :, N);
        end
        
        F = F_jac(x_fp(:, k), dt);
        
        I = pinv(P_fm(:, :, k+1));
        K = P_fp(:, :, k) * F' * I;
        x_bp(:,k) = x_fp(:,k) + K * (x_bp(:,k+1) - x_fm(:,k+1));
        P_bp(:,:,k) = P_fp(:,:,k) - K * (P_fm(:,:,k+1) - P_bp(:,:,k+1)) * K';
        
        if any(isnan(x_bp(:,k))) | any(isnan(P_bp(:,:,k)))
            bad_iterations(k) = true;
        end
        
    end
end
 
toc;
 
%% display results
displayResults(x_fp, x_bp, zv_walk, idx_at_turn, monitor=3);
 
%% Helper functions
 
% this function calculates the state transition function from a given state vector x
% this is also called state extrapolation. Based on the a posteriori state x at
% time k, the state at time k+1 is estimated.
function [x_next, w] = f_fun(x_this, z_this, z_next, zv, dt)
 
% new state vector
x_next = nan(size(x_this));
 
% new quaternion
epsilon = 1e-12;
q_this = x_this(1:4);
gyro_this = z_this(7:9);
gyro_next = z_next(7:9);
gyro_bias = x_this(14:16);
w = getPreprocessedGyro(gyro_this, gyro_next, gyro_bias, zv);
w_norm = norm(w,2);
w_matrix = [0, -w(1) -w(2) -w(3); ...
    w(1) 0 w(3) -w(2); ...
    w(2) -w(3) 0 w(1); ...
    w(3) w(2) -w(1) 0];
 
q_next = (cos(w_norm * dt / 2) * eye(4) + 1/(w_norm + epsilon) * sin(w_norm * dt / 2) * w_matrix) * q_this;
x_next(1:4) = q_next;
 
% new acceleration
acc_this = z_this(1:3);
acc_next = z_next(1:3);
acc_bias = x_this(11:13);
acc_sens = x_this(17:19);
acc = getPreprocessedAcc(acc_this, acc_next, acc_bias, acc_sens, q_this, q_next, zv);
 
% new velocity
x_next(5:7) = x_this(5:7) + acc * dt;
 
% new position
x_next(8:10) = x_this(8:10) + x_this(5:7) * dt + 0.5 * acc * dt^2;
 
% new biases
x_next(11:16) = x_this(11:16);
 
% new sensitivity
x_next(17:19) = x_this(17:19);
 
end
 
% this function calculates the preprocessed gyro measurement as explained in the
% derivation. The gyro measurements are preprocessed to get the bias free mean
% angular velocity between two samples.
function w = getPreprocessedGyro(gyro_this, gyro_next, bias, zv)
 
% if zero velocity is detected, the gyro measurements are set to zero
if false % zv DEBUG
    w = [0;0;0];
else
    w = (gyro_this + gyro_next) / 2 - bias;
end
 
end
 
% this function calculates the preprocessed acceleration measurement as explained in the
% derivation. The acceleration measurements are preprocessed to get the bias free mean
% acceleration without gravity between two samples.
function acc = getPreprocessedAcc(acc_this, acc_next, bias, sens, q_this, q_next, zv)
 
% if zero velocity is detected, the acceleration measurements are set to zero
if zv
    acc = [0;0;0];
else
    % rotate acceleration measurements to global frame
    acc_this = quaternion(q_this').rotmat("frame")' * (acc_this - bias) ./ sens;
    acc_next = quaternion(q_next').rotmat("frame")' * (acc_next - bias) ./ sens;
    
    % calculate mean acceleration
    acc = (acc_this + acc_next) / 2;
    
    % remove gravity
    acc = acc - [0;0;-9.81];
    
end
 
end
 
 
% this function calculates the jacobian of the state transition function
% from a given state vector x
function F = F_jac(w, dt)
 
% F1 ( quaternion and angular velocity )
epsilon = 1e-12;
w_norm = norm(w,2);
alpha = cos(w_norm * dt / 2) * w_norm;
beta = sin(w_norm * dt / 2);
 
F_1_1 = 1/(w_norm + epsilon) * [alpha, -w(1) * beta, -w(2) * beta, -w(3) * beta; ...
    w(1) * beta, alpha, w(3) * beta, -w(2) * beta; ...
    w(2) * beta, -w(3) * beta, alpha, w(1) * beta; ...
    w(3) * beta, w(2) * beta, -w(1) * beta, alpha];
 
F_1 = [F_1_1, zeros(4,15)];
 
% F2 (position, velocity, acceleration)
 
F_2_1 = eye(6);
F_2_1(4:6,1:3) = dt * eye(3);
 
F_2 = [zeros(6,4), F_2_1, zeros(6,9)];
 
% F3 (biases and sensitivity)
F_3 = zeros(9,19);
F_3(:,11:19) = eye(9);
 
F = [F_1; F_2; F_3];
 
end
 
% this function calculates the measurement function from a given state vector x
function h=h_fun(x)
 
g = [0;0;-9.81]; % gravity vector in world frame
 
% acceleration
s_a = x(17:19); % sensitivity
b_a = x(11:13); % acceleration biases
R = quaternion(x(1:4)').rotmat("frame");
 
h1 = s_a .* R * g + b_a;
 
% ZUPT
v_g = x(5:7); % velocity in global frame
b_w = x(14:16); % gyro biases
h2 = [v_g; b_w];
 
% position pseudo measurement
h3 = x(8:10); % position in global frame
 
h = [h1; h2; h3];
 
end
 
% this function calculates the jacobian of the measurement function from a
% given state vector x
function H = H_jac(x)
 
g = 9.81;
 
% H1 (acceleration)
q0 = x(1); q1 = x(2); q2 = x(3); q3 = x(4);
s_a = x(17:19); % sensitivity
 
H_1_1 = s_a .* (2 * g * [ q2, -q3,  q0, -q1; ...
    -q1, -q0, -q3, -q2; ...
    -q0,  q1,  q2, -q3]);
 
H_1_2 = diag([2*(q0*q2 - q1*q3), -2*(q0*q1 + q2*q3), -q0^2 + q1^2 + q2^2 - q3^2]);
 
H_1 = [H_1_1, zeros(3,6), eye(3), zeros(3,3), H_1_2];
 
% H2 (ZUPT)
H_2 = [zeros(3,4), eye(3), zeros(3,12); ...
    zeros(3,13), eye(3), zeros(3,3)];
 
% H3 (position pseudo measurement)
H_3 = [zeros(3,7), eye(3), zeros(3,9)];
 
H = [H_1; H_2; H_3];
 
end
 
% this function calculates the process noise covariance matrix
function Q = Q_cov(x, dt)
 
%> \todo change process noise based on zero velocity detection
 
sigma_w_p = sqrt(0.071) * pi / 180 / 0.5; % angular velocity noise
var_a = 0.045; % acceleration noise
var_b_a = 1e-7; % acceleration bias noise
var_b_w = 1e-9; % gyro bias noise (rad/s): experimental value
var_s_a = 1e-12; % sensitivity noise
 
 
% quaternion
q0 = x(1); q1 = x(2); q2 = x(3); q3 = x(4);
Q_q = (dt * sigma_w_p / 2)^2 * [...
    1 - q0^2, -q0 * q1, -q0 * q2, -q0 * q3; ...
    -q0 * q1, 1 - q1^2, -q1 * q2, -q1 * q3; ...
    -q0 * q2, -q1 * q2, 1 - q2^2, -q2 * q3; ...
    -q0 * q3, -q1 * q3, -q2 * q3, 1 - q3^2];
 
% acceleration, velocity and position
Q_a = zeros(6);
Q_a(1:3, 1:3) = dt^2 * eye(3);
Q_a(4:6, 4:6) = dt^4 / 4 * eye(3);
Q_a(4:6, 1:3) = dt * eye(3);
Q_a(4:6, 1:3) = dt^3 / 2 * eye(3);
Q_a = (triu(Q_a.',1) + tril(Q_a)) * var_a;
 
% biases
Q_b_a = eye(3) * var_b_a;
Q_b_w = eye(3) * var_b_w;
 
% sensitivity
Q_s = eye(3) * var_s_a;
 
% combine
Q = blkdiag(Q_q, Q_a, Q_b_a, Q_b_w, Q_s);
 
end
 
% this function displays the sesults of the filtering and smoothing process
function displayResults(x_fp, x_bp, zv, idx_at_turn, options)
 
    arguments
        x_fp (19,:)
        x_bp (19,:)
        zv (1,:)
        idx_at_turn (1,2)
        options.monitor = 1
    end
 
% filter results
ori_filt = quaternion(x_fp(1:4,:)').eulerd("ZYX", "frame");
pos_filt = x_fp(8:10,:)';
 
% smoothing results
ori_smoo = quaternion(x_bp(1:4,:)').eulerd("ZYX", "frame");
pos_smoo = x_bp(8:10,:)';
 
disps = get(0, 'MonitorPositions');
f = figure;
f.Position = disps(options.monitor,:);
f.WindowState = 'maximized';
 
total_length_filt = mean(vecnorm(pos_filt(idx_at_turn(1):idx_at_turn(2),:),2,2));
total_length_smoo = mean(vecnorm(pos_smoo(idx_at_turn(1):idx_at_turn(2),:),2,2));
 
% x,y plot
pl(1) = subplot(3,3,1);
plot(pos_filt(:,1), pos_filt(:,2), 'DisplayName', 'Filter');
hold on;
plot(pos_smoo(:,1), pos_smoo(:,2), 'DisplayName', 'Smoother', 'LineWidth',2);
hold off;
grid on;
xlabel("x [m]");
ylabel("y [m]");
%legend("Location", "best");
title(sprintf("Total length: Filter: %.2f m", total_length_filt));
 
% x,z plot
pl(2) = subplot(3,3,2);
plot(pos_filt(:,1), pos_filt(:,3), 'DisplayName', 'Filter');
hold on;
plot(pos_smoo(:,1), pos_smoo(:,3), 'DisplayName', 'Smoother', 'LineWidth',2);
hold off;
grid on;
xlabel("x [m]");
ylabel("z [m]");
%legend("Location", "best");
title(sprintf("Total length: Smoother: %.2f m", total_length_smoo));
 
% y,z plot
pl(3) = subplot(3,3,3);
plot(pos_filt(:,2), pos_filt(:,3), 'DisplayName', 'Filter');
hold on;
plot(pos_smoo(:,2), pos_smoo(:,3), 'DisplayName', 'Smoother', 'LineWidth',2);
hold off;
grid on;
xlabel("y [m]");
ylabel("z [m]");
%legend("Location", "best");
 
% bias accel plot
pl(4) = subplot(3,3,4);
yyaxis left;
plot(x_fp(11:13,:)', 'DisplayName', 'Filter');
ylabel("Bias [m/s^2]");
yyaxis right;
plot(x_bp(11:13,:)', 'DisplayName', 'Smoother', 'LineWidth',2);
ylabel("Bias [m/s^2]");
grid on;
xlabel("Sample");
%legend("Location", "best");
title("Bias Acceleration");
 
% bias gyro plot
pl(5) = subplot(3,3,5);
yyaxis left;
plot(x_fp(14:16,:)', 'DisplayName', 'Filter');
ylabel("Bias [rad/s]");
yyaxis right;
plot(x_bp(14:16,:)', 'DisplayName', 'Smoother', 'LineWidth',2);
ylabel("Bias [rad/s]");
grid on;
xlabel("Sample");
%legend("Location", "best");
title("Bias Gyroscope");
 
% sensitivity plot
pl(6) = subplot(3,3,6);
yyaxis left;
plot(x_fp(17:19,:)', 'DisplayName', 'Filter');
ylabel("Sensitivity");
yyaxis right;
plot(x_bp(17:19,:)', 'DisplayName', 'Smoother', 'LineWidth',2);
ylabel("Sensitivity");
grid on;
xlabel("Sample");
%legend("Location", "best");
title("Sensitivity Acceleration");
 
% orientation plot
pl(7) = subplot(3,3,7);
plot(ori_filt, 'DisplayName', 'Filter');
hold on;
plot(ori_smoo, 'DisplayName', 'Smoother', 'LineWidth',2);
hold off;
grid on;
xlabel("Sample");
ylabel("Angle [°]");
%legend("Location", "best");
title("Orientation");
 
% velocity plot
pl(8) = subplot(3,3,8);
plot(x_fp(5:7,:)', 'DisplayName', 'Filter');
hold on;
plot(x_bp(5:7,:)', 'DisplayName', 'Smoother', 'LineWidth',2);
hold off;
grid on;
xlabel("Sample");
ylabel("Velocity [m/s]");
%legend("Location", "best");
title("Velocity");
 
% zero velocity detection plot
pl(9) = subplot(3,3,9);
plot(zv, '.-', 'DisplayName', 'Zero Velocity Detection', 'MarkerSize', 10);
ylim([-0.1, 1.1]);
grid on;
xlabel("Sample");
ylabel("Zero Velocity Detection");
%legend("Location", "best");
title("Zero Velocity Detection");
 
% link axes
linkaxes(pl([1,2]), 'x');
linkaxes(pl([2,3]), 'y');
linkaxes(pl(4:9), 'x');
 
end