# Quaternion Kalman Filter Algorithm

I have been stuck on this for weeks, I really hope that someone can help me with this,thank you in advance. I am trying to write an IMU attitude estimation algorithm using quaternion kalman filter. So based on this research paper: https://hal.archives-ouvertes.fr/hal-00968663/document, I have developed the following pseudo code algorithm:

Predict Stage:

Qk+1/k = Ak * Qk; where Ak contains the gyro measurement.

Pk+1/k = Ak * Pk *Ak.transpose() + Q; where Q is assumed to be zero.

After prediction, we can use this formula to get the supposed gravity measurement of accelerometer Yg in body frame :

Yg = R * G; // R is the rotation matrix generated from quaternion Qk+1/k and G = (0,0,0,9.81).

This equation then translates to the following equation which allows me to get measurement model matrix H.

H * Qk+1/k = 0; //where H stores value related to (Yg-G).

Update Stage:

K = P * H * (H * P * H.transpose()+R)^(-1); //R should be adaptively adjusted but right now initialized as identity matrix

Qk+1/k+1 = (I-KH)Qk+1/k;

Qk+1/K+1 = (Qk+1/K+1)/|Qk+1/k+1|; //Normalize quaternion

Pk+1/K+1 = (I - KH)Pk+1/k;

The following is the main part of my code. The complete C++ code is at here https://github.com/lyf44/fcu if you want to test.

Matrix3f skew_symmetric_matrix(float a, float b, float c, float d){
Matrix3f matrix;
matrix << a,d*(-1),c,
d,a,b*(-1),
c*(-1),b,a;
return (matrix);
}

void Akf::state_transition_matrix(float dt,float gx,float gy, float gz){
Vector3f tmp;
tmp(0) = gx*PI/180;
tmp(1) = gy*PI/180;
tmp(2) = gz*PI/180;
float magnitude = sqrt(pow((float)tmp(0),2)+pow((float)tmp(1),2)+pow((float)tmp(2),2));

/*q(k+1) = |  cos(|w|*dt/2)       | quaternion_multiply q(k)
|  w/|w|*sin(|w|*dt/2) |
*/
//w/|w|*sin(|w|*dt/2)
tmp = tmp/magnitude*sin(magnitude*dt/2);
//quaternion multiplication
A(0,0) = cos(magnitude*dt/2);
A.block<3,1>(1,0) = tmp;
A.block<1,3>(0,1) = tmp.transpose()*(-1);

Matrix3f skew_symmetric;
skew_symmetric = skew_symmetric_matrix((float)A(0,0),(float)tmp(0),(float)tmp(1),(float)tmp(2));
A.block<3,3>(1,1) = skew_symmetric;
}

void Akf::observation_model_matrix(Vector3f meas){
Vector3f G;
Vector3f tmp;
G << 0,0,9.81;
/* H = | 0        -(acc-G).transpose     |
*     | (acc-G)  -(acc+G).skewsymmetric |
*/
tmp = meas-G;
H(0,0) = 0;
H.block<3,1>(1,0) = tmp;
H.block<1,3>(0,1) = tmp.transpose()*(-1);
tmp = tmp+G+G;
Matrix3f matrix;
matrix = skew_symmetric_matrix(0,(float)tmp(0),(float)tmp(1),(float)tmp(2));
H.block<3,3>(1,1) = matrix*(-1);
//H = H*(0.5);
cout<<"H"<<endl;
cout<<H<<endl;
cout<<"H*X"<<endl;
std::cout<<H*X<<std::endl;
}

void Akf::setup(){
X_prev = Vector4f::Zero(4,1);
X_prev(0) = 1;
Q = Matrix4f::Zero(4,4);
Z = Vector4f::Zero(4,1);
R = Matrix4f::Identity(4,4);
P_prev = Matrix4f::Identity(4,4);
P_prev = P_prev*(0.1);
I = Matrix4f::Identity(4,4);

sum = Vector4f::Zero(4,1);
noise_sum = Matrix4f::Zero(4,4);
counter=1;
}

void Akf::predict_state(){
cout<<(60*counter%360)<<endl;
X = A*X_prev;
A_T = A.transpose();
P = A*P_prev*A_T+Q;
}

void Akf::update_state(){
Matrix4f PH_T;
Matrix4f tmp;

PH_T = P*H.transpose();
S = H*PH_T+R;
if (S.determinant()!= 0 )
{
tmp = S.inverse();
K = P*H*tmp;
//std::cout<<"K"<<std::endl;
//std::cout<<K<<std::endl;
X_updated = (I-K*H)*X;
X_updated = X_updated /(X_updated.norm());
P_updated = (I-K*H)*P;
}
else{
X_updated = X;
std::cout<< "error-tmp not inversible!"<<std::endl;
}
X_prev = X_updated;
P_prev = P_updated;
}

void rotation_matrix(Vector4f q,Matrix3f &rot_matrix){
int i;
for (i=1;i<4;i++){
q(i) = q(i)*(-1);
}
Matrix3f matrix;
matrix(0,0) = pow((float)q(0),2)+pow((float)q(1),2)-pow((float)q(2),2)-pow((float)q(3),2);
matrix(0,1) = 2*(q(1)*q(2)-q(0)*q(3));
matrix(0,2) = 2*(q(0)*q(2)+q(1)*q(3));
matrix(1,0) = 2*(q(1)*q(2)+q(0)*q(3));
matrix(1,1) = pow((float)q(0),2)-pow((float)q(1),2)+pow((float)q(2),2)-pow((float)q(3),2);
matrix(1,2) = 2*(q(2)*q(3)-q(0)*q(1));
matrix(2,0) = 2*(q(1)*q(3)-q(0)*q(2));
matrix(2,1) = 2*(q(0)*q(1)+q(2)*q(3));
matrix(2,2) = pow((float)q(0),2)-pow((float)q(1),2)-pow((float)q(2),2)+pow((float)q(3),2);
rot_matrix = matrix;
}

Vector3f generate_akf_random_measurement(Vector4f state){
int i;
//compute quaternion rotation matrix
Matrix3f rot_matrix;
rotation_matrix(state,rot_matrix);
//rot_matrix*acceleration in NED = acceleration in body-fixed frame
Vector3f true_value = rot_matrix*G;
std::cout<<"true value"<<std::endl;
std::cout<<true_value<<std::endl;
for (i=0;i<3;i++){
noisy_value(i) = true_value(i) + (-1) + (float)(rand()/(float)(RAND_MAX/2));
}
return (noisy_value);
}

int main(){
float gx,gy,gz,dt;
gx =60; gy=0; gz =0; //for testing, let it rotate around x axis by 60 degree
myakf.state_transition_matrix(dt,gx,gy,gz); // dt is elapsed time
myakf.predict_state();
Vector4f state = myakf.get_predicted_state();
Vector3f meas = generate_akf_random_measurement(state);
myakf.observation_model_matrix(meas);
myakf.measurement_noise();
myakf.update_state();
q = myakf.get_updated_state();


The problem that I face is that my code does not work.The prediction stage works fine but the updated quaternion state is only correct for the first few iterations and it starts to drift away from the correct value. I have checked my code against the research paper multiple times and ensured that it is in accordance with the algorithm proposed by the research paper.

In my test, I am rotating around x axis by 60 degree per iterations. The number below the started is the angle of rotation. state and updated state is the predicted and updated quaternion respectivly while true value, meas, result are acceleration due to gravity in body frame.As the test result indicates, everything is way off after rotating 360 degrees. The following is my test result:

1
started
60
state
0.866025
0.5
0
0
true value
0
8.49571
4.905
meas
0.314533
7.97407
4.98588
updated state
0.866076
0.499913
-2.36755e-005
1.56256e-005
result
0.000555564
8.49472
4.90671

1
started
120
state
0.500087
0.865975
-2.83164e-005
1.69446e-006
true value
0.000306622
8.4967
-4.90329
meas
-0.532868
8.79841
-4.80453
updated state
0.485378
0.862257
-0.129439
-0.064549
result
0.140652
8.37531
-5.10594

1
started
180
state
-0.0107786
0.989425
-0.0798226
-0.12062
true value
-2.35843
-0.0203349
-9.52226
meas
-1.39627
-0.889284
-8.74243
updated state
-0.0195091
0.981985
-0.151695
-0.110965
result
-2.19598
-0.0456112
-9.56095

1
started
240
state
-0.507888
0.840669
-0.0758893
-0.171946
true value
-3.59229
-8.12105
-4.16894
meas
-4.52356
-7.73113
-4.98735
updated state
-0.53758
0.811101
-0.212643
-0.0889171
result
-3.65783
-8.18397
-3.98485

1
started
300
state
-0.871108
0.433644
-0.139696
-0.183326
true value
-3.94732
-6.909
5.73763
meas
-4.36385
-6.98853
5.39759
updated state
-0.86404
0.436764
-0.102296
-0.228487
result
-3.69216
-6.94565
5.86192
1
started
0
state
-0.966663
-0.0537713
0.0256525
-0.249024
true value
0.749243
0.894488
9.74036
meas
-0.194541
0.318586
10.1868
updated state
-0.78986
-0.0594022
0.0311688
-0.609607
result
1.1935
0.547764
9.72171

1
started
60
state
-0.654338
-0.446374
0.331797
-0.512351
true value
8.74674
2.39526
3.74078
meas
9.36079
2.96653
3.57115
updated state
-0.52697
-0.512048
0.221843
-0.64101
result
8.73351
2.50411
3.70018


Can someone help me confirm that my understanding about the theory of this quaternion kalman filter and my pseudo code is correct? Also, if anyone has implemented attitude estimation using maybe a different version of quaternion kalman filter, I would greatly appreciate if you can provide a pseudo code and a little explanation. Thank you guys very much!

• How do you know the filter is deviating from the correct value? What code/method are you using to find the correct value? – Chuck Apr 11 '16 at 13:48
• I understand that most people use multiplicative quaternion kalman filter out there meaning their last update step is something like Qk+1/k+1 = Qk+1/k * Qkalman. But the linear addictive model is also well proposed in multiple research paper and it is claimed to work better than the multiplicative one. That is why I am trying to make it work – Yunfan Lu Apr 12 '16 at 2:49
• I must ask; did you consider that you may be accumulating gyro error over time? – Biscuits Apr 14 '16 at 0:31

I hope this is / hope it isn't your problem, as it could be a face-palm moment, but I don't think you're calculating your Kalman gain correctly.

I've only used a Kalman filter a couple times in the past, so I'm by no means an expert on it, but I believe that the Kalman gains are calculated:

$$K = P H^T (HP H^T + R)^{-1}$$

You have, in your pseudo-code and in your pasted code:

$$K = P H (HP H^T + R)^{-1}$$

I believe what you're missing is that $H^T$, or H.transpose() in your code. You define

PH_T = P*H.transpose();


but H.transpose() is used a couple times, so I think you would be better off defining

H_T = H.transpose();


and using that in the two locations you need $H^T$.

That said, I hope this is/isn't your problem - I know how painful it is to waste so much time on a typo. If this doesn't solve your problem, please post more information on how you're getting from the gyro output to the quaternion. I believe this is where you will find your issues.

• Thank you very much for your input, but this is not the problem that I am facing, because this method defined by the research paper defines H in a slightly different way. I have not thoroughly understand its theory yet but at least in my test multiplying by H can give me correct result in the first few iterations while by H.transpose() just gives me wrong results. I will post more information. – Yunfan Lu Apr 12 '16 at 2:05
• My prediction stage works fine which means steps from gyro to quaternion should be correct. I have added a picture in my question in the predict stage indicating the equation that I am using. – Yunfan Lu Apr 12 '16 at 2:38

Thanks to the comment by Biscuits, in the end I realized that I have been adding random number to the predicted state instead of true state. Therefore, I am essentially accumulating my gyro errors which leads to the wrong results. I hope that other people won't make the same mistakes again.