EEPW论坛

1 引言

Kalman Filter是一个高效的递归滤波器，它可以实现从一系列的噪声测量中，估计动态系统的状态。

起源于Rudolf Emil Kalman在1960年的博士论文和发表的论文《A New Approach to Linear Eiltering and Prediction Problems》（《线性滤波与预测问题的新方法》）。并且最先在阿波罗登月计划轨迹预测上应用成功，此后kalman filter取得重****展和完善。

它的广泛应用已经超过30年，包括机器人导航，控制。传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等，近年来更被广泛应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

2 实现

平衡小车处理角度     

/**************************************************************************
函数功能：简易卡尔曼滤波
入口参数：加速度、角速度
返回  值：无
**************************************************************************/
void Kalman_Filter(float Accel,float Gyro)
{
angle+=(Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=-PP[1][1];
Pdot[2]=-PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - angle;//zk-先验估计
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
angle+= K_0 * Angle_err; //后验估计
Q_bias+= K_1 * Angle_err; //后验估计
angle_dot   = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}