derivation8Ekf6D_pt3.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: Extended Test (3)

Details

% =========================================================================== %
%> @example derivation8Ekf6D_pt3.m
%>
%> In this example, a 6D orientation and position estimation is performed.
%>
%> @note a more detailed explanation can be found here: @ref derivation8ExtendedTest
%>
%> <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_9m = [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", 10);
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 = mean(data.tt.gyr(zv_walk,:),1);
 
% init pos
p_0 = [0,0,0];
 
% init orientation
a_x = mean(data.tt.acc(idx_initial(1):idx_initial(2),1)) - bias_acc(1);
a_y = mean(data.tt.acc(idx_initial(1):idx_initial(2),2)) - bias_acc(2);
a_z = mean(data.tt.acc(idx_initial(1):idx_initial(2),3)) - 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");
 
%% measurement vector
z = zeros(15, idx_walking(2) - idx_walking(1) + 1);
z(1:3,:) = data.tt.acc(idx_walking(1):idx_walking(2),:)';
z(4:6,:) = data.tt.gyr(idx_walking(1):idx_walking(2),:)' * pi/180; %convert to radians/s
% transitional velocity in rows 7,8,9 is zero
% angular velocity in rows 10,11,12 is zero
% position in rows 13,14,15 is zero
 
%% filter Equations
% number of observations
N = size(z,2);
 
% number of states
% 4 q, 3 a, 3 v, 3 w, 3 b_a, 3 b_w, 3 s_a
n_x = 25;
 
% 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) = 1; % initial quaternion
x_0(11:13) = p_0; % initial position
x_0(17:19) = params.acc.bias; % initial acceleration bias
x_0(23:25) = 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
alpha = 1e-5;
beta = 0.6 / 100;
gamma = 0.5 / 100;
zeta = 5 * pi / 180 / 2;
kappa = 0.03 / 100;
P_0 = diag([repmat(alpha,1,16), repmat(beta,1,2), gamma, repmat(zeta,1,3), repmat(kappa,1,3)]) .^ 2;
 
% process noise
% defined in a function
 
% measurement noise
std_acc_xy = 0.045;
std_acc_z = 0.07;
std_gyr = 0.07 * pi / 180;
std_pseudo_acc = 1e-9;
std_pseudo_w = 1e-9;
R = diag([std_acc_xy, std_acc_xy, std_acc_z, std_gyr, std_gyr, std_gyr, repmat(std_pseudo_acc,1,3), repmat(std_pseudo_w,1,3), 0.01, 0.01, 0.001]) .^ 2;
 
%% iterate over all observations (filtering)
bad_iterations = false(1,N);
warning("off");
for k = 1:N
    if k == 1
        % initial prediction
        F = F_jac(x_0, dt);
        x_fm(:, k) = f_fun(x_0,dt);
        P_fm(:, :, k) = F * P_0 * F' + Q_cov(x_0, dt, zv_walk(k));
    else
        % prediction
        F = F_jac(x_fp(:, k-1),dt);
        x_fm(:, k) = f_fun(x_fp(:, k-1),dt);
        P_fm(:, :, k) = F * P_fp(:, :, k-1) * F' + Q_cov(x_fp(:, k-1), dt, zv_walk(k));
    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)
        % no zero velocity detected
        H(7:15, :) = 0;
    end
    
   % handly x,y measurements
   % initial position
   if k >= idx_initial(1) && k <= idx_initial(2)
        % leave the measurements at zero
   elseif k >= idx_at_9m(1) && k <= idx_at_9m(2)
        % set the measurements of x and y
        z(13:14,k) = [-8,-4]; % 
   elseif k >= idx_end(1) && k <= idx_end(2)
        %leave the measurements at zero
    else
        % otherwise disable x,y measurements
        H(13:14,:) = 0; % no pseudo position measurement
    end
 
    % check observability
    % O = obsv(F,H);
    % if rank(O) < n_x
    %     warning("System not observable at time step %d", k);
    %     bad_iterations(k) = true;
    % end
 
    % kalman gain
    K = P_fm(:, :, k) * H' * pinv(H * P_fm(:, :, k) * H' + R);
 
    % % alternative Kalman gain (nudging biases towards zero)
    % lambda = 1e-5;
    % lambda_matrix = zeros(size(R)); lambda_matrix(7:12,7:12) = lambda * eye(6);
    % K = P_fm(:, :, k) * H' * pinv(H * P_fm(:, :, k) * H' + R + lambda_matrix);
 
    % 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
 
%% prepare results
% filter results
ori_filt = quaternion(x_fp(1:4,:)');
pos_filt = x_fp(11:13,:)';
 
% smoothing results
ori_smoo = quaternion(x_bp(1:4,:)');
pos_smoo = x_bp(11:13,:)';
 
%% orientation animation
%animateQuat(ori_filt, ori_smoo, ...
%               "names", ["Filter", "Smoother"], "title", "Orientation and Position Estimation", ...
%               "position", [0,0.25,0.5,0.5]);
 
% position plot
figure("Units", "normalized", "OuterPosition", [0,0,0.6,0.8]);
 
total_length_filt = mean(vecnorm(pos_filt(idx_at_9m(1):idx_at_9m(2),:),2,2));
total_length_smoo = mean(vecnorm(pos_smoo(idx_at_9m(1):idx_at_9m(2),:),2,2));
 
% 3D plot
subplot(2,2,1);
plot3(pos_filt(:,1), pos_filt(:,2), pos_filt(:,3), 'DisplayName', 'Filter');
hold on;
plot3(pos_smoo(:,1), pos_smoo(:,2), pos_smoo(:,3), 'DisplayName', 'Smoother', 'LineWidth',2);
hold off;
grid on;
xlabel("x [m]");
ylabel("y [m]");
zlabel("z [m]");
legend("Location", "best");
ax = gca();
ax.View = [-10, 30];
ax.Projection = "perspective";
ax.DataAspectRatio = [1,1,1];
title(sprintf("Total length: Filter: %.2f m", total_length_filt));
 
% x,y plot
subplot(2,2,2);
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: Smoother: %.2f m", total_length_smoo));
 
% x,z plot
subplot(2,2,3);
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");
 
% y,z plot
subplot(2,2,4);
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");
 
%% Helper functions
% this function calculates the state transition function from a given state vector x
function x = f_fun(x, dt)
 
    % new state vector
    x_new = nan(size(x));
 
    % new quaternion
    epsilon = 1e-12;
    q_old = x(1:4);
    omega = x(14:16)';
    omega_norm = norm(omega,2);
    omega_matrix = [0, -omega(1) -omega(2) -omega(3); ...
                    omega(1) 0 omega(3) -omega(2); ...
                    omega(2) -omega(3) 0 omega(1); ...
                    omega(3) omega(2) -omega(1) 0];
 
    q_new = (cos(omega_norm * dt / 2) * eye(4) + 1/(omega_norm+epsilon) * sin(omega_norm * dt / 2) * omega_matrix) * q_old;
    x_new(1:4) = q_new;
 
 
    % new acceleration
    x_new(5:7) = x(5:7);
 
    % new velocity
    x_new(8:10) = x(8:10) + x(5:7) * dt;
 
    % new position
    x_new(11:13) = x(11:13) + x(8:10) * dt + 0.5 * x(5:7) * dt^2;
 
    % new angular velocity
    x_new(14:16) = x(14:16);
 
    % new biases
    x_new(17:22) = x(17:22);
 
    % new sensitivity
    x_new(23:25) = x(23:25);
 
    x = x_new;
 
end
 
 
% this function calculates the jacobian of the state transition function
% from a given state vector x
function F = F_jac(x, dt)
 
    % F1 ( quaternion and angular velocity )
    epsilon = 1e-12;
    q0 = x(1); q1 = x(2); q2 = x(3); q3 = x(4);
    wx = x(14); wy = x(15); wz = x(16);
    w_norm = norm([wx, wy, wz],2);
    alpha = cos(w_norm * dt / 2) * w_norm;
    beta = sin(w_norm * dt / 2);
 
    F_1_1 = 1/(w_norm+epsilon) * [alpha, -wx * beta, -wy * beta, -wz * beta; ...
             wx * beta, alpha, wz * beta, -wy * beta; ...
             wy * beta, -wz * beta, alpha, wx * beta; ...
             wz * beta, wy * beta, -wx * beta, alpha];
 
    F_1_2 = beta/(w_norm+epsilon) * [-q1, -q2, -q3; ...
                            q0, -q3,  q2; ...
                            q3,  q0, -q1; ...
                           -q2,  q1,  q0];
 
    F_1 = [F_1_1, zeros(4,9), F_1_2, zeros(4,9)];
 
    % F2 (position, velocity, acceleration)
 
    F_2_1 = eye(9);
    F_2_1(4:6,1:3) = dt * eye(3);
    F_2_1(7:9,4:6) = dt * eye(3);
    F_2_1(7:9,1:3) = dt^2;
 
    F_2 = [zeros(9,4), F_2_1, zeros(9,12)];
 
    % F3 (angular velocity)
    F_3 = zeros(3,numel(x));
    F_3(1:3,14:16) = eye(3);
 
    % F4 (biases)
    F_4 = zeros(9,numel(x));
    F_4(:,17:25) = eye(9);
 
    F = [F_1; F_2; F_3; F_4];    
    
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(23:25); % sensitivity
    a_g = x(5:7); % acceleration in global frame
    b_a = x(17:19); % acceleration biases
    R = quaternion(x(1:4)').rotmat("frame");
 
    h1 = s_a .* R * (a_g + g) + b_a;
 
    % angular velocity
    w_s = x(14:16); % angular velocity in sensor frame
    b_w = x(20:22); % angular velocity biases
 
    h2 = w_s + b_w;
 
    % ZUPT
    v_g = x(8:10); % velocity in global frame
    h3 = [v_g; w_s];
 
    % position pseudo measurement
    h4 = x(11:13); % position in global frame
 
    h = [h1; h2; h3; h4];
 
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(23:25); % sensitivity
    alpha = x(5) + g; % ax_g + g
    beta = x(6) + g; % ay_g + g
    gamma = x(7) + g; % az_g + g
 
    H_1_1 = s_a .* 2 .* [alpha * q0 + beta * q3 - gamma * q2, alpha * q1 + beta * q2 + gamma * q3, beta * q1 - alpha * q2 - gamma * q0, beta * q0 - alpha * q3 + gamma * q1; ...
                 beta * q0 - alpha * q3 + gamma * q1, alpha * q2 - beta * q1 + gamma * q0, alpha * q1 + beta * q2 + gamma * q3, gamma * q2 - beta * q3 - alpha * q0; ...
                 alpha * q2 - beta * q1 + gamma * q0, alpha * q3 - beta * q0 - gamma * q1, alpha * q0 + beta * q3 - gamma * q2, alpha * q1 + beta * q2 + gamma * q3];
    
    H_1_2 = s_a .* quaternion(x(1:4)').rotmat("frame");
 
    H_1_3 = diag([alpha * (q0^2 + q1^2 - q2^2 - q3^2) + 2 * beta * (q0*q3 + q1*q2) - 2 * gamma * (q0 * q2 - q1 * q3), ...
                 -2 * alpha * (q0*q3 - q1*q2) + beta * (q0^2 - q1^2 + q2^2 - q3^2) + 2 * gamma * (q0 * q1 + q2 * q3), ...
                 2 * alpha * (q0*q2 + q1*q3) - 2 * beta * (q0*q1 - q2*q3) + gamma * (q0^2 - q1^2 - q2^2 + q3^2)]);
 
    H_1 = [H_1_1, H_1_2, zeros(3,9), eye(3), zeros(3,3), H_1_3];
 
    % H2 (angular velocity)
    H_2 = [zeros(3,13), eye(3), zeros(3,3), eye(3), zeros(3,3)];    
 
    % H3 (ZUPT)
    H_3 = [zeros(3,7), eye(3), zeros(3,15); ...
           zeros(3,13), eye(3), zeros(3,9)];
 
    % H4 (position pseudo measurement)
    H_4 = [zeros(3,10), eye(3), zeros(3,12)];
 
    H = [H_1; H_2; H_3; H_4];
 
end
 
% this function calculates the process noise covariance matrix
function Q = Q_cov(x, dt, zv_walk)
 
    %> \todo change process noise based on zero velocity detection
 
    sigma_w_p = 0.2 * pi / 180;     % educated guess
    sigma_a_p = 0.5;              % educated guess
    sigma_b_a = 0.001;              % from allan variance
    sigma_b_w = 0.002 * pi / 180;   % from allan variance
    sigma_s_a = 0;               % educated guess
 
    % 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(9);
    Q_a(1:3, 1:3) = eye(3);
    Q_a(4:6, 4:6) = dt^2 * eye(3);
    Q_a(7:9, 7:9) = dt^4 / 4 * eye(3);
    Q_a(4:6, 1:3) = dt * eye(3);
    Q_a(7:9, 4:6) = dt^3 / 2 * eye(3);
    Q_a(7:9, 1:3) = dt^2 / 2 * eye(3);
    Q_a = (triu(Q_a.',1) + tril(Q_a)) * sigma_a_p^2;
 
    % angular velocity
    Q_w = eye(3) * sigma_w_p^2;
 
    % biases
    Q_b = diag([sigma_b_a^2, sigma_b_a^2, sigma_b_a^2, sigma_b_w^2, sigma_b_w^2, sigma_b_w^2]);
 
    % sensitivity
    Q_s = eye(3) * sigma_s_a^2;
 
    % combine
    Q = blkdiag(Q_q, Q_a, Q_w, Q_b, Q_s);
 
end
 
% this function calculates the difference between estimated and true
% orientations. the initial yaw is aligned first.
function [d, ori_est] = oriDiff(ori_est, ori_true)
 
    arguments
        ori_est (:,1) quaternion
        ori_true (:,1) quaternion
    end
 
    assert(length(ori_est) == length(ori_true), "Length of orientations must be equal");
 
    % initial reference yaw
    ref_yaw = ori_true(1).eulerd("ZYX", "frame");
    ref_yaw = ref_yaw(1);
 
    % Initial estimation yaw
    est_yaw = ori_est(1).eulerd("ZYX", "frame");
    est_yaw = est_yaw(1);
 
    % align initial yaw
    diff = ref_yaw - est_yaw;
    q_corr = quaternion([diff,0,0], "eulerd", "ZYX", "frame");
 
    % apply correction
    ori_est = q_corr .* ori_est;
 
    % calculate differences
    d = ori_est.conj .* ori_true;
 
    % get absolute differende
    d = 2*acos(d.normalize.parts) * 180 / pi;
    d = mod(d + 180, 360) - 180;
 
end