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

ti

国外捞回来的一个卡尔曼滤波的栗子，学习了一下

程序解释：
程序中的Q是系统噪声，R是测量噪声，大概意思就是说该信谁多一点，如果Q=0就最后完全信预测结果，R=0则完全信测量结果

/** A simple kalman filter example by Adrian Boeing
www.adrianboeing.com
*/
#include
#include
#include
double frand() {
return 2*((rand()/(double)RAND_MAX) - 0.5);
}
int main() {
//initial values for the kalman filter
float x_est_last = 0;
float P_last = 0;
//the noise in the system
float Q = 0.022;
float R = 0.617;
float K;
float P;
float P_temp;
float x_temp_est;
float x_est;
float z_measured; //the 'noisy' value we measured
float z_real = 0.5; //the ideal value we wish to measure
srand(0);
//initialize with a measurement
x_est_last = z_real + frand()*0.09;
float sum_error_kalman = 0;
float sum_error_measure = 0;
for (int i=0;i<30;i++) {
//do a prediction
x_temp_est = x_est_last;
P_temp = P_last + Q;
//calculate the Kalman gain
K = P_temp * (1.0/(P_temp + R));
//measure
z_measured = z_real + frand()*0.09; //the real measurement plus noise
//correct
x_est = x_temp_est + K * (z_measured - x_temp_est);
P = (1- K) * P_temp;
//we have our new system
printf("Ideal position: %6.3f n",z_real);
printf("Mesaured position: %6.3f [diff:%.3f]n",z_measured,fabs(z_real-z_measured));
printf("Kalman position: %6.3f [diff:%.3f]n",x_est,fabs(z_real - x_est));
sum_error_kalman += fabs(z_real - x_est);
sum_error_measure += fabs(z_real-z_measured);
//update our last's
P_last = P;
x_est_last = x_est;
}
printf("Total error if using raw measured: %fn",sum_error_measure);
printf("Total error if using kalman filter: %fn",sum_error_kalman);
printf("Reduction in error: %d%% n",100-(int)((sum_error_kalman/sum_error_measure)*100));
return 0;
}//