卡尔曼滤波C语言实现

article/2025/10/27 22:34:00

卡尔曼5条基本公式，参考https://wenku.baidu.com/view/8523cb6eaf1ffc4ffe47ac24.html

#include "stdio.h"
#include "stdlib.h"
#include "math.h"#define kal_Q 0.001  /*过程噪声协方差,Q增大，动态响应变快，收敛稳定性变坏*/
#define kal_R 0.542  /*观测噪声协方差,R增大，动态响应变慢，收敛稳定性变好*/double frand()
{return 2 * ((rand() / (double)RAND_MAX) - 0.5);  /*随机噪声 */
}/*Measure 原始测量数据op_flg  0 仅初始化参数  1 执行卡尔曼滤波
*/
double Kalman(double measure,int op_flg)
{double x_mid,kg,p_mid;static double x_last = 0,p_last = 0;double x_now,p_now;if(op_flg)         /*非初始化*/{x_mid = x_last;p_mid = p_last + kal_Q;kg = p_mid / (p_mid + kal_R);x_now = x_mid + kg * (measure - x_mid);p_now = (1 - kg)*p_mid;p_last = p_now;x_last = x_now;}else{x_last = measure;p_last = kal_Q;}return x_now;
}void main()
{float z_real = 0.56;float z_measure,x_now;float summerror_kalman = 0;float summerror_measure = 0;int i;Kalman(z_real,0);for (i = 0; i < 20;i++){z_measure = z_real + frand()*0.05;//测量值x_now = Kalman(z_measure,1);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);}printf("总体测量误差      :%f\n", summerror_measure);printf("总体卡尔曼滤波误差:%f\n", summerror_kalman);printf("卡尔曼滤波效果所占比例:%d%%\n", 100 - (int)((summerror_kalman / summerror_measure) * 100));getchar();
}