kalman filter最为核心的内容是体现它最优化估计和递归特点的5条公式。

下面利用C语言编程实现Kalman Filter Algorithm，代码如下：

1. #include "stdio.h"
2. #include "stdlib.h"
3. #include "math.h"
4. double frand()
5. {
6. return 2 * ((rand() / (double)RAND_MAX) - 0.5);//随机噪声
7. }
8. void main()
9. {
10. float x_last = 0;
11. float p_last = 0.02;
12. float Q = 0.018;
13. float R = 0.542;
14. float kg;
15. float x_mid;
16. float x_now;
17. float p_mid;
18. float p_now;
19. float z_real = 0.56;
20. float z_measure;
21. float summerror_kalman = 0;
22. float summerror_measure = 0;
23. int i;
24. x_last = z_real + frand()*0.03;
25. x_mid = x_last;
26. for (i = 0; i < 20;i++)
27. {
28. x_mid = x_last;
29. p_mid = p_last + Q;
30. kg = p_mid / (p_mid + R);
31. z_measure = z_real + frand()*0.03;//测量值
32. x_now = x_mid + kg*(z_measure - x_mid);//估计出的最有值
33. p_now = (1 - kg)*p_mid;//最优值对应的协方差
34. printf("Real position:%6.3f\n", z_real);
35. printf("Measure position:%6.3f [diff:%.3f]\n", z_measure, fabs(z_real - z_measure));
36. printf("Kalman position: %6.3f [diff:%.3f]\n", x_now, fabs(z_real - x_now));
37. printf("\n\n");
38. summerror_kalman += fabs(z_real - x_now);
39. summerror_measure += fabs(z_real - z_measure);
40. p_last = p_now;
41. x_last = x_now;
42. }
43. printf("总体测量误差 :%f\n", summerror_measure);
44. printf("总体卡尔曼滤波误差:%f\n", summerror_kalman);
45. printf("卡尔曼误差所占比例:%d%%\n", 100 - (int)((summerror_kalman / summerror_measure) * 100));
46. getchar();
47. }

运行结果：

转载自：https://blog.csdn.net/sinat_20265495/article/details/51006311