succes
Diff: kalman.cpp
- Revision:
- 0:c15430f1895f
diff -r 000000000000 -r c15430f1895f kalman.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/kalman.cpp Sat Feb 11 15:55:34 2017 +0000 @@ -0,0 +1,39 @@ +#include "kalman.h" + +// Kalman filter module + // Shamelessly ripped from http://forum.arduino.cc/index.php/topic,8652.0.html + + float Q_angle = 0.001; + float Q_gyro = 0.003; + float R_angle = 0.03; + + float x_angle = 0; + float x_bias = 0; + float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0; + float y, S; + float K_0, K_1; + + + float kalmanCalculate(float newAngle, float newRate, float dt) + { + // dt = float(looptime)/1000; + x_angle += dt * (newRate - 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 = newAngle - 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; + + return x_angle; + }