Marek Korczak / nrf51_microbit

succes

Dependencies:   microbit

kalman.cpp@0:c15430f1895f, 2017-02-11 (annotated)

Committer:
mk1
Date:
Sat Feb 11 15:55:34 2017 +0000
Revision:
0:c15430f1895f
succes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mk1 0:c15430f1895f 1 #include "kalman.h"
mk1 0:c15430f1895f 2
mk1 0:c15430f1895f 3 // Kalman filter module
mk1 0:c15430f1895f 4 // Shamelessly ripped from http://forum.arduino.cc/index.php/topic,8652.0.html
mk1 0:c15430f1895f 5
mk1 0:c15430f1895f 6 float Q_angle = 0.001;
mk1 0:c15430f1895f 7 float Q_gyro = 0.003;
mk1 0:c15430f1895f 8 float R_angle = 0.03;
mk1 0:c15430f1895f 9
mk1 0:c15430f1895f 10 float x_angle = 0;
mk1 0:c15430f1895f 11 float x_bias = 0;
mk1 0:c15430f1895f 12 float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
mk1 0:c15430f1895f 13 float y, S;
mk1 0:c15430f1895f 14 float K_0, K_1;
mk1 0:c15430f1895f 15
mk1 0:c15430f1895f 16
mk1 0:c15430f1895f 17 float kalmanCalculate(float newAngle, float newRate, float dt)
mk1 0:c15430f1895f 18 {
mk1 0:c15430f1895f 19 // dt = float(looptime)/1000;
mk1 0:c15430f1895f 20 x_angle += dt * (newRate - x_bias);
mk1 0:c15430f1895f 21 P_00 += - dt * (P_10 + P_01) + Q_angle * dt;
mk1 0:c15430f1895f 22 P_01 += - dt * P_11;
mk1 0:c15430f1895f 23 P_10 += - dt * P_11;
mk1 0:c15430f1895f 24 P_11 += + Q_gyro * dt;
mk1 0:c15430f1895f 25
mk1 0:c15430f1895f 26 y = newAngle - x_angle;
mk1 0:c15430f1895f 27 S = P_00 + R_angle;
mk1 0:c15430f1895f 28 K_0 = P_00 / S;
mk1 0:c15430f1895f 29 K_1 = P_10 / S;
mk1 0:c15430f1895f 30
mk1 0:c15430f1895f 31 x_angle += K_0 * y;
mk1 0:c15430f1895f 32 x_bias += K_1 * y;
mk1 0:c15430f1895f 33 P_00 -= K_0 * P_00;
mk1 0:c15430f1895f 34 P_01 -= K_0 * P_01;
mk1 0:c15430f1895f 35 P_10 -= K_1 * P_00;
mk1 0:c15430f1895f 36 P_11 -= K_1 * P_01;
mk1 0:c15430f1895f 37
mk1 0:c15430f1895f 38 return x_angle;
mk1 0:c15430f1895f 39 }