Kalman滤波算法测试
一、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是处理过后的数据。
posted on 2019-06-11 10:47 LewisMountain 阅读(...) 评论(...) 编辑 收藏