嵌入式C--卡尔曼滤波算法（已封装，可直接使用）

卡尔曼滤波函数，可直接使用，根据需求更改参数。

#define     M_R 	        1.0f 
#define     P_Q 	        0.1f

KalamanFilter_Type  KalmanPara;

/**** 这条放在函数引用处 ****/
KalmanPara.p_last = 0.5f;
KalmanPara.x_last = 1000; //第一次过滤输入值

/********/

typedef struct
{
    float p_last;
    float x_last;
}KalamanFilter_Type;


float DrvAdc_KalmanFilter( KalamanFilter_Type  *gKalaman,float  Measure)
{
	//Q大R小，K增加；Q小R大，K减小
    double R = M_R;
    double Q = P_Q;

    double x_mid = gKalaman->x_last;
    double x_now;

    double p_mid;
    double p_now;

    double kg;

    //这里p_last 等于 kalmanFilter_A 的p直接取0
    x_mid=gKalaman->x_last;             //x_last=x(k-1|k-1),x_mid=x(k|k-1)
    p_mid=gKalaman->p_last+Q;           //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声

    //卡尔曼滤波的五个重要公式
    kg=p_mid/(p_mid+R);                 //kg为kalman filter，R为噪声
    x_now=x_mid+kg*(Measure-x_mid);     //估计出的最优值
    p_now=(1-kg)*p_mid;                 //最优值对应的covariance
    gKalaman->p_last = p_now;           //更新covariance值
    gKalaman->x_last = x_now;           //更新系统状态值

    return gKalaman->x_last;			//返回估计值
}

/**** 使用举例 ****/

void main(void){
    float filter_value = 0;
    filter_value = DrvAdc_KalmanFilter(&KalmanPara,adc_value); // adc_value就是要过滤的值

}