Extended Kalman Filter With Quaternions for Attitude Estimation
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.
1234567
@article{trawny2005indirect,
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},
volume={2},
year={2005}}
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:
and in order to clean up the code, here’s a one liner that invert the quaternion in the current convention.
123
function[q] =invQuat(q)q=[-1*q(1:3);q(4)];end
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.
1234
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. end
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.
12345678910
function[q] =convertToQuaternion(axisangle)% Convert a axisangle rotation to a quaternionq=[axisangle(1:3).*sin(axisangle(4)/2);cos(axisangle(4)/2)];endfunction[axisangle] =convertToAxisAngle(q)t=wrapToPi(2*acos(q(4)));k=q(1:3)./sin(t/2);axisangle=[NormalizeV(k);t];end
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.
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_0w_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_1w_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_1w_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 Phiifnorm(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));elseTheta=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));endPhi=[ThetaPsi;zeros(3)eye(3)];% Computation of Qdifnorm(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)));elseQ11=(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)));endQ22=(sigw^2)*dt*eye(3);Qd=[Q11Q12;Q12'Q22];% 5. We Compute the state covariance matrix according to the Extended Kalman Filter equationsigma1_=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 representationz_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_hatz_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 asS=H*sigma1_*H'+errZ;% 9. Compute the Kalman gain KK=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 quaternionif(dq'*dq)>1dq_hat_avg_1=(1/sqrt(1+(dq'*dq)))*[dq;1];elsedq_hat_avg_1=[dq;(sqrt(1-(dq'*dq)))];endq=multiplyQuaternion(dq_hat_avg_1,q_hat_avg_1);% 12. Update the biasb=b1+db;mu1=[q;b];% 13. Update the estimated turn rate using the new estimate for the biasw=w1-b;%% Measurement Update - Error% Regular EKF algorithm line equivalent: sigma1 = (eye(6) - K1*H1)*sigma1_% 14. Compute the new updated Covariance matrixsigma1=(eye(6)-K*H)*sigma1_*(eye(6)-K*H)'+K*errZ*K';end
Enjoy and I hope it helps someone someday! Ask me if you need the code in Python.