国外找的卡尔曼滤波算法，程序无改动，供大家学习

国外捞回来的一个卡尔曼滤波的栗子，学习了一下
程序解释：
程序中的Q是系统噪声，R是测量噪声，大概意思就是说该信谁多一点，如果Q=0就最后完全信预测结果，R=0则完全信测量结果

1. /** A simple kalman filter example by Adrian Boeing
   
2. www.adrianboeing.com
   
3. */
   
4. 
   
5. #include <stdio.h>
   
6. #include <stdlib.h>
   
7. #include <math.h>
   
8. 
   
9. double frand() {
   
10.     return 2*((rand()/(double)RAND_MAX) - 0.5);
    
11. }
    
12. 
    
13. int main() {
    
14. 
    
15.     //initial values for the kalman filter
    
16.     float x_est_last = 0;
    
17.     float P_last = 0;
    
18.     //the noise in the system
    
19.     float Q = 0.022;
    
20.     float R = 0.617;
    
21.    
    
22.     float K;
    
23.     float P;
    
24.     float P_temp;
    
25.     float x_temp_est;
    
26.     float x_est;
    
27.     float z_measured; //the 'noisy' value we measured
    
28.     float z_real = 0.5; //the ideal value we wish to measure
    
29.    
    
30.     srand(0);
    
31.    
    
32.     //initialize with a measurement
    
33.     x_est_last = z_real + frand()*0.09;
    
34.    
    
35.     float sum_error_kalman = 0;
    
36.     float sum_error_measure = 0;
    
37.    
    
38.     for (int i=0;i<30;i++) {
    
39.         //do a prediction
    
40.         x_temp_est = x_est_last;
    
41.         P_temp = P_last + Q;
    
42.         //calculate the Kalman gain
    
43.         K = P_temp * (1.0/(P_temp + R));
    
44.         //measure
    
45.         z_measured = z_real + frand()*0.09; //the real measurement plus noise
    
46.         //correct
    
47.         x_est = x_temp_est + K * (z_measured - x_temp_est);
    
48.         P = (1- K) * P_temp;
    
49.         //we have our new system
    
50.         
    
51.         printf("Ideal    position: %6.3f \n",z_real);
    
52.         printf("Mesaured position: %6.3f [diff:%.3f]\n",z_measured,fabs(z_real-z_measured));
    
53.         printf("Kalman   position: %6.3f [diff:%.3f]\n",x_est,fabs(z_real - x_est));
    
54.         
    
55.         sum_error_kalman += fabs(z_real - x_est);
    
56.         sum_error_measure += fabs(z_real-z_measured);
    
57.         
    
58.         //update our last's
    
59.         P_last = P;
    
60.         x_est_last = x_est;
    
61.     }
    
62.    
    
63.     printf("Total error if using raw measured:  %f\n",sum_error_measure);
    
64.     printf("Total error if using kalman filter: %f\n",sum_error_kalman);
    
65.     printf("Reduction in error: %d%% \n",100-(int)((sum_error_kalman/sum_error_measure)*100));
    
66.    
    
67.    
    
68.     return 0;
    
69. }//

复制代码

感谢小编，顶上去；

这个算法很是经典，下来保留。

不是很懂，先拿走再说

谢谢，不知您是否有开源的导航算法代码?

真不是很懂，很多要学习啊

谢谢你无私的分享！

这个好，支持支持。