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

0.引言
1.贝叶斯法则
2.kalman推导
3.总结

0.引言

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

1.贝叶斯法则

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

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

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

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

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

P(xk∣x0,z1:k)∝P(zk∣xk)P(xk∣xk−1)P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{z}_{1: k}\right) \propto P\left(\mathbf{z}_{k} \mid \boldsymbol{x}_{k}\right) P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}\right) P(xk∣x0,z1:k)∝P(zk∣xk)P(xk∣xk−1)
其中:

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

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

在状态估计时：

p(x∣y)=p(y∣x)p(x)p(y)p(\boldsymbol{x} \mid \boldsymbol{y})=\frac{p(\boldsymbol{y} \mid \boldsymbol{x}) p(\boldsymbol{x})}{p(\boldsymbol{y})} p(x∣y)=p(y)p(y∣x)p(x)

赋予该式物理意义：

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

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

2.kalman推导

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

xk=[pkvk]x_{k}=\left[\begin{array}{l} p_{k} \\ v_{k} \end{array}\right] xk=[pkvk]

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

Pk=[ΣppΣpvΣvpΣvv]\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]

其中:

Σpp\Sigma_{p p}Σpp 和 Σvv\Sigma_{v v}Σvv 为状态分量的方差
Σvp\Sigma_{v p}Σvp 和 Σpv\Sigma_{p v}Σpv 描述 ppp 和 vvv 之间协方差

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

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

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

x‾k=[1Δt01]x^k−1=Fkx^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]xk−1=Fkxk−1

其中:

x‾k\overline{\boldsymbol{x}}_{k}xk 为 kkk 时刻的先验分布
x^k−1\widehat{\boldsymbol{x}}_{k-1}xk−1 为 k−1k − 1k−1 时刻的后验分布
Fk\boldsymbol{F}_{k}Fk 为状态转移矩阵

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

x‾k=Fkx^k−1+Bkuk\overline{\boldsymbol{x}}_{k}=\boldsymbol{F}_{k} \widehat{\boldsymbol{x}}_{k-1}+\boldsymbol{B}_{k} \boldsymbol{u}_{k} xk=Fkxk−1+Bkuk

其中:

uk\boldsymbol{u}_{k}uk 表示外部输入
Bk\boldsymbol{B}_{k}Bk 表示外部输入与系统状态变化的转换关系矩阵

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

x‾k=Fkx^k−1+Bkuk+wk(1)\textcolor{blue}{\overline{\boldsymbol{x}}_{k}=\boldsymbol{F}_{k} \widehat{\boldsymbol{x}}_{k-1}+\boldsymbol{B}_{k} \boldsymbol{u}_{k}+\boldsymbol{w}_{k}}\tag{1} xk=Fkxk−1+Bkuk+wk(1)

其中:

wk∼N(0,Qk)\boldsymbol{w}_{k} \sim N\left(0, \boldsymbol{Q}_{k}\right)wk∼N(0,Qk) 为高斯噪声

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

Cov⁡(Ax)=AΣAT\operatorname{Cov}(\boldsymbol{A} \boldsymbol{x})=\boldsymbol{A} \boldsymbol{\Sigma} \boldsymbol{A}^{T} Cov(Ax)=AΣAT贝叶斯法则以及高斯融合

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

Cov⁡(x^k−1)=P^k−1x‾k=Fkx^k−1}⇒Cov⁡(x‾k)=Cov⁡(Fkx^k−1)=FkP^k−1FkT\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(xk−1)=Pk−1xk=Fkxk−1}⇒Cov(xk)=Cov(Fkxk−1)=FkPk−1FkT

即是：

P‾k=FkP^k−1FkT\overline{\boldsymbol{P}}_{k}=\boldsymbol{F}_{k} \widehat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k}^{T} Pk=FkPk−1FkT

考虑噪声的 x‾k\overline{\boldsymbol{x}}_{k}xk, 其协方差可记为：

P‾k=FkP^k−1FkT+Qk(2)\textcolor{blue}{\overline{\boldsymbol{P}}_{k}=\boldsymbol{F}_{k} \widehat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k}^{T}+\boldsymbol{Q}_{k}}\tag{2} Pk=FkPk−1FkT+Qk(2)

根据 k−1k − 1k−1 时刻的后验状态 x^k−1\widehat{\boldsymbol{x}}_{k-1}xk−1，我们可以预测出 kkk 时刻的先验状态 x‾k\overline{\boldsymbol{x}}_{k}xk 以及其协方差矩阵 p‾k\overline{\boldsymbol{p}}_{k}pk :
x‾k=Fkx^k−1+Bkuk+wk(1)\overline{\boldsymbol{x}}_{k}=\boldsymbol{F}_{k} \widehat{\boldsymbol{x}}_{k-1}+\boldsymbol{B}_{k} \boldsymbol{u}_{k}+\boldsymbol{w}_{k}\tag{1} xk=Fkxk−1+Bkuk+wk(1)

x‾k\overline{\boldsymbol{x}}_{k}xk 满足如下分布：

N(x‾k,P‾k)=N(Fkx^k−1+Bkuk,FkP^k−1FkT+Qk)(2)\textcolor{blue}{N\left(\overline{\boldsymbol{x}}_{k}, \overline{\boldsymbol{P}}_{k}\right)=N\left(\boldsymbol{F}_{k} \widehat{\boldsymbol{x}}_{k-1}+\boldsymbol{B}_{k} \boldsymbol{u}_{k}, \boldsymbol{F}_{k} \widehat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k}^{T}+\boldsymbol{Q}_{k}\right)}\tag{2} N(xk,Pk)=N(Fkxk−1+Bkuk,FkPk−1FkT+Qk)(2)

当获得 kkk 时刻的系统观测量 zk\boldsymbol{z}_kzk 时,可以尝试通过 zk\boldsymbol{z}_kzk 重新修正 kkk 时刻的后验状态 x^k\widehat{\boldsymbol{x}}_{k}xk 及其协方差矩阵 p^k\widehat{\boldsymbol{p}}_{k}pk.

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

zk=x‾k\boldsymbol{z}_k = \overline{\boldsymbol{x}}_{k}zk=xk

为了进一步泛化观测量 zk\boldsymbol{z}_kzk 与状态量 x‾k\overline{\boldsymbol{x}}_{k}xk 之间的关系,定义观测矩阵 Hk{\boldsymbol{H}}_{k}Hk:

zk=Hkx‾k(3)\boldsymbol{z}_k = {\boldsymbol{H}}_{k}\overline{\boldsymbol{x}}_{k}\tag{3}zk=Hkxk(3)
根据协方差矩阵的性质,可推导出观测量的方差为:

Σ=HkP‾kHkT(4)\boldsymbol{\Sigma}=\boldsymbol{H}_{k} \overline{\boldsymbol{P}}_{k} \boldsymbol{H}_{k}^{T}\tag{4} Σ=HkPkHkT(4)

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

zk=Hkx‾k+vk(5)\boldsymbol{z}_k={\boldsymbol{H}}_{k}\overline{\boldsymbol{x}}_{k} + \boldsymbol{v}_k \tag{5}zk=Hkxk+vk(5)

zk\boldsymbol{z}_kzk 满足如下分布：

N(zk,Σ)=N(Hkx‾k,HkP‾kHkT+Rk)(6)N\left(\boldsymbol{z}_{k}, \boldsymbol{\Sigma}\right)=N\left(\boldsymbol{H}_{k} \overline{\boldsymbol{x}}_{\boldsymbol{k}}, \boldsymbol{H}_{k} \overline{\boldsymbol{P}}_{k} \boldsymbol{H}_{k}^{T}+\boldsymbol{R}_{k}\right)\tag{6} N(zk,Σ)=N(Hkxk,HkPkHkT+Rk)(6)

其中,公式(2) 描述了 x‾k\overline{\boldsymbol{x}}_{k}xk 的分布,公式(6) 描述了 zk\boldsymbol{z}_kzk 的分布。

高斯分布知识回顾：

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

N(x,μ′,σ′)=N(x,μ0,σ0)⋅N(x,μ1,σ1)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,μ′,σ′)=N(x,μ0,σ0)⋅N(x,μ1,σ1)

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

μ′=μ0+σ02(μ1−μ0)σ02+σ12σ′2=σ02−σ04σ02+σ12\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=σ02σ02+σ12k = \frac{\sigma_{0}^{2}}{\sigma_{0}^{2}+\sigma_{1}^{2}}k=σ02+σ12σ02，上式可化简为：

μ′=μ0+K(μ1−μ0)σ′2=σ02−Kσ02\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推导

xˉk\bar{\boldsymbol{x}}_{k}xˉk 满足如下分布：

N(x‾k,P‾k)=N(Fkx^k−1+Bkuk,FkP^k−1FkT+Qk)(2)N\left(\textcolor{blue}{\overline{\boldsymbol{x}}_{k}, \overline{\boldsymbol{P}}_{k}}\right)=N\left(\boldsymbol{F}_{k} \widehat{\boldsymbol{x}}_{k-1}+\boldsymbol{B}_{k} \boldsymbol{u}_{k}, \boldsymbol{F}_{k} \widehat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k}^{T}+\boldsymbol{Q}_{k}\right)\tag{2} N(xk,Pk)=N(Fkxk−1+Bkuk,FkPk−1FkT+Qk)(2)
zk\mathbf{z}_{k}zk 满足如下分布:
N(zk,Σ)=N(Hkx‾k,HkP‾kHkT+Rk)(6)N\left(\textcolor{blue}{\mathbf{z}_{k}, \Sigma}\right)=N\left(\boldsymbol{H}_{\boldsymbol{k}} \overline{\boldsymbol{x}}_{\boldsymbol{k}}, \boldsymbol{H}_{k} \overline{\boldsymbol{P}}_{k} \boldsymbol{H}_{k}^{T}+\boldsymbol{R}_{k}\right)\tag{6} N(zk,Σ)=N(Hkxk,HkPkHkT+Rk)(6)
将 x‾k\overline{\boldsymbol{x}}_{k}xk 和 zk\boldsymbol{z}_{k}zk 的分布代入上式（高斯分布知识回顾里面的多维空间高斯分布融合公式）:
x^k=x‾k+K(zk−x‾k)P^k=P‾k+KP‾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}} xk=xk+K(zk−xk)Pk=Pk+KPk
其中, K=P‾k(P‾k+Σ)−1\boldsymbol{K}=\overline{\boldsymbol{P}}_{k}\left(\overline{\boldsymbol{P}}_{k}+\boldsymbol{\Sigma}\right)^{-1}K=Pk(Pk+Σ)−1 为卡尔曼增益。

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

当系统为线性马尔可夫系统时，可以通过Kalman Filter来求解融合问题。
{x‾k=Fkx^k−1+Bkuk+wkzk=Hkx‾k+vkk=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=Fkxk−1+Bkuk+wkzk=Hkxk+vkk=1,2,⋯,N(7)
由状态转移方程可得: P(x‾k∣x0,u1:k,z1:k−1)=N(Fkx^k−1+Bkuk,FkP^k−1FkT+Qk)P\left(\overline{\boldsymbol{x}}_{\boldsymbol{k}} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)=N\left(\boldsymbol{F}_{k} \widehat{\boldsymbol{x}}_{k-1}+\boldsymbol{B}_{k} \boldsymbol{u}_{k}, \boldsymbol{F}_{k} \widehat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k}^{T}+\boldsymbol{Q}_{k}\right)P(xk∣x0,u1:k,z1:k−1)=N(Fkxk−1+Bkuk,FkPk−1FkT+Qk)

由观测方程可得: P(zk∣x‾k)=N(Hkx‾k,HkP‾kHkT+Rk)P\left(\boldsymbol{z}_{k} \mid \overline{\boldsymbol{x}}_{\boldsymbol{k}}\right)=N\left(\boldsymbol{H}_{k} \overline{\boldsymbol{x}}_{\boldsymbol{k}}, \boldsymbol{H}_{k} \overline{\boldsymbol{P}}_{k} \boldsymbol{H}_{k}^{T}+\boldsymbol{R}_{k}\right)P(zk∣xk)=N(Hkxk,HkPkHkT+Rk)
注:

x^k−1\widehat{\boldsymbol{x}}_{k-1}xk−1 表示 k−1k-1k−1 时刻系统状态的后验状态;
P^k−1\widehat{\boldsymbol{P}}_{k-1}Pk−1 表示对应状态的后验方差；
Q\boldsymbol{Q}Q 和 R\boldsymbol{R}R 分别表示状态和观测噪声。

根据贝叶斯法则 P(xk∣x0,z1:k)∝P(zk∣xk)P(xk∣x0,z1:k−1)P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{z}_{1: k}\right) \propto P\left(\boldsymbol{z}_{k} \mid \boldsymbol{x}_{k}\right) P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{z}_{1: k-1}\right)P(xk∣x0,z1:k)∝P(zk∣xk)P(xk∣x0,z1:k−1), 将 P(x‾k∣x0,u1:k,z1:k−1)P\left(\overline{\boldsymbol{x}}_{\boldsymbol{k}} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)P(xk∣x0,u1:k,z1:k−1) 和 P(zk∣x‾k)P\left(\mathbf{z}_{k} \mid \overline{\boldsymbol{x}}_{\boldsymbol{k}}\right)P(zk∣xk) 相乘, 得：
N(x^k,P^k)=N(Hkx‾k,HkP‾kHkT+Qk)N(Fkx^k−1+Bkuk,FkP^k−1FkT+Rk)N\left(\widehat{\boldsymbol{x}}_{k}, \widehat{\boldsymbol{P}}_{k}\right)=N\left(\boldsymbol{H}_{k} \overline{\boldsymbol{x}}_{\boldsymbol{k}}, \boldsymbol{H}_{k} \overline{\boldsymbol{P}}_{k} \boldsymbol{H}_{k}^{T}+\boldsymbol{Q}_{k}\right) N\left(\boldsymbol{F}_{k} \widehat{\boldsymbol{x}}_{k-1}+\boldsymbol{B}_{k} \boldsymbol{u}_{k}, \boldsymbol{F}_{k} \widehat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k}^{T}+\boldsymbol{R}_{k}\right) N(xk,Pk)=N(Hkxk,HkPkHkT+Qk)N(Fkxk−1+Bkuk,FkPk−1FkT+Rk)
由此可得, 后验分布 N(x^k,P^k)N\left(\widehat{\boldsymbol{x}}_{k}, \widehat{\boldsymbol{P}}_{k}\right)N(xk,Pk) 的均值和协方差矩阵：
x^k=x‾k+K(zk−Hkx‾k)P^k=(I−KHk)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}} xk=xk+K(zk−Hkxk)Pk=(I−KHk)Pk
其中, K=P‾kHkT(HkP‾kHkT+Qk)−1\boldsymbol{K}=\overline{\boldsymbol{P}}_{k} \boldsymbol{H}_{k}^{T}\left(\boldsymbol{H}_{k} \overline{\boldsymbol{P}}_{k} \boldsymbol{H}_{k}^{T}+\boldsymbol{Q}_{k}\right)^{-1}K=PkHkT(HkPkHkT+Qk)−1 为卡尔曼增益。

3.总结

状态估计问题建模为：

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

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