找到一段卡尔曼滤波的代码 求大神帮看一下

/*

 * KalmanFilter.h

 * Non-original

 * Author: x2d 

 * Copyright (c) 2012 China

 *

 */



#ifndef KalmanFilter_h

#define KalmanFilter_h



#include <WProgram.h>



class KalmanFilter

{

  public:

  KalmanFilter();

  

  /*

    卡尔曼融合计算

    angle_m: 加速度计测量并通过atan2(ax,ay)方法计算得到的角度（弧度值）

    gyro_m:陀螺仪测量的角速度值（弧度值）

    dt:采样时间(s)

    outAngle:卡尔曼融合计算出的角度（弧度值）

    outAngleDot:卡尔曼融合计算出的角速度（弧度值）

  */

  void getValue(double angle_m, double gyro_m, double dt, double &outAngle, double &outAngleDot);

  

  private:

  double C_0, Q_angle, Q_gyro, R_angle;

  double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

  double angle, angle_dot;

  double P[2][2];

  double Pdot[4];

};



/*CPP文件  */

/*

 * KalmanFilter.cpp

 * Non-original

 * Author: x2d 

 * Copyright (c) 2012 China

 *

 */

 

 #include "KalmanFilter.h"

 

 KalmanFilter::KalmanFilter()

 {

   C_0 = 1.0f;

   Q_angle = 0.001f;//加速度计的过程噪声协方差

   Q_gyro = 0.003f;//陀螺仪的过程噪声协方差

   R_angle = 0.5f;//加速度计的测量噪声协方差

   q_bias = angle_err = PCt_0 = PCt_1 = E = K_0 = K_1 = t_0 = t_1 = 0.0f;

   angle = angle_dot = 0.0f;

   P[0][0] = 1.0f;

   P[0][1] = 0.0f;

   P[1][0] = 0.0f;

   P[1][1] = 1.0f;

   Pdot[0] = 0.0f;

   Pdot[1] = 0.0f;

   Pdot[2] = 0.0f;

   Pdot[3] = 0.0f;

 }

 

void  KalmanFilter::getValue(double angle_m, double gyro_m, double dt, double &outAngle, double &outAngleDot)

{

/*

  Serial.print("angle_m = ");

  Serial.print(angle_m);

  Serial.print(";");

  Serial.print("gyro_m = ");

  Serial.print(gyro_m);

  Serial.print(";");

*/

  //陀螺仪积分的先验角度

  angle+=(gyro_m-q_bias) * dt;



  //角度偏差(用于计算后验状态估计)

  angle_err = angle_m - angle;



  //先验误差协方差的微分

  Pdot[0] = Q_angle - P[0][1] - P[1][0];

  Pdot[1] = -P[1][1];

  Pdot[2] = -P[1][1];

  Pdot[3] = Q_gyro;



  //先验误差协方差的积分

  P[0][0] += Pdot[0] * dt;//计算加速度计过程噪声协方差

  P[0][1] += Pdot[1] * dt;

  P[1][0] += Pdot[2] * dt;

  P[1][1] += Pdot[3] * dt;//计算陀螺仪过程噪声协方差



  //卡尔曼增益计算

  PCt_0 = C_0 * P[0][0];

  PCt_1 = C_0 * P[1][0];

  E = R_angle + C_0 * PCt_0;//测量余量协方差

  K_0 = PCt_0 / E;//加速度计的卡尔曼增益

  K_1 = PCt_1 / E;//陀螺仪偏差的卡尔曼增益



  //后验误差协方差计算

  t_0 = PCt_0;

  t_1 = C_0 * P[0][1];

  P[0][0] -= K_0 * t_0;

  P[0][1] -= K_0 * t_1;

  P[1][0] -= K_1 * t_0;

  P[1][1] -= K_1 * t_1;



  //更新状态

  angle += K_0 * angle_err;//后验估计最优角度值

  q_bias += K_1 * angle_err;//更新最优估计值偏差

  angle_dot = gyro_m-q_bias;//更新最优角速度(后验估计)

  

  outAngle = angle;

  outAngleDot = angle_dot;

/*

  Serial.print("angle = ");

  Serial.print(angle);

  Serial.print(";");

  Serial.print("angle_dot = ");

  Serial.print(angle_dot);

  Serial.print(";");

*/

}