卡尔曼滤波算法--C语言实现

/**
  ******************************************************************************
  * @brief  卡尔曼滤波器 函数
  * @param  inData - 输入值
  * @return 滤波后的值
  * @note   r值固定，q值越大，代表越信任测量值，q值无穷大，代表只用测量值。
  *                  q值越小，代表越信任模型预测值，q值为0，代表只用模型预测值。
  *         q:过程噪声，q增大，动态响应变快，收敛稳定性变坏；反之。控制误差 
  *         r:测量噪声，r增大，动态响应变慢，收敛稳定性变好；反之。控制响应速度
  ******************************************************************************
  */
unsigned long KalmanFilter(unsigned long inData)
{
    static float xdata kalman = 0; //上次卡尔曼值(估计出的最优值)
    static float xdata p = 10;
    float xdata q = 0.001; //q:过程噪声
    float xdata r = 0.001; //r:测量噪声
    float xdata kg = 0; //kg:卡尔曼增益
 
    p += q;
    kg = p / ( p + r ); //计算卡尔曼增益
    kalman = kalman + (kg * (inData - kalman)); //计算本次滤波估计值
    p = (1 - kg) * p; //更新测量方差
    
    return (unsigned long)kalman; //返回估计值
}

实际应用

#include "stdio.h"
#include "stdlib.h"
#include "math.h"

double frand()
{
	return 2 * ((rand() / (double)RAND_MAX) - 0.5);//随机噪声
}

void main()
{
	float x_last = 0;
	float p_last = 0.02;
	float Q = 0.018;
	float R = 0.542;
	float kg;
	float x_mid;
	float x_now;

	float p_mid;
	float p_now;
	float z_real = 0.56;
	float z_measure;
	float summerror_kalman = 0;
	float summerror_measure = 0;

	int i;

	x_last = z_real + frand()*0.03;

	x_mid = x_last;

	for (i = 0; i < 20; i++)
	{
		x_mid = x_last;
		p_mid = p_last + Q;
		kg = p_mid / (p_mid + R);
		z_measure = z_real + frand()*0.03;//测量值
		x_now = x_mid + kg * (z_measure - x_mid);//估计出的最有值
		p_now = (1 - kg)*p_mid;//最优值对应的协方差

		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);
		p_last = p_now;
		x_last = x_now;
	}

	printf("总体测量误差 :%f\n", summerror_measure);

	printf("总体卡尔曼滤波误差:%f\n", summerror_kalman);

	printf("卡尔曼误差所占比例:%d%%\n", 100 - (int)((summerror_kalman / summerror_measure) * 100));

	getchar();

}

输出结果：

Real position: 0.560
Measure position: 0.564 [diff:0.004]
Kalman position:  0.532 [diff:0.028]


Real position: 0.560
Measure position: 0.542 [diff:0.018]
Kalman position:  0.533 [diff:0.027]


Real position: 0.560
Measure position: 0.579 [diff:0.019]
Kalman position:  0.538 [diff:0.022]


Real position: 0.560
Measure position: 0.565 [diff:0.005]
Kalman position:  0.541 [diff:0.019]


Real position: 0.560
Measure position: 0.559 [diff:0.001]
Kalman position:  0.544 [diff:0.016]


Real position: 0.560
Measure position: 0.551 [diff:0.009]
Kalman position:  0.545 [diff:0.015]


Real position: 0.560
Measure position: 0.584 [diff:0.024]
Kalman position:  0.551 [diff:0.009]


Real position: 0.560
Measure position: 0.579 [diff:0.019]
Kalman position:  0.555 [diff:0.005]


Real position: 0.560
Measure position: 0.575 [diff:0.015]
Kalman position:  0.558 [diff:0.002]


Real position: 0.560
Measure position: 0.540 [diff:0.020]
Kalman position:  0.555 [diff:0.005]


Real position: 0.560
Measure position: 0.582 [diff:0.022]
Kalman position:  0.560 [diff:0.000]


Real position: 0.560
Measure position: 0.573 [diff:0.013]
Kalman position:  0.562 [diff:0.002]


Real position: 0.560
Measure position: 0.561 [diff:0.001]
Kalman position:  0.562 [diff:0.002]


Real position: 0.560
Measure position: 0.548 [diff:0.012]
Kalman position:  0.559 [diff:0.001]


Real position: 0.560
Measure position: 0.531 [diff:0.029]
Kalman position:  0.555 [diff:0.005]


Real position: 0.560
Measure position: 0.535 [diff:0.025]
Kalman position:  0.552 [diff:0.008]


Real position: 0.560
Measure position: 0.552 [diff:0.008]
Kalman position:  0.552 [diff:0.008]


Real position: 0.560
Measure position: 0.539 [diff:0.021]
Kalman position:  0.549 [diff:0.011]


Real position: 0.560
Measure position: 0.540 [diff:0.020]
Kalman position:  0.548 [diff:0.012]


Real position: 0.560
Measure position: 0.589 [diff:0.029]
Kalman position:  0.555 [diff:0.005]


总体测量误差 :0.312517
总体卡尔曼滤波误差:0.200942
卡尔曼误差所占比例:36%