1
$\begingroup$

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. enter image description here

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!

Improve this question
$\endgroup$
3
  • $\begingroup$ How do you know the filter is deviating from the correct value? What code/method are you using to find the correct value? $\endgroup$
    – Chuck ♦
    Apr 11, 2016 at 13:48
  • $\begingroup$ 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 $\endgroup$
    – Yunfan Lu
    Apr 12, 2016 at 2:49
  • $\begingroup$ I must ask; did you consider that you may be accumulating gyro error over time? $\endgroup$
    – Biscuits
    Apr 14, 2016 at 0:31

2 Answers 2

Reset to default
0
$\begingroup$

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.

Improve this answer
$\endgroup$
2
  • $\begingroup$ 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. $\endgroup$
    – Yunfan Lu
    Apr 12, 2016 at 2:05
  • $\begingroup$ 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. $\endgroup$
    – Yunfan Lu
    Apr 12, 2016 at 2:38
0
$\begingroup$

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.

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.