Kalman Filter 推导.doc

Kalman Filter(卡尔曼滤波)的推导 基本动态系统模型 卡尔曼滤波建立在HYPERLINK /wiki/线性代数线性代数和HYPERLINK /wiki/隐马尔可夫模型隐马尔可夫模型(hidden Markov model)上。其基本动态系统可以用一个HYPERLINK /wiki/马尔可夫链马尔可夫链表示，该马尔可夫链建立在一个被高斯HYPERLINK /wiki/噪声噪声(即正态分布的噪声)干扰的HYPERLINK /wiki/线性算子线性算子上的。系统的HYPERLINK /wiki/状态空间状态可以用一个元素为实数的HYPERLINK /wiki/向量向量表示。 随着HYPERLINK /wiki/离散时间信号离散时间的每一个增加,这个HYPERLINK /wiki/线性算子线性算子就会作用在当前HYPERLINK /wiki/状态状态上，产生一个新的状态,并也会带入一些噪声,同时系统的一些已知的控制器的控制信息也会被加入。同时，另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。 为了从一系列有噪声的观察数据中用卡尔曼滤波器估计出被观察过程的内部状态，我们必须把这个过程在卡尔曼滤波的框架下建立模型。也就是说对于每一步k，定义HYPERLINK /wiki/矩阵矩阵Fk, Hk, Qk, Rk，有时也需要定义Bk，如下。 卡尔曼滤波器的模型。圆圈代表HYPERLINK /wiki/向量向量，方块代表HYPERLINK /wiki/矩阵矩阵，星号代表HYPERLINK /wiki/高斯噪声高斯噪声，其HYPERLINK /wiki/协方差矩阵协方差矩阵在右下方标出。 卡尔曼滤波模型假设k时刻的真实状态是从(k ? 1)时刻的状态演化而来，符合下式： 其中 Fk是作用在xk?1上的状态变换模型（/矩阵/矢量）。 Bk是作用在控制器向量uk上的输入－控制模型。 wk是过程噪声，并假定其符合均值为零，HYPERLINK /wiki/协方差矩阵协方差矩阵为Qk的HYPERLINK /w/index.php?title=多元正态分布action=editredlink=1多元正态分布。 时刻k，对真实状态 xk的一个测量zk满足下式: 其中Hk是观测模型,它把真实状态空间映射成观测空间，vk 是观测噪声，其均值为零，协方差矩阵为Rk,且服从HYPERLINK /wiki/正态分布正态分布。 初始状态以及每一时刻的噪声{x0, w1, ..., wk, v1 ... vk} 都认为是互相HYPERLINK /w/index.php?title=统计学独立action=editredlink=1独立的. 实际上，很多真实世界的动态系统都并不确切的符合这个模型；但是由于卡尔曼滤波器被设计在有噪声的情况下工作,一个近似的符合已经可以使这个滤波器非常有用了。更多其它更复杂的卡尔曼滤波器的变种，在下边讨论中有描述。 卡尔曼滤波器 卡尔曼滤波是一种HYPERLINK /wiki/递归递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器与大多数滤波器不同之处，在于它是一种纯粹的HYPERLINK /wiki/時域时域滤波器，它不需要像HYPERLINK /wiki/低通滤波器低通滤波器等HYPERLINK /wiki/频域频域滤波器那样，需要在频域设计再转换到时域实现。 卡尔曼滤波器的状态由以下两个变量表示： ，在时刻k 的状态的估计； ，误差相关矩阵，度量估计值的精确程度。 卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。 预测 (预测状态) (预测估计协方差矩阵) 可参考：HYPERLINK /~welch/media/pdf/kalman_intro.pdf/~welch/media/pdf/kalman_intro.pdf 更新 首先要算出以下三个量： (测量余量，measurement residual) (测量余量协方差) (最优卡尔曼增益) 然后用它们来更新滤波器变量x 与P ： (更新的状态估计) (更新的协方差估计) 使用上述公式计算仅在最优卡尔曼增益的时候有效。使用其他增益的话，公式要复杂一些，请参见HYPERLINK /wiki/卡尔曼滤波#.E6.8E.A8.E5.AF.BC推导。 不变量(Invariant) 如果模型准确