This website is awesome! I wanted to start this topic to discuss and improve Kalman filters for IMU data.
I downloaded the "KalmanTestBoardV1.zip" from Tom's website at "http://tom.pycke.be/mav/92/kalman-demo-application", which I realize is from 2007, but it was the only code I found.
A couple of things I noticed:
1) In the ars_predict function Q is multiplied by dt, but I don't think it should be multiplied by anything. It's not in the comments above the function and I looked it up in some other resources.
- Code: Select all
//Comments
* P = F P transpose(F) + Q
*
* = [ 1 -dt, 0 1 ] * P * [ 1 0, -dt 1 ] + Q
//Code
filterdata->P_00 += - dt * (filterdata->P_10 + filterdata->P_01) + filterdata->Q_angle * dt;
filterdata->P_01 += - dt * filterdata->P_11;
filterdata->P_10 += - dt * filterdata->P_11;
filterdata->P_11 += + filterdata->Q_gyro * dt;
2)In the ars_update function P_00 and P_01 are recalculated and half the lines use the new values. So in the code below, lines 1 and 2 use the old P_00 and P_01 and lines 3 and 4 use the new value. I simulated the results and it appear to have a negligible effect, but thought I'd mention it.
- Code: Select all
filterdata->P_00 -= K_0 * filterdata->P_00;
filterdata->P_01 -= K_0 * filterdata->P_01;
filterdata->P_10 -= K_1 * filterdata->P_00;
filterdata->P_11 -= K_1 * filterdata->P_01;