卡尔曼滤波

#include "kalman.h"

//#define KALMAN_DEMO_USE

/** 
*卡尔曼滤波器 
*@param KFP *kfp 卡尔曼结构体参数 * float input 需要滤波的参数的测量值（即传感器的采集值） 
*@return 滤波后的参数（最优值） 
*@note r值固定，q值越大，代表越信任测量值，q值无穷大，代表只用测量值
*				q值越小，代表越信任模型预测值，q值为0，代表只用模型预测值。
*				q:过程噪声，q增大，动态响应变快，收敛稳定性变坏；反之，控制误差
* 			r：测量噪声，r增大，动态响应变慢，收敛稳定性变好；反之，控制响应速度
*/
float kalmanFilter(KFP *kfp,float input)
 { 
     //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
     kfp->Now_P = kfp->LastP + kfp->Q;
     //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
     kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
     //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
     kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
     //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
     kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
     return kfp->out;
 }

/* * *调用卡尔曼滤波器 实践 */
#ifdef KALMAN_DEMO_USE
KFP KFP_height={ 
   0.02,0,0,0,0.001,0.543};
   
void demo(void)
{
	int intput = 1;
	int kalman_height=0;	
	kalman_height = kalmanFilter(&KFP_height,(float)intput);
}
#endif

#ifndef __KALMAN_H__
#define __KALMAN_H__

//1. 结构体类型定义
typedef struct 
{ 
   
    float LastP;//上次估算协方差 初始化值为0.02
    float Now_P;//当前估算协方差 初始化值为0
    float out;//卡尔曼滤波器输出 初始化值为0
    float Kg;//卡尔曼增益 初始化值为0
    float Q;//过程噪声协方差 初始化值为0.001
    float R;//观测噪声协方差 初始化值为0.543
}KFP;//Kalman Filter parameter


float kalmanFilter(KFP *kfp,float input);

#endif