derivation6Ekf6D.m

Go to the documentation of this file.

% =========================================================================== %
%> @example derivation6Ekf6D.m
%>
%> In this example, a 6D orientation and position estimation is performed.
%>
%> @note a more detailed explanation can be found here: @ref derivation6Ekf6D
%>
%> @warning This document is in the debug branch. The state vector is augmentet with the sensitivity. This is not the final version.
%>
%> <details>
% =========================================================================== %
clear; close all; clc;
 
% set seed for reproducibility
rng(1);
 
%% Sensor Data Generation
% load predefined trajectory
load('rotatingCube.mat');
 
js = JumpSensor;
 
data = js.generateFromTrajectory(traj);
 
% define time step
dt = 1/data.mpu_rate;
 
% get actual bias and sensitivity
bias_acc = js.getParams.mpu.acc.ConstantBias;
bias_gyr = js.getParams.mpu.gyr.ConstantBias;
 
%% 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(12, size(data.tt.acc,1));
z(1:3,:) = data.tt.acc';
z(4:6,:) = data.tt.gyr' * pi/180; %convert to radians/s
% transitional velocity in rows 7,8,9 is zero
% angular velocity in rows 10,11,12 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);
x_0(1) = 1; % initial quaternion
x_0(11:13) = traj.getTrajectory.pos(1,:); % initial position
x_0(23:25) = 1; % 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-4;
beta = 0.6;
gamma = 0.5;
zeta = 5 * pi / 180;
kappa = 0.03;
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)]) .^ 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, zero_velocity(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, zero_velocity(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 ~zero_velocity(k)
        % no zero velocity detected
        H(7:12, :) = 0;
    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
 
%% 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
 
%% prepare results
% ground truth
ori_gt = quaternion(traj.getTrajectory.ori);
pos_gt = traj.getTrajectory.pos;
 
% 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,:)';
 
%% compare results
 
% orientation animation
animateQuatPos(ori_filt, pos_filt, ori_smoo, pos_smoo, ori_gt, pos_gt, ...
                "names", ["Filter", "Smoother", "Ground Truth"], "title", "Orientation and Position Estimation", ...
                "position", [0,0.25,0.5,0.5]);
 
% position plot
figure("Units", "normalized", "Position", [0.5,0.25,0.5,0.5]);
plot3(pos_gt(:,1), pos_gt(:,2), pos_gt(:,3), 'DisplayName', 'Ground Truth');
hold on;
plot3(pos_filt(:,1), pos_filt(:,2), pos_filt(:,3), 'DisplayName', 'Filter');
plot3(pos_smoo(:,1), pos_smoo(:,2), pos_smoo(:,3), 'DisplayName', 'Smoother');
hold off;
grid on;
xlabel("x [m]");
ylabel("y [m]");
zlabel("z [m]");
legend;
 
 
%% 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;
 
    % pseudo measurements
    v_g = x(8:10); % velocity in global frame
    h3 = [v_g; w_s];
 
    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(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 (pseudo measurements)
    H_3 = [zeros(3,7), eye(3), zeros(3,15); ...
           zeros(3,13), 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, zero_velocity)
 
    %> \todo change process noise based on zero velocity detection
 
    sigma_w_p = 0.1 * pi / 180;     % educated guess
    sigma_a_p = 0.025;              % educated guess
    sigma_b_a = 0.001;              % from allan variance
    sigma_b_w = 0.002 * pi / 180;   % from allan variance
    sigma_s_a = 1e-5;               % 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