卡尔曼滤波算法及C语言实现(转载)

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/sinat_20265495/article/details/51006311

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

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

  
  

    
    

     
     

    
    
    
    

     
     

      
      #include "stdio.h"
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
      #include "stdlib.h"
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
      #include "math.h"
     
     
    
    

    
    

     
     

    
    
    
    

     
     
 
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
      double frand()
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
      {
     
     
    
    

    
    

     
     

    
    
    
    

     
     
    
      
      return 
      
      2 * ((rand() / (
      
      double)RAND_MAX) - 
      
      0.5);
      
      //随机噪声
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
      }
     
     
    
    

    
    

     
     

    
    
    
    

     
     
 
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
      void main()
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
      {
     
     
    
    

    
    

     
     

    
    
    
    

     
     
 
      
      float x_last = 
      
      0;
     
     
    
    

    
    

     
     

    
    
    
    

     
     
    
      
      float p_last = 
      
      0.02;
     
     
    
    

    
    

     
     

    
    
    
    

     
     
 
      
      float Q = 
      
      0.018;
     
     
    
    

    
    

     
     

    
    
    
    

     
     
 
      
      float R = 
      
      0.542;
     
     
    
    

    
    

     
     

    
    
    
    

     
     
 
      
      float kg;
     
     
    
    

    
    

     
     

    
    
    
    

     
     
 
      
      float x_mid;
     
     
    
    

    
    

     
     

    
    
    
    

     
     
  
      
      float x_now;
     
     
    
    

    
    

     
     

    
    
    
    

     
     
  
      
      float p_mid;
     
     
    
    

    
    

     
     

    
    
    
    

     
     
  
      
      float p_now;
     
     
    
    

    
    

     
     

    
    
    
    

     
     
  
      
      float z_real = 
      
      0.56;
     
     
    
    

    
    

     
     

    
    
    
    

     
     
 
      
      float z_measure;
     
     
    
    

    
    

     
     

    
    
    
    

     
     
  
      
      float summerror_kalman = 
      
      0;
     
     
    
    

    
    

     
     

    
    
    
    

     
     
  
      
      float summerror_measure = 
      
      0;
     
     
    
    

    
    

     
     

    
    
    
    

     
     
 
      
      int i;
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
          x_last = z_real + frand()*
      
      0.03;
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
          x_mid = x_last;
     
     
    
    

    
    

     
     

    
    
    
    

     
     
 
      
      for (i = 
      
      0; i < 
      
      20;i++)
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
        {
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
             x_mid = x_last;
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
           p_mid = p_last + Q;
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
           kg = p_mid / (p_mid + R);
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
             z_measure = z_real + frand()*
      
      0.03;
      
      //测量值
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
            x_now = x_mid + kg*(z_measure - x_mid);
      
      //估计出的最有值
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
            p_now = (
      
      1 - kg)*p_mid;
      
      //最优值对应的协方差
     
     
    
    

    
    

     
     

    
    
    
    

     
     
 
     
     
    
    

    
    

     
     

    
    
    
    

     
     
      
      
      printf(
      
      "Real position:%6.3f\n", z_real);
     
     
    
    

    
    

     
     

    
    
    
    

     
     
        
      
      printf(
      
      "Measure position:%6.3f [diff:%.3f]\n", z_measure, 
      
      fabs(z_real - z_measure));
     
     
    
    

    
    

     
     

    
    
    
    

     
     
     
      
      printf(
      
      "Kalman position: %6.3f [diff:%.3f]\n", x_now, 
      
      fabs(z_real - x_now));
     
     
    
    

    
    

     
     

    
    
    
    

     
     
     
      
      printf(
      
      "\n\n");
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
           summerror_kalman += 
      
      fabs(z_real - x_now);
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
              summerror_measure += 
      
      fabs(z_real - z_measure);
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
             p_last = p_now;
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
           x_last = x_now;
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
       }
     
     
    
    

    
    

     
     

    
    
    
    

     
     
   
      
      printf(
      
      "总体测量误差      :%f\n", summerror_measure);
     
     
    
    

    
    

     
     

    
    
    
    

     
     
 
      
      printf(
      
      "总体卡尔曼滤波误差:%f\n", summerror_kalman);
     
     
    
    

    
    

     
     

    
    
    
    

     
     
 
      
      printf(
      
      "卡尔曼误差所占比例:%d%%\n", 
      
      100 - (
      
      int)((summerror_kalman / summerror_measure) * 
      
      100));
     
     
    
    

    
    

     
     

    
    
    
    

     
     
 
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
       getchar();
     
     
    
    

    
    

     
     

    
    
    
    

     
     

      
      }
     
     
    
    
  
  

运行结果：

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