三、卡尔曼滤波器算法的原理。
说白了,卡尔曼滤波器就是五个公式达到滤波的效果的:
(1)预测当前系统状态的公式
X(k|k-1)=A X(k-1|k-1)+B U(k)
(2)对应当前系统状态值的协方差(covariance)
P(k|k-1)=A P(k-1|k-1) A’+Q
(3)计算卡尔曼增益Kg的公式
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
(4)得到当前状态的最优估算值
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))
(5)对应当前状态最优估算值的协方差
P(k|k)=(I-Kg(k) H)P(k|k-1)
对了,除了这五个公式还需引入一个有关系统的线性随机微分方程:
X(k)=A X(k-1)+B U(k)+W(k)
以及系统的测量值方程:
Z(k)=H X(k)+V(k)
这就是神奇的卡尔曼滤波的核心东西了,理解了这五个公式也就理解了卡尔曼滤波器的原理了。
四、一个卡尔曼滤波器的简单例子。
这里我们结合第二第三节,举一个非常简单的例子来说明卡尔曼滤波器的工作过程。所举的例子是进一步描述第二节的例子,而且还会配以程序模拟结果。
根据第二节的描述,把房间看成一个系统,然后对这个系统建模。当然,我们见的模型不需要非常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的,所以A=1。没有控制量,所以U(k)=0。因此得出:
X(k|k-1)=X(k-1|k-1) ……….. (6)
式子(2)可以改成:
P(k|k-1)=P(k-1|k-1) +Q ……… (7)
因为测量的值是温度计的,跟温度直接对应,所以H=1。式子3,4,5可以改成以下:
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)
P(k|k)=(1-Kg(k))P(k|k-1) ……… (10)
现在我们模拟一组测量值作为输入。假设房间的真实温度为25度,我模拟了200个测量值,这些测量值的平均值为25度,但是加入了标准偏差为几度的高斯白噪声(在图中为蓝线)。
为了令卡尔曼滤波器开始工作,我们需要告诉卡尔曼两个零时刻的初始值,是X(0|0)和P(0|0)。他们的值不用太在意,随便给一个就可以了,因为随着卡尔曼的工作,X会逐渐的收敛。但是对于P,一般不要取0,因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的,从而使算法不能收敛。我选了X(0|0)=1度,P(0|0)=10。
该系统的真实温度为25度,图中用黑线表示。图中红线是卡尔曼滤波器输出的最优化结果(该结果在算法中设置了Q=1e-6,R=1e-1)。
所以到此,卡尔曼滤波器就介绍的差不多了,相信你应该已经理解卡尔曼滤波器喽。
(什么,你还不明白? 那好吧,下面是一个流传在网上的卡尔曼滤波器的C源码,你拿去吧。)
//卡尔曼滤波---------------------------------------------------------
float Q_angle=0.001;//0.001
float Q_gyro=0.003;//0.003
float R_angle=0.5;//0.5
float dt=0.014;//0.1 //dt为kalman滤波器采样时间;
char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
//卡尔曼函数------------------------------------------------------
float Kalman_Filter(float Accel,float Gyro)//输入angleAx 和 gyroGy
{
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; //后验估计
Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
return Angle;
}
|