5分钟了解卡尔曼滤波——初学者实例
废话
在学长们不厌其烦地教导后，我想我大概也许可能。。。知道卡尔曼滤波是个什么了，，，，，，
我觉得对于我们初学菜鸟入门级别的，可能浅显粗俗的话更容易理解一些。所以，本贴不包含原理以及公式推导，仅是自己的一点心得——关于Kalman滤波的应用（所以写论文的朋友千万不要直接Copy）。如有错误，不吝指正！
首先说一下Kalman滤波与非线性优化。Kalman滤波是对问题进行线性处理（一次一阶泰勒展开），非线性优化自然是非线性咯（多次一阶或二阶泰勒展开）。
下面就开始Kalman吧，为了方便理解，先自定义一些概念：
预测值（X_），即通过状态估计方程计算得到的；观测值（Z），观测得到的值，无需计算，直接得到；终值（X），即最后我们要取的值。
它的大概思想是这样：我们最后要取的值，既不是预测值，也不是观测值，而是通过二者合并计算后的值。为什么呢？因为Kalman认为，真实的理论值是不可能被正确观测到的，总会有一定误差。举个不恰当的例子，理论上4/3是存在的，但在现实生活中是不可能有这个的，你拿尺子去量任何确定的实物都会得到一个长度，无限小数是不存在的！
那么怎么根据预测值和观测值得到终值呢？如下：
X = X_ + ◊
很简单对吧，就是让预测值再加上一个东西 ◊，这个 ◊ 是通过 K、X_ 和 Z 计算得到的，所以核心工作就是求 X_、K。
思路
先看一下Kalman的两个方程（第一个是用来计算的，第二个仅是理论表示）：
状态方程： X(k) = AX(k-1) + BU(k) + W(k)
观测方程： Z(k) = HX(k) + V(k)
其中状态方程表示的是如何用上一时刻的状态计算得到这一时刻的状态，A、B 为系数矩阵，U(k) 为控制输入（可以为0），W(k) 为噪声（通常假设为高斯白噪声，方差为 Q ）；
其中观测方程表示的是当前时刻状态被噪声干扰然后被观测得到的过程，H 为系数矩阵， V(k) 为噪声（通常假设为高斯白噪声，方差为 R ）。
在控制问题中通常有控制输入，而另外某些问题中状态被观测时会有一定变化，所以这两个方程看起来很复杂。实际上很多问题中，没有控制，观测也很简单，所以 A、H 可以取 1（或单位矩阵），U(k) 为 0，甚至 W(k) 也可以为 0。
所以最简单的形式为：
X(k) = X(k-1)
Z(k) = X(k) + V(k)
这表示，很短时间内，现在时刻的值（终值）可以近似认为等于上一时刻的值（很多惯性系统，如温度）；而观测值仅仅是终值基础上加了一个噪声。
计算
下面说一下Kalman的迭代计算过程（核心是计算 X_、K，同时定义了中间变量 P_和P ）
（首先，X(k-1)、P(k-1)已知，初始时刻这两个值可以随便给，最终会收敛；一般X(k-1)取观测值，P(k-1)可以取单位矩阵等实数矩阵 ）
首先明确我们可以用的参数有：A、B、H、Q、R。 上一时刻的值：X(k-1)、P(k-1)。 当前时刻控制输入：U(k)。 当前时刻观测值：Z(k)
然后开始迭代，迭代可以分为两大部分。
第一部分（计算参数：中间变量 P_、 预测值 X_、 卡尔曼增益 K ）（矩阵相乘的乘号我就省了，为了看清一点特意加了空格）：
P_(k) = AP(k-1)AT + Q
X_(k) = AX(k-1) + BU(k) ；可以发现这个式子就是原来的状态方程计算的式子（所以前面说状态方程是用来参与计算的）
K(k) = P_(k) HT ( H P_(k) HT + R )-1 ；用中间变量 P_ 来计算 K
第二部分（用计算得到的参数计算终值与中间变量 P ）
X(k) = X_(k) + K(k) ( Z(k) - HX_(k) ) ；用卡尔曼增益 K 和观测值来更新 X，可以看到观测值 Z 直接就用了，而不是通过计算得到的（所以前面说观测方程仅供理论分析，计算时不是直接用）
P(k) = ( I - K(k) H ) P_(k) ；用卡尔曼增益 K 和 P_ 来更新 P
然后用 X(k) 和 P(k) 进行下一轮迭代。
续
前面是完整的两个方程进行迭代的过程，对于简单系统：
X(k) = X(k-1)
Z(k) = X(k) + V(k)
计算过程就相当简单：
上一时刻 X(k-1)、P(k-1) 已知，可直接计算 Kalman 增益 K，
K(k) = P(k-1)/ (P(k-1) + R)
然后可计算终值及 P，
X(k) = X(k-1) + K(k) ( Z(k) - X(k-1) )
P(k) = ( I - K(k) ) P(k-1)
可以看到，Kalman本质上是一个利用上一时刻状态估计当前时刻状态的一阶线性系统。当前值为上一时刻值加上一个偏差，而这个偏差又可以通过当前时刻的观测值进行修正。
