卡尔曼滤波算法推导卡尔曼滤波算法.doc

文档介绍：卡尔曼滤波算法推导 卡尔曼滤波算法
卡尔曼滤波器的介绍
为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。
到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance还没更新。我们用P表示covariance:
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
式(2)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的covariance，A’表示A
的转置矩阵，Q是系统过程的covariance。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。
现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们
可以得到现在状态(k)的最优化估算值X(k|k):
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3) 其中Kg为卡尔曼增益(Kalman Gain):
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4) 到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance:
P(k|k)=(I-Kg(k) H)P(k|k-1) ……… (5)
其中I为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就
5
可以自回归的运算下去。 使用C语言编程实现(核心算法)。
程序1
x_mid=x_last;//x_last=x(k-1|k-1),x_mid=x(k|k-1)
p_mid=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为噪声 z_measure=z_real+frand()*;//测量值
x_now=x_mid+kg*(z_measure-x_mid);//估计出的最优值 p_now=(1-kg)*p_mid;//最优值对应的covariance
p_last = p_now;//更新covariance值
x_last = x_now;//更新系统状态值
#include
#include
#include
double frand()
{ return 2*((rand()/(double)RAND_MAX) - ); //随机噪声} void main()
{
float x_last=0;
float p_last=;
float Q=;
float R=;
float kg;
6
float x_mid;
float x_now;
float p_mid;
float p_now;
float z_real=;//
float z_measure;
float sumerror_kalman=0;
float sumerror_measure=0;
int i;
x_last=z_real+frand()*;
x_mid=x_last;
for(i=0;i{
x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
p_mid=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为噪声 z_measure=z_real+frand()*;//测量值
x_now=x_mid+kg*(z_measure-x_mid);//估计出的最优值 p_now=(1-kg)*p_mid;//最优值对应的covariance
printf(
[diff:%.3f]\n
printf(
7
[diff:%.3f]\n
sumerror_kalman += fabs(z_real -