以下是讲的比较细致的一个资料.http://jpkc.nwpu.edu.cn/jp2005/40/ebook/kcsj/chp1/Kalman.htm 1.卡尔曼滤波器算法 X(k)=A X(k-1)+B U(k)+W(k) 再加上系统的测量值: Z(k)=H X(k)+V(k) 上两式子中,X(k)是k时刻的系统状态,U(k)是k时刻对系统的控制量。A和B是系统参数,对于多模型系统,他们为矩阵。Z(k)是k时刻的测量 值,H是测量系统的参数,对于多测量系统,H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise),他们的covariance 分别是Q,R(这里我们假设他们不随系统状态变化而变化)。 X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1) 式(1)中,X(k|k-1)是利用上一状态预测的结果,X(k-1|k-1)是上一状态最优的结果,U(k)为现在状态的控制量,如果没有控制量,它可以为0。 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个公式当中的前两个,也就是对系统的预 测。 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)。这样,算法就可以自回归的运算下 去。卡尔曼滤波器的原理基本描述了,式子1,2,3,4和5就是他的5 个基本公式。根据这5个公式,可以很容易的实现计算机的程序。 2.简单例子 这里我们结合第二第三节,举一个非常简单的例子来说明卡尔曼滤波器的工作过程。所举的例子是进一步描述第二节的例子,而且还会配以程序模拟结果。 根据第二节的描述,把房间看成一个系统,然后对这个系统建模。当然,我们见的模型不需要非常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的,所以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) http://blog.csdn.net/luckydongbin/archive/2007/03/01/1518594.aspx Kalman滤波是个最优化递归处理算法 总结 在目标检测与跟踪过程中,为了提高跟踪速度,在确定目标的情况下,对目标在下一时刻的可能的位置进行估计,然后以这个估计的位置为中心,在一定的范围内进行目标的搜索,这样就可以缩小目标的搜索范围,提高跟踪速度. 1)最优( optimal )依赖于评价性能的判据。Kalman滤波器充分利用如下信息估计感兴趣变量当前取值:a.系统和测量装置的动态特性;b.系统噪声、测量误差和动态模型的不确定性的统计描述;c.感兴趣变量的初始条件的相关信息。(2)递归( recursive )是指kalman不需要保存先前的数据,当进行新的测量时也不需要对原来数据进行处理。 (3)filter(DPA)实际上是数据处理算法,只不过是计算中处理的程序,因此能处理离散时间测量样本,而不是连续时间输入。 • 基本假设 采用线性模型是合理的;这是典型工程模型在某些主要点或轨迹是线性的,线性模型比非线性模型更简单。因此用线性模型来近似。 白噪声意味着噪声值和时间不相关;白噪声指在整个频率上都有相同强度的频率特性的噪声。实际应用中将频率设为常值,带宽大大超过系统带宽的噪声称为白噪声,用高斯白噪声来模拟,可以大大简化模型。 采用高斯密度函数在实践上是可行的。因为采用高斯函数在数学上容易处理。当缺少高阶统计量时,除了假定高斯密度外,没有更好的可以表示的函数形式。用一阶和二阶统计量完全可以描述高斯白噪声。 (luckydongbin) |