Extended Kalman Filter with Quaternions for attitude estimation

Extended Kalman Filter with Quaternions for attitude estimation

/ Olivier Dugas

During my Master's degree, I found a complete tutorial for the quaternion algebra. The authors explained the process of implementing a Kalman filter for attitude estimation with 6 degrees of freedom. In this post, I show an implementation in Matlab. I've also made a Python version of the code, so write to me if you want to have it!

Before we start, please note that the coordinate system of the quaternions used are following the left-hand rule instead of the regular right-hand rule system. Still, you should not have any huge problem using it. The only thing is that you will need to be extra careful when you'll convert your data from and to quaternions.

Here's the paper I used. If you want an annotated version, I've made one while I studied this paper.

  title={Indirect Kalman filter for 3D attitude estimation},
  author={Trawny, Nikolas and Roumeliotis, Stergios I},
  journal={University of Minnesota, Dept. of Comp. Sci. \& Eng., Tech. Rep},

Ok now, let's start with the operations that are used in the filter when I manipulate the quaternions.

Here's a function that gets a skew matrix from a quaternion:

function [ qx ] = skew( q )   
    qx = [ 0   -q(3)  q(2);
          q(3)   0   -q(1);
         -q(2)  q(1)   0 ];

and in order to clean up the code, here's a one liner that invert the quaternion in the current convention.

function [q] = invQuat(q)
q = [-1*q(1:3);q(4)];

Also, according to the quaternion convention, here's how the paper said we could convert a quaternion into a rotation matrix. Unfortunately, it's a left-hand-rule rotation matrix.

function [c] = convertToMatrix(q)
    c = eye(3) - 2*q(4)*skew(q) + 2*(skew(q)*skew(q));
    % We have a left-hand-rule rotation matrix because of the current quaternion convention. 

Because of this fact, I tend to use axis-angle conversions instead of the above function. My program needed right-hand-rule matrix, so I did not make this function work.

Here's how to do the conversion of a quaternion from and to an axis-angle rotation.

function [q] = convertToQuaternion(axisangle)
% Convert a axisangle rotation to a quaternion
q = [axisangle(1:3).*sin(axisangle(4)/2); cos(axisangle(4)/2)];    

function [axisangle] = convertToAxisAngle(q)
t = wrapToPi(2*acos(q(4)));
k = q(1:3)./sin(t/2);
axisangle = [NormalizeV(k);t];

That last code snippet called a function called NormalizeV. It's a simple shortcut that normalize any vector. It helps to increase the readability of the main code.

function [normalized] = NormalizeV(aVector)
    normalized = aVector./norm(aVector);

I also implemented a function for the matrix Omega, which appears in the product of a vector and a quaternion.

function [omega] = Omega(w)
    omega = [ 0    w(3) -w(2) w(1);
             -w(3) 0     w(1) w(2);
              w(2) -w(1) 0    w(3);
             -w(1) -w(2) -w(3) 0 ];

The next function allows the multiplication of two quaternions in the current convention.

function [result] = multiplyQuaternion(q, p)
    result = [( q(4)*p(1) + q(3)*p(2) - q(2)*p(3) + q(1)*p(4));
              (-q(3)*p(1) + q(4)*p(2) + q(1)*p(3) + q(2)*p(4));
              ( q(2)*p(1) - q(1)*p(2) + q(4)*p(3) + q(3)*p(4));
              (-q(1)*p(1) - q(2)*p(2) - q(3)*p(3) + q(4)*p(4))];

And finally what you were looking for, the densely documented EKF filter using quaternions. Note that I added a lot of comments. Although Uncle Bob would probably be mad at me for this, I think they are useful. The numbers in the comments tell what steps in the paper I am implementing.

function [ mu1, sigma1, w ] = ekf( mu0, sigma0, u1, z, errZ, sigr, sigw, dt)
% EKF with quaternions as seen in
% 'Indirect Kalman Filter for 3D Attitude Estimation'
% by Trawny and Roumeliotis
% Author: Olivier Dugas, ing. jr

%% Initial Setup
% Assume we receive gyro measurements w0 and w1. 
w0 = u1(:,1);
w1 = u1(:,2);
% We have an estimate of the quaternion q_hat_avg and the bias b0
% mu0 = [q_hat_avg_0 ; b0]
q_hat_avg_0 = mu0(1:4);
b0 = mu0(5:7);
% Together with the corresponding covariance matrix P_k|k
% sigma0 = P_k|k
% From this, we also have an estimate of the old turn rate w_hat_0
w_hat_0 = w0 - b0;

%% State Prediction
% Regular EKF algorithm line equivalent: mu1_ = g(u1, mu0)

% We instead proceed as follows:
% 1. We propagate the bias (assuming the bias is constant over the integration interval)
b1 = b0; % TODO : Here is a problem : we have no way of validating or updating the bias. Update it externally periodically can do the trick.

% 2. Using the measurement w1 and b1, we form the estimate of the new turn rate w_hat_1
w_hat_1 = w1 - b1;

% 3. We propagate the quaternion using a first order integrator with w_hat_0 and w_hat_1 tp obtain q_hat_avg_1
w_avg = (w_hat_0 + w_hat_1) / 2;
q_hat_avg_1 = NormalizeV((exp(1/2*Omega(w_avg)*dt) + 1/48*(Omega(w_hat_1)*Omega(w_hat_0) - Omega(w_hat_0)*Omega(w_hat_1))*dt^2) * q_hat_avg_0);
% q_hat_avg_1
% mu1_ = [q_hat_avg_1 ; b1]

%% Error Propagation during State Prediction
% Regular EKF algorithm line equivalent: sigma1_ = G1*sigma0*G1' + R1

% 4. We compute the state transition matrix Phi and the discrete time noise covariance matrix Qd
% Computation of Phi
if norm(w_avg) < 0.00001 %Too small, may induce numerical instability. Estimating instead.
    Theta = eye(3) - dt*skew(w_avg) + ((dt.^2)/2)*(skew(w_avg)*skew(w_avg));
    Psi = -eye(3)*dt + ((dt^2)/2)*skew(w_avg) - ((dt^3)/6)*(skew(w_avg)*skew(w_avg));
    Theta = cos(norm(w_avg)*dt)*eye(3) - sin(norm(w_avg)*dt)*skew(w_avg/norm(w_avg)) + (1 -cos(norm(w_avg)*dt))*(w_avg/norm(w_avg))*(w_avg'/norm(w_avg));
    Psi = -eye(3)*dt + (1/norm(w_avg).^2)*(1-cos(norm(w_avg)*dt))*skew(w_avg) - (1/norm(w_avg).^3)*(norm(w_avg)*dt - sin(norm(w_avg)*dt))*(skew(w_avg)*skew(w_avg));
Phi = [Theta Psi; zeros(3) eye(3)];

% Computation of Qd
if norm(w_avg) < 0.00001  %Too small, may induce numerical instability. Estimating instead.
    Q11 = (sigr.^2)*dt*eye(3) + (sigw.^2)*(eye(3)*dt^3/3 + (dt^5/60)*(skew(w_avg)*skew(w_avg)));
    Q12 = -(sigw^2) * ( eye(3)*dt.^2/2 - (dt^3/6)*skew(w_avg) + (dt^4/24)*(skew(w_avg)*skew(w_avg)));
    Q11 = (sigr.^2)*dt*eye(3) + (sigw.^2)*( eye(3)*dt.^3/3 + (((norm(w_avg)*dt)^3/3 + 2*sin(norm(w_avg)*dt) - 2*norm(w_avg)*dt )/ (norm(w_avg)^5))*(skew(w_avg)*skew(w_avg)));
    Q12 = -(sigw^2) * ( eye(3)*dt.^2/2 - ((norm(w_avg)*dt - sin(norm(w_avg)*dt))/(norm(w_avg)^3))*skew(w_avg) + (((norm(w_avg)*dt)^2/2 + cos(norm(w_avg)*dt) - 1)/(norm(w_avg)^4))*(skew(w_avg)*skew(w_avg)));
Q22 = (sigw^2)*dt*eye(3);
Qd = [Q11 Q12; Q12' Q22];

% 5. We Compute the state covariance matrix according to the Extended Kalman Filter equation
sigma1_ = Phi*sigma0*Phi' + Qd;

%% Computation of the Kalman gain
% Regular EKF algorithm line equivalent: K1 = sigma1_*H1'/(H1*sigma1_*H1' + errZ)

% Measurement prediction function: we convert q_hat_avg_1 to an axis-angle representation
z_hat = convertToAxisAngle(q_hat_avg_1); 
% z_hat = q_hat_avg_1; 

% 6. Compute the measurement matrix H
% H = [skew(z_hat) zeros(3)]; 
H = [skew(q_hat_avg_1) zeros(3)]; 

% 7. Compute residual r according to r = z - z_hat
z_quat = convertToQuaternion(z);
z_hat_quat = convertToQuaternion(z_hat);
r = multiplyQuaternion(z_quat,invQuat(z_hat_quat));
% r = multiplyQuaternion(z,invQuat(z_hat))

% 8. Compute the covariance of the residual S as
S = H*sigma1_*H' + errZ;

% 9. Compute the Kalman gain K
K = sigma1_*(H'*inv(S));

%% Measurement Update - State
% Regular EKF algorithm line equivalent: mu1 = mu1_ + K1*(z1-h(mu1_))

% 10. Compute the correction deltaX = [2*dq;db]
deltaX = K*r(1:3);
dq = deltaX(1:3)/2;
db = deltaX(4:6);

% 11. Update the quaternion
if (dq'*dq) > 1
    dq_hat_avg_1 = (1/sqrt(1 + (dq'*dq))) * [dq ; 1];
    dq_hat_avg_1 = [dq ; (sqrt(1 - (dq'*dq)))];
q = multiplyQuaternion(dq_hat_avg_1, q_hat_avg_1);

% 12. Update the bias
b = b1 + db;

mu1 = [q ; b];

% 13. Update the estimated turn rate using the new estimate for the bias
w = w1 - b;

%% Measurement Update - Error
% Regular EKF algorithm line equivalent: sigma1 = (eye(6) - K1*H1)*sigma1_

% 14. Compute the new updated Covariance matrix
sigma1 = (eye(6) - K*H) * sigma1_ * (eye(6) - K*H)' + K*errZ*K';


Enjoy and I hope it helps someone someday! Ask me if you need the code in Python.