Kalman滤波器--从高斯融合推导

0.引言

“如果古希腊人知道正态分布，想必奥林匹斯山的神殿里会多出一个正态女神，由她来掌管世间的混沌！”

1.贝叶斯法则

• 另一种推导方式，从误差角度的推导？参考之前的推导。之前的推导过于繁琐，感觉更多的是数学上的推导，从贝叶斯法则以及高斯融合的角度推导，则物理意义十分明确，更加的浅显易懂。
• 任乾大佬博客
• 一句话总结就是贝叶斯法则+高斯融合：根据贝叶斯法则有，后验估计 $\propto$ 似然 * 先验 ，参考链接；然后根据假设（误差服从高斯分布），通过高斯分布的性质，将 似然项高斯分布 和 先验项高斯分布 相乘就得到了后验估计的分布。

这篇文章看到这里就够了。

---

状态估计问题的求解思路：

假设系统 $k$ 时刻的观测量为 $z_k$ ,状态量为 $x_k$ ,这两个变量是符合某种分布的随机变量,且它们不相互独立。我们希望求出:
$$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{z}}_{1:k}\right)$$
根据贝叶斯法则,(估计中的概率公式参考)将系统状态的概率求解拆分如下:
$$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{z}}_{1:k}\right)\propto P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{z}}_{1:k-1}\right)$$

假设系统 满足马尔可夫性质,即 $x_k$ 仅与 $x_{K-1}$ 相关,与更早的状态无关（如下图），可进一步简化为：

$$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{z}}_{1:k}\right)\propto P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)P\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1}\right)$$
其中:

• $P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)$ 为似然项,可由观测方程给出
• $P\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1}\right)$ 为先验项,可通过状态转移方程推导得到

该问题可用滤波器相关算法解决,如Kalman Filter或Extented Kalman Filter。

---

在状态估计时：

$$p(\mathbf{x}\mid \mathbf{y})=\frac{p(\mathbf{y}\mid \mathbf{x})p(\mathbf{x})}{p(\mathbf{y})}$$

赋予该式物理意义：

• $x$ ： 状态，可由状态转移方程推出，也称为先验
• $y$ ：传感器读数
• $p(y|x)$ : 传感器模型，可由观测方程给出，也称为似然
• $p(x|y)$ : 状态估计， 也称后验

因此贝叶斯估计： 后验估计 $\propto$ 似然 * 先验 。参考链接。

---

2.kalman推导

从一个例子开始，定义 $k$ 时刻的系统的状态为 $x_k$ ,假设包含位置和速度两部分:

$$x_k=\left[\begin{array}{l}{p}_k\\ v_k\end{array}\right]$$

为进一步表示$x_k$ 各成员的不确定性和各维度之间的相互关系,引入协方差矩阵:

$${\mathbf{P}}_k=\left[\begin{array}{cc}{\Sigma }_{pp}& {\Sigma }_{pv}\\ {\Sigma }_{vp}& {\Sigma }_{vv}\end{array}\right]$$

其中:

• ${\Sigma }_{pp}$ 和 ${\Sigma }_{vv}$ 为状态分量的方差
• ${\Sigma }_{vp}$ 和 ${\Sigma }_{pv}$ 描述 $p$ 和 $v$ 之间协方差

如上图(左),速度和位置关系是独立的,因为其方差互相不受影响;而图(右)则相反。

进一步,已知 $k-1$ 时刻的状态 $x_{k-1}$ ,我们首先可以通过运动关系预测其 $k$ 时刻的状态 $x_k$ 。

情况1:假设短时间内满足匀速运动的条件:

$${\overline{\mathbf{x}}}_k=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]{\hat{\mathbf{x}}}_{k-1}={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}$$

其中:

• ${\overline{\mathbf{x}}}_k$ 为 $k$ 时刻的先验分布
• ${\hat{\mathbf{x}}}_{k-1}$ 为 $k-1$ 时刻的后验分布
• ${\mathbf{F}}_k$ 为状态转移矩阵

情况2:以上状态转移的过程,是系统没有任何外部干预的情况下匀速运动,但试想如果在运动过程中有外界影响会怎么样呢? 比如,人为地推了一下。

$${\overline{\mathbf{x}}}_k={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k$$

其中:

• ${\mathbf{u}}_k$ 表示外部输入
• ${\mathbf{B}}_k$ 表示外部输入与系统状态变化的转换关系矩阵

情况3:在上述的系统状态建模中,均是理想化的模型,没有考虑系统噪声。为更好地建模系统状态转换关系,我们引入高斯噪声项来模拟系统噪声。考虑噪声后的 ${\overline{\mathbf{x}}}_k$ 如下:

$${\overline{\mathbf{x}}}_k={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k+{\mathbf{w}}_k\text{\hspace{1em}(1)}$$

其中:

• ${\mathbf{w}}_k\sim N\left(0,{\mathbf{Q}}_k\right)$ 为高斯噪声

$Cov(x)=\mathbf{\Sigma }$ ,根据协方差矩阵的性质:

$$\mathrm{Cov}(\mathbf{A}\mathbf{x})=\mathbf{A}\mathbf{\Sigma }{\mathbf{A}}^T$$贝叶斯法则以及高斯融合

对于预测而来的状态,可以描述为:

$$\begin{array}{c}\mathrm{Cov}\left({\hat{\mathbf{x}}}_{k-1}\right)={\hat{\mathbf{P}}}_{k-1}\\ {\overline{\mathbf{x}}}_k={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}\end{array}\}\Rightarrow \mathrm{Cov}\left({\overline{\mathbf{x}}}_k\right)=\mathrm{Cov}\left({\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}\right)={\mathbf{F}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_k^T$$

即是：

$${\overline{\mathbf{P}}}_k={\mathbf{F}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_k^T$$

考虑噪声的 ${\overline{\mathbf{x}}}_k$, 其协方差可记为：

$${\overline{\mathbf{P}}}_k={\mathbf{F}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_k^T+{\mathbf{Q}}_k\text{\hspace{1em}(2)}$$

根据 $k-1$ 时刻的后验状态 ${\hat{\mathbf{x}}}_{k-1}$，我们可以预测出 $k$ 时刻的先验状态 ${\overline{\mathbf{x}}}_k$ 以及其协方差矩阵 ${\overline{\mathbf{p}}}_k$ :
$${\overline{\mathbf{x}}}_k={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k+{\mathbf{w}}_k\text{\hspace{1em}(1)}$$

${\overline{\mathbf{x}}}_k$ 满足如下分布：

$$N\left({\overline{\mathbf{x}}}_k,{\overline{\mathbf{P}}}_k\right)=N\left({\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k,{\mathbf{F}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_k^T+{\mathbf{Q}}_k\right)\text{\hspace{1em}(2)}$$

当获得 $k$ 时刻的系统观测量 ${\mathbf{z}}_k$ 时,可以尝试通过 ${\mathbf{z}}_k$ 重新修正 $k$ 时刻的后验状态 ${\hat{\mathbf{x}}}_k$ 及其协方差矩阵 ${\hat{\mathbf{p}}}_k$.

假设通过一些传感器测量的 ${\mathbf{z}}_k=(position,velocity)$ ,这样可以得到如下结果：

$${\mathbf{z}}_k={\overline{\mathbf{x}}}_k$$

为了进一步泛化观测量 ${\mathbf{z}}_k$ 与状态量 ${\overline{\mathbf{x}}}_k$ 之间的关系,定义观测矩阵 ${\mathbf{H}}_k$:

$${\mathbf{z}}_k={\mathbf{H}}_k{\overline{\mathbf{x}}}_k\text{\hspace{1em}(3)}$$
根据协方差矩阵的性质,可推导出观测量的方差为:

$$\mathbf{\Sigma }={\mathbf{H}}_k{\overline{\mathbf{P}}}_k{\mathbf{H}}_k^T\text{\hspace{1em}(4)}$$

进一步,在考虑观测的高斯噪声的情况下 ${\mathbf{v}}_k$ 满足 $N(0,{\mathbf{R}}_k)$分布 ,可得出下式：

$${\mathbf{z}}_k={\mathbf{H}}_k{\overline{\mathbf{x}}}_k+{\mathbf{v}}_k\text{\hspace{1em}(5)}$$

${\mathbf{z}}_k$ 满足如下分布：

$$N\left({\mathbf{z}}_k,\mathbf{\Sigma }\right)=N\left({\mathbf{H}}_k{\overline{\mathbf{x}}}_{\mathbf{k}},{\mathbf{H}}_k{\overline{\mathbf{P}}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k\right)\text{\hspace{1em}(6)}$$

其中,公式(2) 描述了 ${\overline{\mathbf{x}}}_k$ 的分布,公式(6) 描述了 ${\mathbf{z}}_k$ 的分布。

---

高斯分布知识回顾：

两个高斯分布的乘积依然是高斯分布,而且为了得到两个高斯分布的重叠部分的分布函数,我们通常将两个高斯分布相乘。

$$N\left(x,{\mu }^{\prime },{\sigma }^{\prime }\right)=N\left(x,{\mu }_0,{\sigma }_0\right)\cdot N\left(x,{\mu }_1,{\sigma }_1\right)$$

由 $N(x,\mu ,\sigma )=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}$推导可得:

$$\begin{array}{rl}& {\mu }^{\prime }={\mu }_0+\frac{{\sigma }_0^2\left({\mu }_1-{\mu }_0\right)}{{\sigma }_0^2+{\sigma }_1^2}\\ & {\sigma }^{\prime 2}={\sigma }_0^2-\frac{{\sigma }_0^4}{{\sigma }_0^2+{\sigma }_1^2}\end{array}$$

假设 $k=\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}$，上式可化简为：

$$\begin{array}{rl}& {\mu }^{\prime }={\mu }_0+K\left({\mu }_1-{\mu }_0\right)\\ & {\sigma }^{\prime 2}={\sigma }_0^2-K{\sigma }_0^2\end{array}$$

将上式扩展到多维空间:

$$\begin{array}{c}\mathbf{K}={\mathbf{\Sigma }}_0{\left({\mathbf{\Sigma }}_0+{\mathbf{\Sigma }}_1\right)}^{-1}\\ {\mathbf{\mu }}^{\prime }={\mathbf{\mu }}_0+\mathbf{K}\left({\mathbf{\mu }}_1-{\mathbf{\mu }}_0\right)\\ {\mathbf{\Sigma }}^{\prime }={\mathbf{\Sigma }}_0+\mathbf{K}{\mathbf{\Sigma }}_0\end{array}$$

---

回到kalman推导

${\bar{\mathbf{x}}}_k$ 满足如下分布：

$$N\left({\overline{\mathbf{x}}}_k,{\overline{\mathbf{P}}}_k\right)=N\left({\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k,{\mathbf{F}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_k^T+{\mathbf{Q}}_k\right)\text{\hspace{1em}(2)}$$
${\mathbf{z}}_k$ 满足如下分布:
$$N\left({\mathbf{z}}_k,\Sigma \right)=N\left({\mathbf{H}}_{\mathbf{k}}{\overline{\mathbf{x}}}_{\mathbf{k}},{\mathbf{H}}_k{\overline{\mathbf{P}}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k\right)\text{\hspace{1em}(6)}$$
将 ${\overline{\mathbf{x}}}_k$ 和 ${\mathbf{z}}_k$ 的分布代入上式（高斯分布知识回顾里面的多维空间高斯分布融合公式）:
$$\begin{array}{c}{\hat{\mathbf{x}}}_k={\overline{\mathbf{x}}}_k+\mathbf{K}\left({\mathbf{z}}_k-{\overline{\mathbf{x}}}_k\right)\\ {\hat{\mathbf{P}}}_k={\overline{\mathbf{P}}}_k+\mathbf{K}{\overline{\mathbf{P}}}_k\end{array}$$
其中, $\mathbf{K}={\overline{\mathbf{P}}}_k{\left({\overline{\mathbf{P}}}_k+\mathbf{\Sigma }\right)}^{-1}$ 为卡尔曼增益。

以上为根据历史状态和观测量, 估计当前位置和速度状态的过程。

---

当系统为线性马尔可夫系统时，可以通过Kalman Filter来求解融合问题。
$$\{\begin{array}{c}{\overline{\mathbf{x}}}_{\mathbf{k}}={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k+{\mathbf{w}}_k\\ {\mathbf{z}}_k={\mathbf{H}}_k{\overline{\mathbf{x}}}_{\mathbf{k}}+{\mathbf{v}}_k\end{array}k=1,2,\cdots ,N\text{\hspace{1em}(7)}$$
由状态转移方程可得: $$P\left({\overline{\mathbf{x}}}_{\mathbf{k}}\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=N\left({\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k,{\mathbf{F}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_k^T+{\mathbf{Q}}_k\right)$$

由观测方程可得: $$P\left({\mathbf{z}}_k\mid {\overline{\mathbf{x}}}_{\mathbf{k}}\right)=N\left({\mathbf{H}}_k{\overline{\mathbf{x}}}_{\mathbf{k}},{\mathbf{H}}_k{\overline{\mathbf{P}}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k\right)$$
注:

• ${\hat{\mathbf{x}}}_{k-1}$ 表示 $k-1$ 时刻系统状态的后验状态;
• ${\hat{\mathbf{P}}}_{k-1}$ 表示对应状态的后验方差；
• $\mathbf{Q}$ 和 $\mathbf{R}$ 分别 表示状态和观测噪声。

根据贝叶斯法则 $$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{z}}_{1:k}\right)\propto P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{z}}_{1:k-1}\right)$$, 将 $$P\left({\overline{\mathbf{x}}}_{\mathbf{k}}\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)$$ 和 $$P\left({\mathbf{z}}_k\mid {\overline{\mathbf{x}}}_{\mathbf{k}}\right)$$ 相乘, 得：
$$N\left({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k\right)=N\left({\mathbf{H}}_k{\overline{\mathbf{x}}}_{\mathbf{k}},{\mathbf{H}}_k{\overline{\mathbf{P}}}_k{\mathbf{H}}_k^T+{\mathbf{Q}}_k\right)N\left({\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k,{\mathbf{F}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_k^T+{\mathbf{R}}_k\right)$$
由此可得, 后验分布 $N\left({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k\right)$ 的均值和协方差矩阵：
$$\begin{array}{c}{\hat{\mathbf{x}}}_k={\overline{\mathbf{x}}}_k+\mathbf{K}\left({\mathbf{z}}_k-{\mathbf{H}}_k{\overline{\mathbf{x}}}_k\right)\\ {\hat{\mathbf{P}}}_k=\left(I-\mathbf{K}{\mathbf{H}}_k\right){\overline{\mathbf{P}}}_k\end{array}$$
其中, $\mathbf{K}={\overline{\mathbf{P}}}_k{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\overline{\mathbf{P}}}_k{\mathbf{H}}_k^T+{\mathbf{Q}}_k\right)}^{-1}$ 为卡尔曼增益。

3.总结

状态估计问题建模为：

$$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{z}}_{1:k}\right)\propto P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)P\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1}\right)$$
其中:

• $P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)$ 为似然项,可由观测方程给出
• $P\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1}\right)$ 为先验项,可通过状态转移方程推导得到

一句话总结就是贝叶斯法则+高斯融合：根据贝叶斯法则有，后验估计 $\propto$ 似然 * 先验 ，参考链接；然后根据假设（误差服从高斯分布），通过高斯分布的性质，将 似然项高斯分布 和 先验项高斯分布 相乘就得到了后验估计的分布。