卡尔曼滤波解读

卡尔曼滤波器（Kalman Filter）

一、递归算法

卡尔曼滤波器是一种最优化递归数字处理算法（Optimal Recursire Data Processing Algorithm），不是传统意义上的滤波器，更像是一种观测器，在导航中应用广泛，原因是生活中的不确定性，不确定性表现在：
（1）不存在完美的数学模型；
（2）系统的扰动不可控，也很难建模；
（3）测量传感器存在误差。

1. 举例：
找不同的人测量一个硬币的直径，令Zk为测量结果，k表示第k次测量，因为每个人测量的不一样并且尺子也有一定的测量误差，所以测量结果也不尽相同，比如Z1=50.1mm，Z2=50.4mm，Z3=50.2mm，…。

现在要求给出估计结果，那么自然想到要取平均值。用数学表示为hat(x_k) = 1/k(Z1+Z2+…+Zk) ，其中hat(x_k)表示估计值。

hat(x_k) = 1/k * (Z_1+Z_2+...+Z_k)
         = 1/k * (Z_1+Z_2+...+Z_(k-1)) + 1/k * Z_k 
         = 1/k * (k-1)/(k-1) *  (Z_1+Z_2+...+Z_(k-1)) + 1/k * Z_k 
         = (k-1)/k * hat(x_(k-1)) + 1/k * Z_k
         = hat(x_(k-1)) - 1/k * hat(x_(k-1)) + 1/k * Z_k 
 ===>  hat(x_k) = hat(x_(k-1)) + 1/k * (Z_k - hat(x_(k-1)))
其中，1/(k-1) *  (Z_1+Z_2+...+Z_(k-1))是k-1次的平均值hat(x_(k-1))

观察下面这个公式，

hat(x_k) = hat(x_(k-1)) + 1/k * (Z_k - hat(x_(k-1)))

随着k的增加，1/k趋近于0，hat(x_k)也越趋近于hat(x_(k-1))，即测量结果不再重要，这很好理解，也就是当有大量的数据，测量了很多次以后，对估计的结果就有信心了，测量结果就没那么重要了。反之，当k很小，1/k就很大，那么测量结果就很重要。

令系数1/k = K_k，那么上面的公式可写为

hat(x_k) = hat(x_(k-1)) + K_K(z_k - hat(x_(k-1)))
即当前估计值=上一次估计值+系数*（当前测量值-上一次估计值）
系数K_k就是Kalman Gain，即卡尔曼增益/因数

2. 详解K_k:
引入两个参数，估计误差e_EST和测量误差e_MEA，那么有

K_k = e_(EST_(k-1))/(e_(EST_(k-1)) + e_(MEA_k))
即K_k = 第k-1次的估计误差除以第k-1次的估计误差和第k次的测量误差之和
该公式为卡尔曼滤波的核心

当k-1时的估计误差远大于k时的测量误差，K_k趋近于1，则hat(x_k) = hat(x_(k-1)) + z_k - hat(x_(k-1)) = z_k，即估计值趋近于测量值。也就是说，估计误差太大，那么应该信任测量值。反之，当k-1时的估计误差远小于k时的测量误差，K_k趋近于0，则hat(x_k) = hat(x_(k-1))，即估计误差远小于测量误差，那么应该信任估计值。

3. 举例：用卡尔曼滤波解决一个实际问题

step 1： 计算K_k = e_(EST_(k-1))/(e_(EST_(k-1)) + e_(MEA_k))
step 2： 计算hat(x_k) = hat(x_(k-1)) + K_K(z_k - hat(x_(k-1)))
step 3： 更新e_(EST_k) = (1-K_k) * e_(EST_(k-1))

假设要估计一个物体的长度，该物体长度真实值x = 50mm，假设第一次的估计值为hat(x_0) = 40mm，第一次估计误差e_(EST_0) = 5mm，第一次测量值为z_1 = 51mm，第一次测量误差为e_(MEA_k) = 3mm。画出表格为

k	z_k	e_(MEA_k)	hat(x_k)	K_k	e_(EST_k)
0			40		5
1	51	3	46.875	0.625	1.875
2	48	3	47.308	0.3846	1.154

表格中粗体内容根据步骤算出，斜体内容根据测量误差，在真实值x±e_(MEA_k)范围内即可。随着次数增加，估计值会逐渐趋近于真实值，是一种递归的思想。

二、数学基础

数据融合、协方差矩阵、状态空间方程、观测器

1.数据融合（Data Fusion）

假设用两个称去称一个物体的重量，分别得出z_1 = 30g，z_2 = 32g，然而两个称都不准，测量都有误差，假设第一个称的标准差α_1 = 2g，第二个称的标准差为α_2 = 4g，它们都符合正态分布，则（插入图）

现要求根据这些已知条件给出估计真实值hat(z)。由卡尔曼滤波方程

hat(z) = z_1 + K*(z_2 - z_1)
rang(k) = [0,1]，当k=0,hat(z)=z_1;当k=1,hat(z)=z_2

下面求K使得标准差α_hat(z)最小，即方差Var(hat(z))最小，那么有

(α_hat(z))^2 = Var(Z_1 + K(Z_2 - Z_1))
             = Var(Z_1 - K*Z_1 + K*Z_2))
             = Var((1-K)*Z_1 + K*Z_2)
             = (1-k)^2 * Var(Z_1) + K^2 * Var(Z_2)
             = (1-k)^2 * (α_1)^2 + K^2 * (α_2)^2
要求(α_hat(z))^2的最小值，需要(α_hat(z))^2对K求导得到极值，
即d(α_hat(z))^2 / dK = 0
===>
d(α_hat(z))^2 / dK = -2(1-K) * (α_1)^2 +2K * (α_2)^2 = 0
调整可得，
-(α_1)^2 + K(α_1)^2 + K(α_2)^2 = 0
将α_1 = 2，α_2 = 4代入，得
(K - 1) * 4 + K * 16 = 0
求得K = 0.2

有了K之后，将K代回hat(z) = z_1 + K*(z_2 - z_1)求出hat(z)

hat(z) = z_1 + K*(z_2 - z_1)
       = 30 + 0.2 * (32 - 30)
       = 30.4    最优解
计算方差：
(α_hat(z))^2 = (1-k)^2 * (α_1)^2 + K^2 * (α_2)^2
             = (1-0.2)^2 * 4 +0.2^2 * 16
             = 3.2
则α_hat(z) = sqrt(3.2) = 1.79

综上所述，算得hat(z) = 30.4，α_hat(z) = 1.79，画图如下所示，该过程就是数据融合。

2.协方差矩阵（Covariance Matrix）

协方差矩阵就是将方差和协方差在一个矩阵表现出来，体现了变量间的联动关系。

例子：

球员	身高x	体重y	年龄z
1	179	74	33
2	187	80	31
3	175	71	28
平均数	180.3	75	30.7

求方差：

(α_x)^2 = 1/3 * ((179-180.3)^2 + (187-180.3)^2 + (175-180.3)^2) = 24.89
同理算出，(α_y)^2 = 14, (α_z)^2 = 4.22

求协方差：

α_x*α_y = 1/3((179-180.3)(74-75) + (187-180.3)(80-75) + (175-180.3)(71-75)) = 18.7 = α_y*α_x
同理，α_x*α_z = α_z*α_x = 4.4, α_y*α_z = α_z*α_y = 3.3
说明：值为正则正相关，负则负相关。

将上述计算得到的数据代入协方差矩阵可得

    [(α_x)^2  α_x*α_y  α_x*α_z]     [24.89 18.7 4.4]
P = [α_y*α_x  (α_y)^2  α_y*α_z]  =  [18.7  14   3.3]
    [α_z*α_x  α_z*α_y  (α_z)^2]     [4.4   3.3 4.22]

通过这个矩阵就可以分析各个数据之间的关系。

复杂的问题求解协方差矩阵
需要先求过渡矩阵，

    [x_1  y_1  z_1]       [1 1 1][x_1  y_1  z_1] 
a = [x_2  y_2  z_2] - 1/3 [1 1 1][x_2  y_2  z_2]
    [x_3  y_3  z_3]       [1 1 1][x_3  y_3  z_3]
其中，后面这一项展开就是平均值

根据下面的公式求解协方差矩阵，

P = 1/3 * a^T * a
其中，1/3因为是有3行

3.状态空间的表达（State Space Representation）

例子：
动态方称表达式：

m'x' + Bx' + kx = F
将F定义为u，即系统输入
m'x' = u - Bx' - kx

将其化成状态空间的表示，需要先确定两个状态，比如x_1 = x，x_2 = x’，那么(x_1)’ = x_2，(x_2)’ = ‘x’ = 1/m * u - B/m * x’ - k/m * x = 1/m * u - B/m * x_2 - k/m * x_1，这样就把该形式用两个一阶微分方程表示出来了。

如果还有测量，比如z_1 = x = x_1测量的是位置，z_2 = x’ = x_2测量的是速度。那么上面的状态用矩阵表示为

[(x_1)']   [  0     1 ][x_1]   [ 0 ]
[(x_2)'] = [-k/m - B/m][x_2] + [1/m]u
归纳得，连续表示：X'(t) = A X(t) + B u(t)；
离散表示：X_k = A X_(k-1) + B u_k + w_(k-1)
w_(k-1)为过程噪音，是不确定的

测量用矩阵表示为

[z_1]   [1 0][x_1]
[z_2] = [0 1][x_2]
归纳得，连续表示：Z(t) = H X(t)；
离散表示：Z_k = H X_k + v_k
下标k表示时间单位，采样时间的变化
v_k是测量噪音，是不确定的

这就是状态空间表示，可以看为是对时间的导数，体现了随时间的变化。

问题：模型不准确，测量也不准确，在两个不确定的情况下，如何估计一个精确的hat(x_k)呢？这就是卡尔曼滤波需要做的。结合数据融合部分讲的内容考虑，即根据一个不准确的计算结果和一个不准确的测量值通过数据融合得到一个较为准确的估计值。