kalman filter for idiots

x_angle += dt * (gyro_angle - x_bias);P_00 += - dt * (P_10 + P_01) + Q_angle * dt;P_01 += - dt * P_11;P_10 += - dt * P_11;P_11 += + Q_gyro * dt;y = acc_angle - x_angle;S =P_00 + R_angle;K_0 = P_00 / S;K_1 = P_10 / S;x_angle += K_0 * y;x_bias += K_1 * y;P_00 -= K_0 * P_00;P_01 -= K_0 * P_01;P_10 -= K_1 * P_00;P_11 -= K_1 * P_01;x_angle = kalman filter state estimatenow was that realy so hard?gyro_angle = gyro input in rad per secacc_angle = accel input in raddt=time since last loopQ_angle= 0.01Q_gyro= 0.03R_angle = 0.7x_angle = final estimate

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –


  • What are the dynamics and measurement equations you are using to feed this filter?
  • I have writen the Kalman filter codes in C on my MCU LPC2114 ,but it seems that my MCU is not strong enough to run the codes. My 12 states SINS/GPS integration system can not work correctly, and the time the filter costs is too long.
    Has any body ever did the same work?
  • I don't see the point of using a Kalman filter if you assume the noise variances to be constant. The Kalman gain coefficients K_0 and K_1 converge to constant values in this case, since they don't depend on any measurement. So instead you can reduce the filter of the (matlab-) example to :

    x_angle(i) = x_angle(i) + dt*(gyro_angle(i-1)-x_bias(i-1));
    y(i) = acc_angle(i)-x_angle(i);
    x_angle(i+1) = x_angle(i)+0.0233*y(i);
    x_bias(i+1) = x_bias(i)-0.0209*y(i);

    You omit all P-stuff and the CPU intensive divisions by S that way. Moreover the gain doesn't need to converge.
    I think Kalman filtering is only useful in this context if the variances are continuously monitored and evolving over time (for example, R_angle could be increased during accelerations, turns or at resonance frequencies of the engine). But this isn't that easy.
  • So I believe the title of this post aptly applies to me!! I pirated your code and matlab'ed it and let me say, WOW! So, what's the next step from here? I suppose that it is not as simple as running two independent loops for pitch and roll. Any further advice on how to proceed?
  • Hi!!, i implement this code in the Mc and i mounted on the airplane.... the x axis of the accelerometer is pointing to the airplane nose so... when the airplane starts to run the angle drifts as the ariplane accelerate... can i solve this problem with kalman filter o what i shoul use....

    Pls!!! Help!!!

  • T3
    Q_angle and Q_gyro are inverse of standard deviation?
    R_angle is convergence factor, the smaller - the faster we converge from gyro towards accelerometer angle?
    I see no problems for it to work with degrees... right?
  • 3D Robotics
    Very nice. Can you define the variables for us?
This reply was deleted.