Monitor program only for mbed GR-PEACH
Diff: kf.cpp
- Revision:
- 4:76b3113c79ff
diff -r 989d13762f43 -r 76b3113c79ff kf.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/kf.cpp Sun Dec 14 09:17:01 2014 +0000 @@ -0,0 +1,52 @@ +/* + * mbed Application program for the mbed ST NUCLEO F401RE Board + * + * Modified by Kenji Arai / JH1PJL + * http://www.page.sannet.ne.jp/kenjia/index.html + * http://mbed.org/users/kenjiArai/ + * Revised: August 28th, 2014 + */ + +// <Original> http://www.x-firm.com/?page_id=191 + +// Main module K_bot angle angles in Quids, 10 bit ADC --------------------------------------- +// 7 - Data fusing, Kalman filter + +// Installation: +// Create "Kalman" and "Sensors" tabs +// Cut and paste the 2 modules in their respective tab +// Save as "Kas_bot_angle" + +// --- Kalman filter module ---------------------------------------------------------------------- +float Q_angle = 0.001; //0.001 +float Q_gyro = 0.003; //0.003 +float R_angle = 0.03; //0.03 + +float x_angle = 0; +float x_bias = 0; +float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0; +float dt, y, S; +float K_0, K_1; + +float kalmanCalculate(float newAngle, float newRate, int looptime) { + 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; +}