卡尔曼滤波C语言实现

 卡尔曼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();
}

设定协方差 Q和R后，能滤除97%的噪声