卡尔曼滤波--用于对采集到的传感器数据，进行滤波

卡尔曼滤波的原理和公式，这里就不介绍了，网上百度下就有很多帖子在讲，本片文章主要是给大家提供一个简单的c语言实现的用于对对采集到的传感器数据，进行滤波的程序
大家有需要的可以放到自己的项目里使用。喜欢的朋友可以订阅哟，后续会有更多成熟的算法程序会放在专栏里。

#include "stdio.h"
#include "stdlib.h"
#include "math.h"
 
#define kal_Q 0.001  /*过程噪声协方差,Q增大，动态响应变快，收敛稳定性变坏*/
#define kal_R 0.36  /*观测噪声协方差,R增大，动态响应变慢，收敛稳定性变好*/
 
/*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[20]={5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5} ;//此处替换成实际传感器采集数据,采集数据填充可以做成滑动的，先入先出
	float z_measure,x_now;
	int i;
	Kalman(z_real,0);
	for (i = 0; i < 20;i++)
	{
		z_measure = z_real[i]; //传感器当前采集值
		x_now = Kalman(z_measure,1);
	}
}

效果可以通过调整Q和R的值来调节，
#define kal_Q 0.001 /过程噪声协方差,Q增大，动态响应变快，收敛稳定性变坏/
#define kal_R 0.36 /观测噪声协方差,R增大，动态响应变慢，收敛稳定性变好/