手撕卡尔曼滤波器
卡尔曼滤波器（Kalman Filter），字面上看，Filter滤波器一词不能很好地体现其特点。用一句话来说，卡尔曼滤波器Optimal Recursive Data-Processing Algorithm，即最优化 递归 数字处理 算法像是一种观测器，而不是一般意义上的滤波器。特别是在导航中，卡尔曼滤波器被广泛使用。它的广泛应用是因为世界中存在大量的不确定性，当我们描述一个系统时，这个不确定性主要体现在三个方面：
递归算法
下面是一个例子，用同一把尺子多次测量同一枚硬币的直径 z k z_k zk 表示第k次测量结果。由于各种误差，测量得到： z 1 = 50.1 m m z 2 = 50.4 m m z 3 = 50.2 m m \begin{aligned} z_1=50.1mm \\ z_2=50.4mm \\ z_3=50.2mm \end{aligned} z1=50.1mmz2=50.4mmz3
观察最后一行结论， k ↑ , 1 k − 1 → 0 , x ^ k → x ^ k − 1 k\uparrow, \frac{1}{k-1}\to0,\hat{x}_k\to\hat{x}_{k-1} k↑,k−11→0,x^k→x^k−1 ，也就是说，随着k的增加，此时拥有了大量的数据，对估计的结果就比较有信心了，测量的结果就不是很重要了。相反，如果k比较小， 1 k − 1 \frac{1}{k-1} k−11 就会比较大，测量结果 z k z_k zk 就会起到很大的作用，尤其是测量结果和估计值差距比较大的时候。
令 K k = 1 k − 1 K_k=\frac{1}{k-1} Kk=k−11 ，则此时公式可以表示为： x ^ k = x ^ k − 1 + K k ( z k − x ^ k − 1 ) \hat{x}_k = \hat{x}_{k-1} + K_k(z_k-\hat{x}_{k-1}) x^k=x^k−1+Kk(zk−x^k−1) 上式表示的含义为：当前的估计值 = 上一次的估计值 + 系数 * ( 当前测量值 - 上一次的估计值 ) ，其中的 K k K_k Kk 就是卡尔曼增益/因数（Kalman Gain），通过这个公式可以看出，新的估计值 x ^ k \hat{x}_k x^k 与上一次的估计值 x ^ k − 1 \hat{x}_{k-1} x^k−1 有关，上一次的又与上上次的有关，这就是一种递归思想（Recursive），这也是卡尔曼滤波器的优势，他不需要追溯很久以前的数据，只需要上一次的就可以。下面来讨论一下这个 K k K_k Kk ：
引入两个误差：
- 估计误差 e E S T e_{EST} eEST （e代表误差error，EST代表估计estimate）
- 测量误差 e M E A e_{MEA} eMEA （e代表误差error，MEA代表测量measurement）
则 K k K_k Kk 可以表示为 K k = e E S T k − 1 e E S T k − 1 + e M E A k K_k = \frac{e_{EST_{k-1}}}{e_{EST_{k-1}}+e_{MEA_{k}}} 标签： 9qb型传感器hpk无功功率变送器