derivation5Ekf6D.m

Go to the documentation of this file.

% =========================================================================== %
%> @example derivation5Ekf6D.m
%>
%> In this example, a 6D orientation estimation filter is implemented and
%> compared to the 6D VQF filter.
%>
%> Using the flag USE_SENSITIVITY, the sensitivity states can be included in
%> the filter. The sensitivity states are used to estimate the sensor
%> sensitivities.
%>
%> @note a more detailed explanation can be found here: @ref derivation5Ekf6D
%>
%> <details>
% =========================================================================== %
clear; close all; clc;
 
% set seed for reproducibility
rng(1);
 
% flag to use the sensitivity states
global USE_SENSITIVITY
USE_SENSITIVITY = false;
 
%% Sensor Data Generation
% load predefined trajectory
load('rotatingCube.mat');
 
% set all positions to zero
tab = traj.getTab;
tab.pos(:,:) = 0;
traj.setTab(tab);
traj.generateTrajectory;
 
js = JumpSensor;
 
data = js.generateFromTrajectory(traj);
 
% define time step
global dt
dt = 1/data.mpu_rate;
 
% get actual bias and sensitivity
bias_acc = js.getParams.mpu.acc.ConstantBias;
sensitivity_acc = [js.getParams.mpu.acc.AxesMisalignment(1,1),js.getParams.mpu.acc.AxesMisalignment(2,2),js.getParams.mpu.acc.AxesMisalignment(3,3)] / 100;
bias_gyr = js.getParams.mpu.gyr.ConstantBias;
sensitivity_gyr = [js.getParams.mpu.gyr.AxesMisalignment(1,1),js.getParams.mpu.acc.AxesMisalignment(2,2),js.getParams.mpu.acc.AxesMisalignment(3,3)] / 100;
 
%% Zero velocity detection
std_acc = max(abs(movstd(data.tt.acc, 100)),[],2);
std_gyr = max(abs(movstd(data.tt.gyr, 100)),[],2);
 
zero_velocity = std_acc < 0.15 & std_gyr < 0.15;
 
%% measurement vector
z = zeros(9, size(data.tt.acc,1));
z(1:3,:) = data.tt.acc';
z(4:6,:) = data.tt.gyr' * pi/180; %convert to radians
% angular velocity in rows 7,8,9 is zero
 
%% filter Equations
% number of observations
N = size(z,2);
 
global n_x
if USE_SENSITIVITY
    n_x = 19; % 13 states + 6 sensitivity states
else
    n_x = 13; % 13 states
end
 
% state vector
% q0, q1, q2, q3, gyr_x, gyr_y, gyr_z, bias_gyr_x, bias_gyr_y, bias_gyr_z, ...
% bias_acc_x, bias_acc_y, bias_acc_z
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
if USE_SENSITIVITY
    x_0 =  [1; zeros(12,1); ones(6,1)];
else
    x_0 =  [1; zeros(12,1)];
end
 
% 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);
if USE_SENSITIVITY
    P_0 = diag([repmat(1e-3,1,4),repmat(1e-2,1,6),0.6,0.6,0.8, repmat(3e-2,1,6)]).^2;
else
    P_0 = diag([repmat(1e-3,1,4),repmat(1e-2,1,6),0.6,0.6,0.8]).^2;
end
 
% process noise
Q = diag([repmat(1e-5,1,4), repmat(1e-3,1,3), 1e-12 * ones(1,n_x-7)]) .^ 2;
 
% measurement noise
std_acc_xy = 0.045;
std_acc_z = 0.07;
std_gyr = 0.1 * pi / 180;
std_pseudo = 1e-9;
R = diag([std_acc_xy, std_acc_xy, std_acc_z, std_gyr, std_gyr, std_gyr, std_pseudo, std_pseudo, std_pseudo]) .^ 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);
        x_fm(:, k) = f_fun(x_0);
        P_fm(:, :, k) = F * P_0 * F' + Q;
    else
        % prediction
        F = f_jac(x_fp(:, k-1));
        x_fm(:, k) = f_fun(x_fp(:, k-1));
        P_fm(:, :, k) = F * P_fp(:, :, k-1) * F' + Q;
    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 ~zero_velocity(k)
        % no zero velocity detected
        H(7:9, :) = 0;
    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));
 
        I = P_fm(:, :, k+1)^-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';
 
    end
end
 
%% try the VQF filter
try
    vqf = MyVqf();
    vqf_offline = vqf.filterOffline(data.tt.acc, data.tt.gyr, data.tt.mag);
    vqf_offline = quaternion(vqf_offline.quat6D);
    vqf_offline = quaternion(0, sqrt(2)/2, sqrt(2)/2, 0).normalize .* vqf_offline; % ENU -> NED
    vqf_filter = vqf.filter(data.tt.acc, data.tt.gyr, data.tt.mag);
    vqf_filter = quaternion(vqf_filter.quat6D);
    vqf_filter = quaternion(0, sqrt(2)/2, sqrt(2)/2, 0).normalize .* vqf_filter; % ENU -> NED
catch ME
    warning("VQF Filtering failed. Python package 'vqf' installed?")
    vqf_offline = quaternion.ones(1,N);
end
 
%% calculate orientation differences
% filter results
[d_ori_filt, ori_filt] = oriDiff(quaternion(x_fp(1:4,:)'), traj.getTrajectory.ori);
 
% smoothing results
[d_ori_smoo, ori_smoo] = oriDiff(quaternion(x_bp(1:4,:)'), traj.getTrajectory.ori);
 
% VQF results
[d_ori_vqf_off, vqf_offline] = oriDiff(vqf_offline, traj.getTrajectory.ori);
[d_ori_vqf_fil, vqf_filter] = oriDiff(vqf_filter, traj.getTrajectory.ori);
 
fprintf("Mean absolute orientation errors:" + newline + newline + "Filtered: %.2f°" + ...
    newline + "Smoothed: %.2f°" + newline + "VQF offline: %.2f°" + newline + "VQF filtered: %.2f°" + newline, ...
    mean(abs(d_ori_filt)), mean(abs(d_ori_smoo)), mean(abs(d_ori_vqf_off)), mean(abs(d_ori_vqf_fil)));
 
%% plot orientation differences
figure("Units","normalized",Position=[0.2,0.3,0.6,0.4])
plot(traj.getTrajectory.t, abs([d_ori_filt, d_ori_smoo, d_ori_vqf_fil, d_ori_vqf_off]),'.-','LineWidth',0.5);
grid on;
legend("filtered","smoothed", "VQF filtered", "VQF offline");
ylabel("Orientation absolute error: ||dq|| [°]")
xlabel("Time");
title("Comparison of 6D orientation estimation filter and smoother versus 9D VQF")
 
%% compare results
animateQuat(ori_filt,ori_smoo,traj.getTrajectory.ori, vqf_filter, vqf_offline, ...
            "position", [0.1,0.25,0.9,0.5], "speedup",1, "names", ["filtered","smoothed","ground truth", "vqf filtered", "vqf offline"])
 
 
%% Helper functions
% this function calculates the state transition function from a given state vector x
function x = f_fun(x)
    global dt n_x
 
    % quaternion
    x1 = [2/dt, -x(5), -x(6), -x(7); ...
          x(5),  2/dt,  x(7), -x(6); ...
          x(6), -x(7),  2/dt,  x(5); ...
          x(7),  x(6), -x(5),  2/dt] * dt/2 * x(1:4);
 
    % angular speed
    x2 = x(5:7);
 
    % biases
    x3 = x(8:n_x);
 
    x = [x1; x2; x3];
end
 
 
% this function calculates the jacobian of the state transition function
% from a given state vector x
function F = f_jac(x)
 
    global dt n_x
    
    F1 = [2/dt, -x(5), -x(6), -x(7), -x(2), -x(3), -x(4); ...
          x(5),  2/dt,  x(7), -x(6),  x(1), -x(4),  x(3); ...
          x(6), -x(7),  2/dt,  x(5),  x(4),  x(1), -x(2); ...
          x(7),  x(6), -x(5),  2/dt, -x(3),  x(2),  x(1)] * dt/2;
 
    F_2_6 = diag(ones(n_x-4,1));
 
    F = [[F1, zeros(4,n_x-7)]; [zeros(n_x-4,4), F_2_6]];
end
 
% this function calculates the measurement function from a given state vector x
function h=h_fun(x)
 
    global USE_SENSITIVITY
 
    g = 9.81;
 
    if USE_SENSITIVITY
 
        % accelerometer
        h1 = [x(17) * -g * 2 * (x(2)*x(4) - x(1)*x(3)) + x(11); ...
              x(18) * -g * 2 * (x(3)*x(4) + x(1)*x(2)) + x(12); ...
              x(19) * -g * (x(1)^2 - x(2)^2 - x(3)^2 + x(4)^2) + x(13)];
 
        % gyroscope
        h2 = [x(14) * x(5) + x(8); ...
              x(15) * x(6) + x(9); ...
              x(16) * x(7) + x(10)];
 
    else
 
        % accelerometer
        h1 = [-g * 2 * (x(2)*x(4) - x(1)*x(3)) + x(11); ...
              -g * 2 * (x(3)*x(4) + x(1)*x(2)) + x(12); ...
              -g * (x(1)^2 - x(2)^2 - x(3)^2 + x(4)^2) + x(13)];
 
        % gyroscope
        h2 = [x(5) + x(8); ...
              x(6) + x(9); ...
              x(7) + x(10)];
 
    end
 
    % pseudo measurement
    h3 = [x(5); x(6); x(7)];
 
    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)
 
    global USE_SENSITIVITY n_x
 
    g = 9.81;
 
    if USE_SENSITIVITY
        % H1
        H_1_1 = [ x(3)*x(17), -x(4)*x(17),  x(1)*x(17),  -x(2)*x(17); ...
                 -x(2)*x(18), -x(1)*x(18), -x(4)*x(18),  -x(3)*x(18); ...
                 -x(1)*x(19),  x(2)*x(19),  x(3)*x(19),  -x(4)*x(19)] * 2 * g;
 
        H_1_2 = g * [2  * (x(1) * x(3) - x(2) * x(4)), 0, 0 ; ...
                    0, -2  * (x(1) * x(2) + x(3) * x(4)), 0; ...
                    0, 0, -x(1)^2 + x(2)^2 + x(3)^2 + x(4)^2];
 
        H_1 = [H_1_1, zeros(3,6), diag([1,1,1]), zeros(3,3), H_1_2];
 
        % H2
        H_2_1 = [x(14), 0, 0; ...
                 0, x(15), 0; ...
                 0, 0, x(16)];
 
        H_2_2 = [x(5), 0, 0; ...
                0, x(6), 0; ...
                0, 0, x(7)];
 
        H_2 = [zeros(3,4), H_2_1, diag([1,1,1]), zeros(3,3), H_2_2, zeros(3,3)];
 
    else
        % H1
        H_1_1 = [ x(3), -x(4),  x(1),  -x(2); ...
                    -x(2), -x(1), -x(4),  -x(3); ...
                    -x(1),  x(2),  x(3),  -x(4)] * 2 * g;
 
        H_1_2 = g * [2  * (x(1) * x(3) - x(2) * x(4)), 0, 0 ; ...
                        0, -2  * (x(1) * x(2) + x(3) * x(4)), 0; ...
                        0, 0, -x(1)^2 + x(2)^2 + x(3)^2 + x(4)^2];
 
        H_1 = [H_1_1, zeros(3,6), diag([1,1,1])];
 
        % H2
        H_2_1 = [1, 0, 0; ...
                0, 1, 0; ...
                0, 0, 1];
 
        H_2_2 = [x(5), 0, 0; ...
                0, x(6), 0; ...
                0, 0, x(7)];
 
        H_2 = [zeros(3,4), H_2_1, diag([1,1,1]), zeros(3,3)];
 
    end
 
    % H3
    H_3 = [zeros(3,4), diag([1,1,1]), zeros(3,n_x-7)];
 
    H = [H_1; H_2; H_3];
 
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