卡尔曼滤波C语言

H文件：
/*
 * 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: