卡尔曼滤波函数,可直接使用,根据需求更改参数。
#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就是要过滤的值
}