Kalman滤波器--从高斯融合推导_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 = [ p k v k ] x_{k}=\left[

$$\begin{array}{l} p_{k} \\ v_{k} \end{array}$$ \right] xk​=[pk​vk​​]

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

P k = [ Σ p p Σ p v Σ v p Σ v v ] \boldsymbol{P}_{k}=\left[

$$\begin{array}{cc} \Sigma_{p p} & \Sigma_{p v} \\ \Sigma_{v p} & \Sigma_{v v} \end{array}$$ \right] Pk​=[Σpp​Σvp​​Σpv​Σvv​​]

其中:

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

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

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

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

x ‾ k = [ 1 Δ t 0 1 ] x ^ k − 1 = F k x ^ k − 1 \overline{\boldsymbol{x}}_{k}=\left[

$$\begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array}$$ \right] \widehat{\boldsymbol{x}}_{k-1}=\boldsymbol{F}_{k} \widehat{\boldsymbol{x}}_{k-1} xk​=[10​Δt1​]x k−1​=Fk​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$$贝叶斯法则以及高斯融合

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

Cov ⁡ ( x ^ k − 1 ) = P ^ k − 1 x ‾ k = F k x ^ k − 1 } ⇒ Cov ⁡ ( x ‾ k ) = Cov ⁡ ( F k x ^ k − 1 ) = F k P ^ k − 1 F k T \left.

$$\begin{array}{c} \operatorname{Cov}\left(\widehat{\boldsymbol{x}}_{k-1}\right)=\widehat{\boldsymbol{P}}_{k-1} \\ \overline{\boldsymbol{x}}_{k}=\boldsymbol{F}_{k} \widehat{\boldsymbol{x}}_{k-1} \end{array}$$ \right\} \Rightarrow \operatorname{Cov}\left(\overline{\boldsymbol{x}}_{k}\right)=\operatorname{Cov}\left(\boldsymbol{F}_{k} \widehat{\boldsymbol{x}}_{k-1}\right)=\boldsymbol{F}_{k} \widehat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k}^{T} Cov(x k−1​)=P k−1​xk​=Fk​x k−1​​}⇒Cov(xk​)=Cov(Fk​x k−1​)=Fk​P k−1​FkT​

即是：

$${\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}}$推导可得:

μ ′ = μ 0 + σ 0 2 ( μ 1 − μ 0 ) σ 0 2 + σ 1 2 σ ′ 2 = σ 0 2 − σ 0 4 σ 0 2 + σ 1 2

$$\begin{aligned} &\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{aligned}$$ ​μ′=μ0​+σ02​+σ12​σ02​(μ1​−μ0​)​σ′2=σ02​−σ02​+σ12​σ04​​​

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

μ ′ = μ 0 + K ( μ 1 − μ 0 ) σ ′ 2 = σ 0 2 − K σ 0 2

$$\begin{aligned} &\mu^{\prime}=\mu_{0}+K\left(\mu_{1}-\mu_{0}\right) \\ &\sigma^{\prime 2}=\sigma_{0}^{2}-K \sigma_{0}^{2} \end{aligned}$$ ​μ′=μ0​+K(μ1​−μ0​)σ′2=σ02​−Kσ02​​

将上式扩展到多维空间:

K = Σ 0 ( Σ 0 + Σ 1 ) − 1 μ ′ = μ 0 + K ( μ 1 − μ 0 ) Σ ′ = Σ 0 + K Σ 0

$$\begin{gathered} \boldsymbol{K}=\boldsymbol{\Sigma}_{0}\left(\boldsymbol{\Sigma}_{0}+\boldsymbol{\Sigma}_{1}\right)^{-1} \\ \boldsymbol{\mu}^{\prime}=\boldsymbol{\mu}_{\mathbf{0}}+\boldsymbol{K}\left(\boldsymbol{\mu}_{\mathbf{1}}-\boldsymbol{\mu}_{\mathbf{0}}\right) \\ \boldsymbol{\Sigma}^{\prime}=\boldsymbol{\Sigma}_{0}+\boldsymbol{K} \boldsymbol{\Sigma}_{0} \end{gathered}$$ K=Σ0​(Σ0​+Σ1​)−1μ′=μ0​+K(μ1​−μ0​)Σ′=Σ0​+KΣ0​​

---

回到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$ 的分布代入上式（高斯分布知识回顾里面的多维空间高斯分布融合公式）:
x ^ k = x ‾ k + K ( z k − x ‾ k ) P ^ k = P ‾ k + K P ‾ k \textcolor{blue}{

$$\begin{gathered} \widehat{\boldsymbol{x}}_{k}=\overline{\boldsymbol{x}}_{k}+\boldsymbol{K}\left(\mathbf{z}_{k}-\overline{\boldsymbol{x}}_{k}\right) \\ \widehat{\boldsymbol{P}}_{k}=\overline{\boldsymbol{P}}_{k}+\boldsymbol{K} \overline{\boldsymbol{P}}_{k} \end{gathered}$$ } x k​=xk​+K(zk​−xk​)P k​=Pk​+KPk​​
其中, $\mathbf{K}={\overline{\mathbf{P}}}_k{\left({\overline{\mathbf{P}}}_k+\mathbf{\Sigma }\right)}^{-1}$ 为卡尔曼增益。

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

---

当系统为线性马尔可夫系统时，可以通过Kalman Filter来求解融合问题。
{ x ‾ k = F k x ^ k − 1 + B k u k + w k z k = H k x ‾ k + v k k = 1 , 2 , ⋯   , N (7) \left\{

$$\begin{array}{c} \overline{\boldsymbol{x}}_{\boldsymbol{k}}=\boldsymbol{F}_{k} \widehat{\boldsymbol{x}}_{k-1}+\boldsymbol{B}_{k} \boldsymbol{u}_{k}+\boldsymbol{w}_{k} \\ \boldsymbol{z}_{k}=\boldsymbol{H}_{k} \overline{\boldsymbol{x}}_{\boldsymbol{k}}+\boldsymbol{v}_{k} \end{array}$$ \quad k=1,2, \cdots, N\right.\tag{7} {xk​=Fk​x k−1​+Bk​uk​+wk​zk​=Hk​xk​+vk​​k=1,2,⋯,N(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)$ 的均值和协方差矩阵：
x ^ k = x ‾ k + K ( z k − H k x ‾ k ) P ^ k = ( I − K H k ) P ‾ k \textcolor{blue}{

$$\begin{gathered} \widehat{\boldsymbol{x}}_{k}=\overline{\boldsymbol{x}}_{k}+\boldsymbol{K}\left(\mathbf{z}_{k}-\boldsymbol{H}_{k} \overline{\boldsymbol{x}}_{k}\right) \\ \widehat{\boldsymbol{P}}_{k}=\left(I-\boldsymbol{K} \boldsymbol{H}_{k}\right) \overline{\boldsymbol{P}}_{k} \end{gathered}$$ } x k​=xk​+K(zk​−Hk​xk​)P k​=(I−KHk​)Pk​​
其中, $\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$ 似然 * 先验 ，参考链接；然后根据假设（误差服从高斯分布），通过高斯分布的性质，将 似然项高斯分布 和 先验项高斯分布 相乘就得到了后验估计的分布。