清晰易懂的卡尔曼滤波

卡尔曼滤波用于在变化的动态系统中，由当前状态和观测值相融合估计下一个状态。这是一个迭代的过程，也是一个数据融合的过程。
比如用在汽车导航、deepsort目标跟踪里面的位置预测等。

以下推导都是一维变量形式。

step 1

假设有一个系统，有状态转移方程：

$$x_k=A{\overline{x}}_{k-1}+B{u}_k+w_k\text{\hspace{1em}(1)}$$

其中${\overline{x}}_{k-1}$是上一个状态值，$x_k$是系统输出，$u_k$是系统控制量，$w_k$是系统扰动，$A$就是系统转移矩阵。
其中，${\overline{x}}_{k-1}-N({\mu }_1,{\sigma }_1^2)$，$w_k-N(0,Q)$
由于系统存在干扰，$x_k$是一个服从正态分布的随机变量。

$$x_k-N(A{\mu }_1,A^2{\sigma }_1^2+Q)\text{\hspace{1em}(2)}$$

但一般$Q$不知道，后面计算都会使用$x_k-N(A\mu ,A^2{\sigma }_1^2)$代替。

step2

通过传感器等检测出系统观测值$y_k$，但是测量存在误差：

$$y_k-N({\mu }_2,{\sigma }_2^2)$$

step3

此时，我们有了一个根据上一状态对现在状态的预测值$x_k$和一个通过实际测量得到的观测值$y_k$，那么哪一个是对的？
两个都是随机变量，如果状态突变，预测可能不正确，如果测量出错，那测量可能不准，所以要对现有的两个数据做融合。
设置一个融合系数$K$，常叫做增益系数，定义以下融合公式：

$${\overline{x}}_k=(1-K)x_k+K{y}_k$$

那么如何确定$K$的合适取值?
两个正态分布的和仍然是正态分布，那希望${\overline{x}}_k$的方差越小越好，这样估计会更准确，把它转化成一个优化问题。

$$argmi{n}_{(K)}D({\overline{x}}_k)=argmi{n}_{(K)}[(1-K^2)A^2{\sigma }_1^2+K^2{\sigma }_2^2]$$

容易求出
$$K=\frac{A^2{\sigma }_1^2}{A^2{\sigma }_1^2+{\sigma }_2^2}\text{\hspace{1em}(3)}$$
把$K$带入可以求出

$${\overline{x}}_k=(1-K)x_k+K{y}_k=x_k+K(y_k-x_k)={\mu }_3\text{\hspace{1em}(4)}$$

$$D({\overline{x}}_k)=(1-K)A^2{\sigma }_1^2=A^2{\sigma }_1^2-K{A}^2{\sigma }_1^2={\sigma }_3^2\text{\hspace{1em}(5)}$$

此时的${\overline{x}}_k-N({\mu }_3,{\sigma }_3^2)$

这里$K$常写做$\overline{K}=\frac{K}{A}$对应的公式相应变换下。

---

总结

卡尔曼滤波的5个公式，每一个都对应到上面的公式编号：
1.状态方程求$x_k$；
2.求$x_k$的方差；
3.求$K$;
4.融合求${\overline{x}}_k$；
5.更新${\overline{x}}_k$方差；

那么此时得到${\overline{x}}_k$服从一个正态分布，是否又回到了step1？如此迭代…