kalman.cpp

00001 #include "kalman.h"
00002 
00003 // Kalman filter module
00004  // Shamelessly ripped from http://forum.arduino.cc/index.php/topic,8652.0.html
00005 
00006  float Q_angle  =  0.001;
00007  float Q_gyro   =  0.003;
00008  float R_angle  =  0.03; 
00009 
00010  float x_angle = 0;
00011  float x_bias = 0;
00012  float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;      
00013  float y, S;
00014  float K_0, K_1;
00015 
00016 
00017  float kalmanCalculate(float newAngle, float newRate, float  dt) 
00018  {
00019   // dt = float(looptime)/1000;                                  
00020    x_angle += dt * (newRate - x_bias);
00021    P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
00022    P_01 +=  - dt * P_11;
00023    P_10 +=  - dt * P_11;
00024    P_11 +=  + Q_gyro * dt;
00025    
00026    y = newAngle - x_angle;
00027    S = P_00 + R_angle;
00028    K_0 = P_00 / S;
00029    K_1 = P_10 / S;
00030    
00031    x_angle +=  K_0 * y;
00032    x_bias  +=  K_1 * y;
00033    P_00 -= K_0 * P_00;
00034    P_01 -= K_0 * P_01;
00035    P_10 -= K_1 * P_00;
00036    P_11 -= K_1 * P_01;
00037    
00038    return x_angle;
00039  }