The discrete-time Kalman filter
Optimal Recursire Data Processing Algorithm
这是一种最优化递归数据处理算法。更像是一种观测器而非一般意义的滤波器。
卡尔曼滤波器应用很广泛，尤其是在导航里。因为物理世界有很多不确定性，对于一个系统：
- 不存在完美的数学模型
- 系统的扰动不可控，也很难建模
- 传感器存在测量误差
举个例子，一把尺子测量硬币直径。测量结果为 \(z_k\)
如果去估计结果，很自然的想到取平均值。即
\[\begin{aligned} \hat{x}_k &= \frac{1}{k} ( z_1 + z_2 + \cdots + z_k ) \\ &= \frac{1}{k} ( z_1 + z_2 + \cdots + z_{k-1} ) + \frac{1}{k} z_k \\ &= \frac{k-1}{k} \cdot \color{blue} \frac{1}{k-1} ( z_1 + z_2 + \cdots + z_{k-1} ) \color{default} + \frac{1}{k} z_k \\ &= \frac{k-1}{k} \hat{x}_{k-1} + \frac{1}{k} z_k \\ &= \hat{x}_{k-1} + \frac{1}{k} ( z_k - \hat{x}_{k-1} ) \end{aligned}\]写出来这个公式
\[\hat{x}_k = \hat{x}_{k-1} + \frac{1}{k} ( z_k - \hat{x}_{k-1} )\]随着测量次数的增加，\(\frac{1}{k} \to 0, \hat{x}_k \to \hat{x}_{k-1}\)，也就是说测量的结果就不再重要了。
这也很好理解，当有了足够的测量数据，我们就对估计的结果很有信心了。以后的测量值就不重要了。当 \(k\) 比较小时，测量结果还是比较重要的。
把这个公式提炼一下，让这个系数 \(\frac{1}{k} = K_k\)
\[\hat{x}_k = \hat{x}_{k-1} + K_k ( z_k - \hat{x}_{k-1} )\]这个式子的含义就是当前的估计值=上次的估计值 + 系数 * ( 当前测量值 - 上次的估计值 )
这个 \(K_k\) 就是卡尔曼增益。
这个公式里有递归的思想。
先做一个最简单的讨论，引入两个参数：
- 估计误差：\(e_{EST}\) ，估计值和真实值的差距
- 测量误差：\(e_{MEA}\) ，测量值和真实值的差距
这时候卡尔曼增益为
\[K_k = \frac{ e_{EST_{k-1}} }{ e_{EST_{k-1}} + e_{MEA_{k}} }\]这个公式是卡尔曼滤波器的核心公式，后面会推导。
先来分析一下
- 当 \(e_{EST_{k-1}} >> e_{MEA_{k}}\) 时，\(K_k \to 1\)，
递归算法
