卡尔曼滤波算法程序,输入随机数，Q和R自己选

银魂

1. 卡尔曼滤波算法，自己，输入为随机数，Q和R要自己选
   
2. 主函数：
   
3. 
   
4. #include "kalman.h"
   
5. 
   
6. #include "stdio.h"
   
7. 
   
8. #include "stdlib.h"
   
9. 
   
10. void main(void)
    
11. {
    
12. 
    
13. 
    
14.         KalmanCountData k;
    
15.         //定义一个卡尔曼运算结构体
    
16.         Kalman_Filter_Init(&k);
    
17.         //讲运算变量初始化
    
18.         int n;        
    
19.         int a;
    
20.         srand((unsigned)time(NULL)); // 初始化随机种子，使rand()产生的随机数每次不一样
    
21.            for(a = 0;a<1000;a++)
    
22.         //测试1000次
    
23.         {
    
24.                 //m,n为1到100的随机数
    
25.                
    
26.                 n = 1+ rand() %100;
    
27.                 //卡尔曼滤波，传递2个测量值以及运算结构体
    
28.       
    
29.         Kalman_Filter(&k,n);
    
30. 
    
31.                 //打印结果
    
32.                 printf("第%d次迭代\n  输入：%d, 输出滤波值为：%f\r\n",a+1,n,k.Filter_Value);
    
33.       
    
34.         }
    
35. }
    
36. kalman.h:
    
37. typedef struct                    //定义结构体
    
38. {
    
39.         float                                Filter_Value;                 //K-1时刻的系滤波值
    
40.         float                                A;                           //系统参数，一维模型中通常选为1
    
41.         float                                H;                          //系统测量参数，一维模型选择为1
    
42.         float                                Kg;                        //卡尔曼增益
    
43.         float                                Q;                           //过程噪声偏差
    
44.         float                                 R;                          //测量噪声偏差
    
45.         float                                 P;                      //估计误差协方差     
    
46. } KalmanCountData;
    
47. 
    
48. void Kalman_Filter_Init(KalmanCountData * Kalman_Struct)             //卡尔曼滤波器初始化
    
49. {
    
50.     Kalman_Struct->A = 1;
    
51.     Kalman_Struct->H = 1;
    
52.     Kalman_Struct->P = 3;                //后验状态估计值方差的初始值
    
53.     Kalman_Struct->Q = 0.1;               //过程噪声偏差，实验测的 ，要填的！
    
54.     Kalman_Struct->R = 10;                 //测量噪声偏差，要填的！
    
55.     Kalman_Struct->Filter_Value = 0;                   //初始化设置k-1时刻的系统滤波后的值为0
    
56. }
    
57. double Kalman_Filter(KalmanCountData* Kalman_Struct, double Last_Measurement){
    
58.     double PredictValue = Kalman_Struct->Filter_Value*Kalman_Struct->A;     //先验估计的预测值
    
59.     Kalman_Struct->P = Kalman_Struct->A*Kalman_Struct->P*Kalman_Struct->A+Kalman_Struct->Q; //求协方差
    
60.     double prevalve = Kalman_Struct->Filter_Value;  //记录上一次（k-1）的值
    
61.     Kalman_Struct->Kg = Kalman_Struct->P*Kalman_Struct->H/(Kalman_Struct->H*Kalman_Struct->P*Kalman_Struct->H + Kalman_Struct->R);
    
62.     /*
    
63.         卡尔曼增益Kg = P(k|k-1) H’ / (H P(k|k-1) H’ + R)
    
64.     */
    
65.     Kalman_Struct->Filter_Value = PredictValue + (Last_Measurement-PredictValue) * Kalman_Struct->Kg;
    
66.     /*
    
67.         修正结果，更新预测值；X(k|k) =  X(k|k-1) + (Z(k) - H* X(k|k-1)) * Kg
    
68.     */
    
69.     Kalman_Struct->P = (1 - Kalman_Struct->Kg * Kalman_Struct->H) * Kalman_Struct->P;
    
70.     //更新后验估计的协方差，P(k|k) = (1 - Kg * H) * P(k|k-1)
    
71.     return Kalman_Struct->Filter_Value;
    
72. }

复制代码