Multiplicative Extended Kalman Filter for Attitude Quaternion Estimation. Intended for @zenitheesc CubeSats

mekf.m

clc; clear all; close all;
warning('off','Octave:data-file-in-path') 
addpath 'igrf/'

data = csvread("../python/simulation.csv");
% [time, position, velocity, attitude, omega,  B,     gyroscope,      gps])
%  1     2:4          5:7       8:11    12:14  15:17    18:20         21:23
sim_time = data(:, 1)';
attitude  = data(:, 8:11)';
sim_gyro = data(:, 18:20)';
sim_mag = data(:, 15:17)';
% sim_mag = sim_mag ./ vecnorm(sim_mag);
SENSacc = zeros(size(sim_mag));
cartesian = data(:, 2:4)';
position = data(:, 21:23)';

%% Some sensors output in degrees/second instead of rad/s
% sim_gyro = sim_gyro*(pi/180);

TF = length(sim_time);

%%% SkewSymetric Matrix from Vector
function M = skewsym(v)
    M = [      0 -v(3)    v(2); ...
             v(3)    0   -v(1); ...
            -v(2) v(1)      0 ];
end

%%% Quaternion Multiplication
function q = q_mul(q0, q1)
  r = q0(4);
  e = q0(1:3);
  psi = [r*eye(3) - skewsym(e); ...
         -e'];
  q = [psi q0]*q1;
end

%% Used to Propagate Covariance, given gyro measurement
function M = PhiOf(wpos, dt)
    s = sin(norm(wpos)*dt);
    c = cos(norm(wpos)*dt);
    sks = skewsym(wpos);
    n = norm(wpos);
    
    p11 = eye(3) - sks*s/n + sks^2*(1-c)/(n^2);
    p12 = sks * (1-c)/(n^2) - eye(3)*dt - sks^2*(n*dt - s)/(n^3);
    p21 = zeros(3,3);
    p22 = eye(3);
    M = [p11 p12; p21 p22];
end

%%% Discrete State propagation Matrix
%%% System dynamics, predicts attitude from previous
%%% estimate using the gyro data
function M = Omega(wpos, dt)
    psik = (sin(0.5*norm(wpos)*dt)*wpos)/norm(wpos);
    c = cos(0.5*norm(wpos)*dt);
    M = [ (c*eye(3)-skewsym(psik)) psik; -psik' c ];
end

%%% Quaternion to Cosine Matrix
function M = DCM(qprv)
    v = qprv(1:3);
    w = qprv(4);
    M = (w^2 - v'*v)*eye(3) + 2*v*v' - 2*w*skewsym(v);
end

%%% Measurement Model
function M = H(A, v)
    M = [ 2*skewsym(A*v) zeros(3,3)];
end

function out = DCMfromEuler(phi,theta,psi)
    %% Euler to DCM
    ct = cos(theta);
    st = sin(theta);
    sp = sin(phi);
    cp = cos(phi);
    ss = sin(psi);
    cs = cos(psi);
    out = [
            ct*cs,  sp*st*cs-cp*ss, cp*st*cs+sp*ss;
            ct*ss,  sp*st*ss+cp*cs, cp*st*ss-sp*cs;
            -st,    sp*ct,          cp*ct
        ];

end

function BI =  magneticModel(gps, day)
    lat = gps(1); 
    lng = gps(2);
    rho = gps(3); % alt from center of the earth
    
    thetaE = (90 - lat) * pi/180;
    psiE = lng * pi/180;
    phiE = 0;

    BNED = igrf(day, lat, lng, rho * 1e-3 , 'geocentric');
    
    %% Rotate with Satellite Orbit
    BI = DCMfromEuler(phiE, thetaE + pi, psiE) * BNED'; 
end

%%% Initial Conditions and Variable declarations
%%%
%% Process Covariance Matrix
Ppos = diag([ 0.5 0.5 0.5 0.1 0.1 0.1 ])^2;
%% Posteri attitude
qpos = [ 0; 0; 0; 1];
%% Posteri gyro bias 
bpos = [0; 0; 0];
%% delta of gyro bias
db = [0; 0; 0];
%% delta of attitude 
da = [0; 0; 0];

%% Gyro measurement noise spectral density
%% \omega = \omega_measured - Bias - sigma_v^2
gyro_range = max(vecnorm(sim_gyro(:,:)))
sigma_v =  0.5e-4;

%% Gyro bias drift noise spectral density
%% \dot Bias = sigma_u^2
sigma_u = 0.5e-3;

mag_range = max(vecnorm(sim_mag(:,:)))
sigma_medida = 1e-5 / mag_range;


%% Sensor Update Frequency
dt = sim_time(:, 2) - sim_time(:, 1)


R = eye(3)*sigma_medida^2

Gam = [-eye(3) zeros(3,3);...
       zeros(3,3) eye(3)];
       
Q = [eye(3)*(sigma_v^2*dt+(1/3)*(sigma_u^2)*(dt^3)) -eye(3)*(0.5*sigma_u^2*dt^2); ...
     -eye(3)*(0.5*sigma_u^2*dt^2)              eye(3)*(sigma_u^2*dt)]


t = sim_time;

est_att = zeros(4, 0);
est_bias = zeros(3, 0);
exp_B = zeros(3, 0);
process_cov = zeros(6, 0);
measurement_cov = zeros(3, 0);


%%% Filter Loop (predict + update)
for i = 1:(length(t))
    %% Get current gyro measurement
    w = sim_gyro(:,i);
    %% Get Accelerometer measurement
    z = sim_mag(:,i);
    %% Normalize measurement vector
    z = z./norm(z);
    
    %% Apply last estimate of bias to current measurement
    wpos = (w - bpos); %
    
    %% Predict Attitude
    qprv = Omega(wpos, dt)*qpos;

    %% Prepare to predict Covariance 
    Phi = PhiOf(wpos, dt);
    
    %% Predict current Covariance Matrix
    Pprv = Phi*Ppos*Phi' + Gam*Q*Gam';
 
    %% Predict that bias is constant
    bprv = bpos;
   
    %% Attitude as DCM to operate on measurement vector
    A = DCM(qprv);

    %% Expected Measurement given orbit position and date
    z_expected = magneticModel(position(:, i), '01-Jan-2020');
    z_expected = z_expected./norm(z_expected);

    %% Get Sensor Covariance around current attitude estimate (A)
    Hk = H(A, z_expected);

    %% Calculate Kalman Gain
    K = Pprv*Hk'*inv(Hk*Pprv*Hk' + R);
    
    %% Use K to find the difference to best
    %% estimate give the measurement (z)
    dx = K*(z - A*z_expected);
    
    %% Attitude Change, vector part of quaternion 
    da = dx(1:3);
    %% Change in Gyro bias
    db = dx(4:6);

    %% Apply Attitude change to prediction   
    qpos = q_mul([ da; sqrt(1 - dot(da', da) )], qprv);
    %% Keep quaternion from breaking norm invariant
    %% due to Floating-Point error
    qpos = qpos ./ norm(qpos);
    
    %% Apply Bias change to prediction
    bpos = bprv + db;
    
    %% Estimate Covariance Matrix given the 
    %% measurement (idirectly in K), and predicted
    %% Covariance Matrix. 
    Ppos = (eye(6) - K*Hk)*Pprv;
    
    %% Save Data
    est_att = [est_att qpos]; 
    est_bias = [est_bias bpos];
    
    exp_B = [exp_B z_expected];
    process_cov = [process_cov [Ppos(1, 1); Ppos(2, 2); Ppos(3, 3); Ppos(4, 4); Ppos(5, 5);Ppos(6, 6); ]];
    measurement_cov = [measurement_cov [Hk(1, 1); Hk(2, 2); Hk(3, 3)]];
end

%% Plotting
% time in hours
time = time ./ 60 ./ 60;

%% Check Sensors output
figure
subplot(2, 4, 1)
grid on;
plot(t, est_att, "LineWidth", 2)
hold on;
plot(t, attitude, "b", "LineWidth", 2)
l = legend("[Est] q_x","[Est] q_y","[Est] q_z", "[Est] q_w", "q_x","q_y","q_z", "q_w", 'Interpreter', 'latex' ); 
set(l, 'Interpreter','latex')
title("Estimated Attitude compared to Simulation");
ylim([-1 1]);


subplot(2, 4, 2)
grid on;
plot(t, est_bias , "LineWidth", 2.0)
legend("b_{gx}", "b_{gy}", "b_{gz}");
title("Estimated Gyroscope Bias");
ylim([-.5 .5]);

subplot(2, 4, 3)
grid on;
plot(t, sim_gyro' , "LineWidth", 2.0)
legend("\omega_x", "\omega_y", "\omega_z");
title("Gyroscope Data");
ylim([-1 1]);


subplot(2, 4, 4)
grid on;
plot(t, sim_mag', "LineWidth", 2.0)
legend("B_{mx}", "B_{my}", "B_{mz}");
title("Magnetometer Data")

subplot(2, 4, 5)
grid on;
plot(t, exp_B, "LineWidth", 2.0)
title("Simulated Magnetic Field")
legend("B_x", "B_y", "B_z");

subplot(2, 4, 6);
grid on;
plot(t, attitude - est_att, "LineWidth", 2.0);
title("Attitude Error");
legend("\Delta q_x","\Delta q_y","\Delta q_z", "\Delta q_w");
ylim([-2 2]);

subplot(2, 4, 7);
plot(t, process_cov, "LineWidth", 2.0);
grid on;
title("Process Covariance");
legend("(1,1)", "(2,2)", "(3,3)", "(4,4)", "(5,5)", "(6,6)");

subplot(2, 4, 8);
grid on;
plot(t, measurement_cov, "LineWidth", 2.0);
title("Measurement Covariance");
legend("(1,1)", "(2,2)", "(3,3)");


% uiwait(f);