Kalman Filter 推导

Kalman Filter(卡尔曼滤波)的推导 基本动态系统模型 卡尔曼滤波建立在线性代数和隐马尔可夫模型(hidden Markov model)上。其基本动态系统可以用一个马尔可夫链示，该马尔可夫链建立在一个被高斯噪声(即正态分布的噪声)干扰的线性算子上的。系统的状态可以用一个元素为实数的向量表示。 随着离散时间的每一个增加,这个线性算子就会作用在当前状态上，产生一个新的状态,并也会带入一些噪声,同时系统的一些已知的控制器的控制信息也会被加入。同时，另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。 为了从一系列有噪声的观察数据中用卡尔曼滤波器估计出被观察过程的内部状态，我们必须把这个过程在卡尔曼滤波的框架下建立模型。也就是说对于每一步k，定义矩阵Fk, Hk, Qk, Rk，有时也需要定义Bk，如下。 卡尔曼滤波器的模型。圆圈代表向量，方块代表矩阵，星号代表高斯噪声，其协方差矩阵在右下方标出。 卡尔曼滤波模型假设k时刻的真实状态是从(k ? 1)时刻的状态演化而来，符合下式： 其中 l Fk是作用在xk?1上的状态变换模型（/矩阵/矢量）。 l Bk是作用在控制器向量uk上的输入－控制模型。 l wk是过程噪声，并假定其符合均值为零，协方差矩阵为Qk的多元正态分布。 时刻k，对真实状态 xk的一个测量zk满足下式: 其中Hk是观测模型,它把真实状态空间映射成观测空间，vk 是观测噪声，其均值为零，协方差矩阵为Rk,且服从正态分布。 初始状态以及每一时刻的噪声{x0, w1, ..., wk, v1 ... vk} 都认为是互相独立的. 实际上，很多真实世界的动态系统都并不确切的符合这个模型；但是由于卡尔曼滤波器被设计在有噪声的情况下工作,一个近似的符合已经可以使这个滤波器非常有用了。更多其它更复杂的卡尔曼滤波器的变种，在下边讨论中有描述。 卡尔曼滤波器 卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要观测或者估计的历史信息。卡尔曼滤波器与大多数滤波器不同之处，在于它是一种纯粹的时域滤波器，它不需要像低通滤波器等频域滤波器那样，需要在频域设计再转换到时域实现。 卡尔曼滤波器的状态由以下两个变量表示： l ，在时刻k 的状态的估计； l ，误差相关矩阵，度量估计值的精确程度。 卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。 预测 (预测状态) (预测估计协方差矩阵) 可参考：~welch/media/pdf/kalman_intro.pdf 更新 首先要算出以下三个量： (测量余量，measurement residual) (测量余量协方差) (最优卡尔曼增益) 然后用它们来更新滤波器变量x 与P ： (更新的状态估计) (更新的协方差估计) 使用上述公式计算 仅在最优卡尔曼增益的时候有效。使用其他增益的话，公式要复杂一些，请参见推导。 不变量(Invariant) 如果模型准确，而且 与 的值准确的反映了最初状态的分布，那么以下不变量就保持不变：所有估计的误差均值为零 l l 且 协方差矩阵 准确的反映了估计的协方差: l l l 请注意，其中 表示 的期望值, 。 推导 推导后验协方差矩阵 按照上边的定义，我们从误差协方差 开始推导如下： 代入 再代入 与 整理误差向量，得 因为测量误差vk 与其他项是非相关的，因此有 利用协方差矩阵的性质，此式可以写作 使用不变量Pk|k-1以及Rk的定义 这一项可以写作 ： 这一公式对于任何卡尔曼增益Kk都成立。如果Kk是最优卡尔曼增益，则可以进一步简化，请见下文。 最优卡尔曼增益的推导 卡尔曼滤波器是一个最小均方误差估计器，后验状态误差估计(英文:a posteriori state estimate)是 我们最小化这个矢量幅度平方的期望值， ，这等同于最小化后验估计协方差矩阵 Pk|k的迹(trace)。将上面方程中的项展开、抵消，得到：       当矩阵导数是 0 的时候得到Pk|k的迹(trace)的最小值： 此处须用到一个常用的式子, 如下： 从这个方程解出卡尔曼增益Kk： 这个增益称为最优卡尔曼增益，在使用时得到最小均方误差。 后验误差协方差公式的化简 在卡尔曼增益等于上面导出的最优值时，计算后验协方差的公式可以进行简化。在卡尔曼增益公式两侧的右边都乘以 SkKkT 得到 根据上面后验误差协方差展开公式， 最后两项可以抵消，得到 . 这个公式的计算比较简单，所以实际中总是使用这个公式，但是需注意这公式仅在使用最优卡尔曼增益的时候它才成立。如果算术精度总是很低而导致数值稳定性出现问题，或者特意使用非最优卡尔曼增益，那么就不能使用这个简化；必须使用上面导出的后验误差协方差公式。