Kalman滤波算法C语言实现以及演示

 一、Kalman滤波算法简介

卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。
1953，1954年于麻省理工学院分别获得电机工程学 士及硕士学位。
1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文
《ANewApproachtoLinearFilteringandPredictionProblems》（线性滤波与预测问题的新方法）。

卡尔曼滤波 Kalman filtering
一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

表达式 X(k)=A X(k-1)+B U(k)+W(k)

Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
R:测量噪声，R增大，动态响应变慢，收敛稳定性变好

二、C语言实现

　　基于LPC1768最小系统硬件平台，内部模拟产生正弦输入信号，通过配置不同的Kalman系数进行滤波测试。核心滤波算法C语言部分如下：

#if TKIT_KALMAN_EN
TYPE_KM fAlg_KalmanFilter (　　  const TYPE_KM SrcData,
　　　　　　　　　　　　　　　　const TYPE_KM ProcessNiose_Q,
　　　　　　　　　　　　　　　　const TYPE_KM MeasureNoise_R,
　　　　　　　　　　　　　　　　TYPE_KM * x_p_last,
　　　　　　　　　　　　　　　　TYPE_KM * p_p_last)
{
　　TYPE_KM x_last=*x_p_last;
　　TYPE_KM p_last=*p_p_last;

　　TYPE_KM x_mid, x_now;
　　TYPE_KM p_mid, p_now;
　　TYPE_KM kg;

　　x_mid = x_last;
　　p_mid = p_last+ProcessNiose_Q;
　　kg = p_mid/(p_mid+MeasureNoise_R);

　　x_now = x_mid+kg*(SrcData-x_mid);
　　p_now = (1-kg)*p_mid;

　　*p_p_last = p_now;
　　*x_p_last = x_now;

　　return x_now;
}
#endif //TKIT_KALMAN_EN

三、模拟输入和测试

　　模拟方法是通过配置不同的输入参数ProcessNiose_Q和MeasureNoise_R，以及内部模拟产生原始正弦信号。绘制原始数据以及经过滤波处理之后的数据进行对比，如下。

　　其中fx是输入的原始数据，fy是处理过后的数据。