[关闭]
@zwh8800 2016-04-30T01:33:21.000000Z 字数 859 阅读 327106

卡尔曼滤波的实现

blog 算法


卡尔曼滤波在数据采集中十分有用。本文介绍一下卡尔曼滤波的理论,然后给出一个实现。

具体的理论性介绍可以在 这里 找到

具体来说对于一维的卡尔曼滤波, 如果认为控制量为零的话卡尔曼滤波公式可以简化为以下 5 条:

X(k|k-1)=X(k-1|k-1) ……… (6)
P(k|k-1)=P(k-1|k-1) +Q ……… (7)
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)
P(k|k)=(1-Kg(k))P(k|k-1) ……… (10)

其中, X(k|k-1) 这次的估计值, X(k-1|k-1) 是上次的最优值 (也就是上次计算结果)

由于我们没有控制量, 所以假设这次的值和上次的相等, 所以令第 k 次估计值 = 第 k-1 次最优值

公式 7 中 P(k|k-1) 是这次估计值的偏差 (协方差), 令其等于上次最优值的偏差加上一个参数 (因为系统总是越运行偏差越大, 所以每次都加上一个 Q)

X(k|k)是本次的最优值 (也就是结果), Z(k) 是本次的测量值, Kg 是卡尔曼增益, 根据 (9) 计算出

由 (8), (9) 可以计算出结果

另外, 需要计算出这次最优值的偏差, 以便下次回归 (递归)

代码写出来很简单, 但是完成的功能却很强大:

  1. volatile uint16_t adc_values[3]; //三个前方传感器
  2. #define Q 1e-6f
  3. #define R 1e-1f
  4. volatile float kalman_value[3];
  5. volatile float kalman_p[3];
  6. void kalman_filter()
  7. {
  8. int i;
  9. float p;
  10. float x;
  11. float kg;
  12. for (i = 0; i < 3; ++i)
  13. {
  14. x = kalman_value[i]; //(6)
  15. p = kalman_p[i] + Q; //(7)
  16. kg = p / (p + R); //(9)
  17. kalman_p[i] = (1 - kg) * p; //(10)
  18. kalman_value[i] = x + kg * (adc_values[i] - x); //(8)
  19. }
  20. }
添加新批注
在作者公开此批注前,只有你和作者可见。
回复批注