一, 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 是处理过后的数据.
来源: http://www.bubuko.com/infodetail-3089023.html