DSP

卡尔曼滤波算法及C语言实现

2019-07-13 17:38发布

站内文章 / DSP
12778 0 1148
摘自：http://blog.csdn.net/sinat_20265495/article/details/51006311kalman filter最为核心的内容是体现它最优化估计和递归特点的5条公式。 下面利用C语言编程实现Kalman Filter Algorithm，代码如下：[cpp] view plain copy
  1. #include "stdio.h"  
  2. #include "stdlib.h"  
  3. #include "math.h"  
  4.   
  5. double frand()  
  6. {  
  7.     return 2 * ((rand() / (double)RAND_MAX) - 0.5);//随机噪声  
  8. }  
  9.   
  10. void main()  
  11. {  
  12.     float x_last = 0;  
  13.     float p_last = 0.02;  
  14.     float Q = 0.018;  
  15.     float R = 0.542;  
  16.     float kg;  
  17.     float x_mid;  
  18.     float x_now;  
  19.     float p_mid;  
  20.     float p_now;  
  21.     float z_real = 0.56;  
  22.     float z_measure;  
  23.     float summerror_kalman = 0;  
  24.     float summerror_measure = 0;  
  25.     int i;  
  26.     x_last = z_real + frand()*0.03;  
  27.     x_mid = x_last;  
  28.     for (i = 0; i < 20;i++)  
  29.     {  
  30.         x_mid = x_last;  
  31.         p_mid = p_last + Q;  
  32.         kg = p_mid / (p_mid + R);  
  33.         z_measure = z_real + frand()*0.03;//测量值  
  34.         x_now = x_mid + kg*(z_measure - x_mid);//估计出的最有值  
  35.         p_now = (1 - kg)*p_mid;//最优值对应的协方差  
  36.   
  37.         printf("Real position:%6.3f ", z_real);  
  38.         printf("Measure position:%6.3f [diff:%.3f] ", z_measure, fabs(z_real - z_measure));  
  39.         printf("Kalman position: %6.3f [diff:%.3f] ", x_now, fabs(z_real - x_now));  
  40.         printf(" ");  
  41.         summerror_kalman += fabs(z_real - x_now);  
  42.         summerror_measure += fabs(z_real - z_measure);  
  43.         p_last = p_now;  
  44.         x_last = x_now;  
  45.     }  
  46.     printf("总体测量误差      :%f ", summerror_measure);  
  47.     printf("总体卡尔曼滤波误差:%f ", summerror_kalman);  
  48.     printf("卡尔曼误差所占比例:%d%% ", 100 - (int)((summerror_kalman / summerror_measure) * 100));  
  49.   
  50.     getchar();  
  51. }  

热门文章