中科因仑“3+1”工程特种兵精英论坛

标题: 应用于两轮平衡车上的卡尔曼滤波程序 [打印本页]

作者: 痛苦的滋味 时间: 2016-5-9 22:57
标题: 应用于两轮平衡车上的卡尔曼滤波程序
struct kalman
{
      float       KalmanFilterGain;                //卡尔曼增益
      float       CurrentEstimateValue;       //当前估计值
      float       CurrentEstimateCovariance;       //当前估计协方差
      float       CurrentMeasureCovariance;       //当前测量协方差
}Kalman_Filter;

void InitialKalmanFilter()
{
      Kalman_Filter.CurrentEstimateValue = 0;
      Kalman_Filter.CurrentEstimateCovariance = 0.1;
      Kalman_Filter.CurrentMeasureCovariance = 0.03;
}

float KalmanFilter(float InputValue)
{
      Kalman_Filter.KalmanFilterGain = Kalman_Filter.CurrentEstimateCovariance *
                                                                     sqrt(1 / (Kalman_Filter.CurrentEstimateCovariance * Kalman_Filter.CurrentEstimateCovariance +
                                                                     Kalman_Filter.CurrentMeasureCovariance * Kalman_Filter.CurrentMeasureCovariance));
      Kalman_Filter.CurrentEstimateValue = Kalman_Filter.CurrentEstimateValue + Kalman_Filter.KalmanFilterGain * (InputValue -
                                                                              Kalman_Filter.CurrentEstimateValue);
      Kalman_Filter.CurrentEstimateCovariance = sqrt(1 - Kalman_Filter.KalmanFilterGain) * Kalman_Filter.CurrentEstimateCovariance;
      Kalman_Filter.CurrentMeasureCovariance = sqrt(1 - Kalman_Filter.KalmanFilterGain) * Kalman_Filter.CurrentMeasureCovariance;

      return Kalman_Filter.CurrentEstimateValue;
}以上是小弟我写的一个卡尔曼滤波程序，主要是用在SISO系统中也就是我要把这段函数写到我的两轮平衡车里面实现角度的卡尔曼滤波，不知是否正确，InputValue就是传感器采集到的角度值。请各位大虾帮小弟看看。

欢迎光临中科因仑“3+1”工程特种兵精英论坛 (http://bbs.enlern.com/)