Kalman滤波器

本博客参考: http://xiahouzuoxin.github.io/notes

假设一辆质量为m的小车，受恒力F推动，沿r方向做匀加速直线运动，小车在t-ΔT时刻的位移是s(t-1)，此时的速度为

v(t-1)。求：t时刻的位移是s(t)，速度为v(t)？由牛顿第二定律，求得加速度：

那么就有下面的位移和速度关系：

将上面的表达式用矩阵表示如下：

卡尔曼滤波器是建立在动态过程之上，由于物理量（位移，速度）的不可突变特性，这样就可以通过t-1时刻估计(预

测)t时刻的状态，其状态空间模型为：

其中：

匀加速直线运动过程就是卡尔曼滤波中状态空间模型的一个典型应用，将(2)是表示成离散形式为：

其中，各个量之间的含义是：

1. x(n)是状态向量，包含了观测的目标（如：位移、速度）

2. u(n)是驱动输入向量，如上面的运动过程是通过受力驱动产生加速度，所以u(n)和受力有关

3. A是状态转移矩阵，其隐含指示了“n-1时刻的状态会影响到n时刻的状态

4. B是控制输入矩阵，其隐含指示了“n时刻给的驱动如何影响n时刻的状态”

从运动的角度，很容易理解：小车当前n时刻的位移和速度一部分来自于n-1时刻的惯性作用，通过Ax(n)来度量，另

一部分来自于现在n时刻小车新增加的外部受力，通过Bu(n)来度量。

5. w(n)是过程噪声，w(n)~N(0,Q)的高斯分布，过程噪声是使用卡尔曼滤波器时一个重要的量

另外，计算n时刻的位移也可以通过直接测量的方法得到，

其中，z(n)就是测量结果，H(n)=[1,0]，v(n)~N(0,R)为测量噪声

--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

设预测过程噪声w(n)~N(0,Q)，测量噪声v(n)~N(0,R)。Kalman计算输出分为预测过程和修正过程如下：

1. 预测

   预测值：

   

   最小均方误差矩阵：

   

2. 修正

   误差增益：

   

   修正值：

    这里的A去掉

   最小均方误差矩阵：

   

从(5)~(9)中：

1. x(n)：Nx1的状态矢量

2. z(n)：Mx1的观测矢量，Kalman滤波器的输入

3. x(n|n-1)：用n时刻以前的数据进行对n时刻的估计结果

4. x(n|n)：用n时刻及n时刻以前的数据对n时刻的估计结果，这也是Kalman滤波器的输出

5. P(n|n-1)：NxN，最小预测均方误差矩阵，其定义式为

，通过计算最终得到(6)式。

6. P(n|n)：NxN，修正后最小均方误差矩阵。

7. K(n)：NxM，误差增益，从增益的表达式看，相当于“预测最小均方误差”除以“n时刻的测量误差+预测最小均方误

差”

--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

 Kalman滤波器巧妙的用“独立高斯分布的乘积”将这两个测量值和估计值进行融合！高斯分布乘积运算的结果仍为高

斯分布，高斯分布的均值对应n时刻的估计值，高斯分布的方差对应n时刻的均方误差。

现在令，则(10)(11)变成：