卡尔曼滤波

1、递归算法

1、引出估计问题

由一个硬币测量问题推导出
$$\begin{array}{rl}{\hat{x}}_k& =\frac{1}{k}({\hat{x}}_1+{\hat{x}}_2+\cdots +{\hat{x}}_{k-1}+Z_k)\\ & =\frac{1}{k}({\hat{x}}_1+{\hat{x}}_2+\cdots +{\hat{x}}_{k-1}+Z_k)\\ & =\frac{1}{k}({\hat{x}}_1+{\hat{x}}_2+\cdots +{\hat{x}}_{k-1})+\frac{1}{k}{Z}_k\\ & =\frac{1}{k}\frac{k-1}{k-1}({\hat{x}}_1+{\hat{x}}_2+\cdots +{\hat{x}}_{k-1})+\frac{1}{k}{Z}_k\\ & =\frac{k-1}{k}{\hat{x}}_{k-1}+\frac{1}{k}{Z}_k\\ & ={\hat{x}}_{k-1}+\frac{1}{k}(Z_k-{\hat{x}}_{k-1})\end{array}$$
其中$\frac{1}{k}$可以定义为一个不断更新的系数$K_k$,上述式子表征为
$${\hat{x}}_k={\hat{x}}_{k-1}+K_k(Z_k-{\hat{x}}_{k-1})$$
定义这个不断更新的系数为卡尔曼系数
$$K_k=\frac{e_{ES{T}_{k-1}}}{e_{ES{T}_{k-1}}+e_{MS{A}_k}}$$
当估计误差 $e_{EST}$ 很大时,$K_k$接近1，${\hat{x}}_k$取测量值$Z_k$,

当测量误差 $e_{EST}$ 很大时,$K_k$接近0，${\hat{x}}_k$取估计值${\hat{x}}_{k-1}$忽略测量值$Z_k$.

具体为什么这么定义接下来会详细推导。

2、应用一下

step1 计算卡尔曼系数
$$K_k=\frac{e_{ES{T}_{k-1}}}{e_{ES{T}_{k-1}}+e_{MS{A}_k}}$$
step2 计算当前估计值
$${\hat{x}}_k={\hat{x}}_{k-1}+K_k(Z_k-{\hat{x}}_{k-1})$$
step3 更新估计误差$$e_{ES{T}_k}$$
$$e_{ES{T}_k}=(1-K_k)e_{ES{T}_{k-1}}$$

2、数学基础

1. 数据融合

   拿两个误差不一样的秤去称一个30kg的物品，秤1称出来的结果为$Z_1=30kg$,称1的方差为${\delta }_1=2$ ，秤2称出来的结果为$Z_2=34kg$，秤2的方差为${\delta }_2=4$，问：根据两个秤的测量值如何得到一个更准确的估计值$\hat{Z}$？

   估计值$\hat{Z}$ ：
   $$\hat{Z}=Z_1+K(Z_2-Z_1)$$
   估计值的方差$Var(\hat{Z})$为
   $$\begin{array}{rl}Var(\hat{Z})& =Var(Z_1+K(Z_2-Z_1))\\ & =Var(Z_1(1-K)+K{Z}_2)\\ & =(1-K{)}^{2}Var(Z_1)+K^{2}Var(Z_2)\\ & ={\delta }_1^2(1-K{)}^2+{\delta }_2^2{K}^2\end{array}$$
   根据上一章的方法，我们求一个最优的$$K$$，使得估计值的方差$Var(\hat{Z})$最小
   $$\begin{array}{c}\begin{array}{rl}\underset{K}{\min }& Var(\hat{Z})\end{array}\end{array}$$
   当$\dot{Var(\hat{Z})}=0$时最小
   $$\begin{array}{rl}-8(1-K)+32K& =0\\ K& =0.2\end{array}$$
   带入$$Var(\hat{Z})=4\ast 0.64+16\ast 0.04=3.2$$，$$\hat{Z}=30.8kg$$，$${\delta }_3=1.79$$，

2. 协方差矩阵

   方差：
   $$Var(x)={\delta }_x^2=\frac{1}{n}((x_1-\overline{x}{)}^2+(x_2-\overline{x}{)}^2+\cdots +(x_n-\overline{x}{)}^2)$$
   协方差：
   $$Var(xy)={\delta }_x{\delta }_y=\frac{1}{n}((x_1-\overline{x})(y_1-\overline{y})+(x_2-\overline{x})(y_2-\overline{y})+\cdots +(x_n-\overline{x})(y_n-\overline{y}))$$
   协方差矩阵：
   $$P=\left[\begin{array}{ccc}{\delta }_x^2& {\delta }_x{\delta }_y& {\delta }_x{\delta }_z\\ {\delta }_x{\delta }_y& {\delta }_y^2& {\delta }_y{\delta }_z\\ {\delta }_z{\delta }_x& {\delta }_z{\delta }_y& {\delta }_z^2\end{array}\right]$$
   写代码的时候如何求协方差矩阵：

   先求出过度矩阵
   $$a=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ ⋮& ⋮& ⋮\\ x_n& y_n& z_n\end{array}\right]-\frac{1}{n}\left[\begin{array}{ccc}1& 1& 1\\ 1& 1& 1\\ ⋮& ⋮& ⋮\\ 1& 1& 1\end{array}\right]\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ ⋮& ⋮& ⋮\\ x_n& y_n& z_n\end{array}\right]$$
   然后求协方差矩阵
   $$P=\frac{1}{n}{a}^{T}a$$

3. 状态空间方程

   状态空间方程
   $$\begin{gathered} x_k=A{x}_{k-1}+B{u}_{k-1}+{\omega }_{k-1} \\ {Z}_k=H{x}_k+{\nu }_k \end{gathered}$$
   其中${\omega }_{k-1}$是过程误差，${\nu }_k$是测量误差。

3、卡尔曼增益详细推导

模型

有状态空间方程
$$\begin{gathered} x_k=A{x}_{k-1}+B{u}_{k-1}+{\omega }_{k-1} \\ {Z}_k=H{x}_k+{\nu }_k \end{gathered}$$
其中${\omega }_{k-1}$是过程误差，服从正态分布$p(\omega )=(0,Q)$，$Q$是协方差矩阵，$Q=\mathbb{E}(\omega {\omega }^T)$，${\nu }_k$是测量误差，也服从正态分布$p(\nu )=(0,R)$，$R=\mathbb{E}(\nu {\nu }^T)$。

​ 为什么$Q=\mathbb{E}(\omega {\omega }^T)$？？？

假设
$$\omega =\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\end{array}\right]$$
那么
$$Q=\mathbb{E}(\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\end{array}\right]\left[\begin{array}{cc}{\omega }_1& {\omega }_2\end{array}\right])=\mathbb{E}\left[\begin{array}{cc}{\omega }_1^2& {\omega }_1{\omega }_2\\ {\omega }_1{\omega }_2& {\omega }_2^2\end{array}\right]=\left[\begin{array}{cc}\mathbb{E}({\omega }_1^2)& \mathbb{E}({\omega }_1{\omega }_2)\\ \mathbb{E}({\omega }_1{\omega }_2)& \mathbb{E}({\omega }_2^2)\end{array}\right]$$
对于${\omega }_1$的方差$Var({\omega }_1)=\mathbb{E}(({\omega }_1-\mathbb{E}({\omega }_1){)}^2)=\mathbb{E}[{\omega }_1^2]-(\mathbb{E}[{\omega }_1]{)}^2$，其中$\mathbb{E}[{\omega }_1]=0$，因此$Var({\omega }_1)=\mathbb{E}[{\omega }_1^2]={\delta }_{{\omega }_1}^2$。

因此
$$Q=\left[\begin{array}{cc}\mathbb{E}({\omega }_1^2)& \mathbb{E}({\omega }_1{\omega }_2)\\ \mathbb{E}({\omega }_1{\omega }_2)& \mathbb{E}({\omega }_2^2)\end{array}\right]=\left[\begin{array}{cc}{\delta }_{{\omega }_1}^2& {\delta }_{{\omega }_1}{\delta }_{{\omega }_2}\\ {\delta }_{{\omega }_1}{\delta }_{{\omega }_2}& {\delta }_{{\omega }_2}^2\end{array}\right]$$
注：为什么$Var({\omega }_1)=\mathbb{E}[{\omega }_1^2]-(\mathbb{E}[{\omega }_1]{)}^2$？？？

定义方差为：
$$\text{Var}(X)=\mathbb{E}[(X-\mathbb{E}[X]{)}^2]$$
展开括号：
$$(X-\mathbb{E}[X]{)}^2=X^2-2X\mathbb{E}[X]+(\mathbb{E}[X]{)}^2$$
对两边取期望：
$$\text{Var}(X)=\mathbb{E}[X^2]-2\mathbb{E}[X]\mathbb{E}[X]+(\mathbb{E}[X]{)}^2$$
由于期望是线性的：
$$\mathbb{E}[X\mathbb{E}[X]]=\mathbb{E}[X]\cdot \mathbb{E}[X]=(\mathbb{E}[X]{)}^2$$
因此：
$$\text{Var}(X)=\mathbb{E}[X^2]-(\mathbb{E}[X]{)}^2$$

已知

算出来的：先验
$${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_k$$
测出来的：测量
$$Z_k^{-}=H{x}_k\to {\hat{x}}_{kmea}=H^{-}{Z}_k$$

如何用两个不准的结果估计出一个准确值？？？

后验
$${\hat{x}}_k={\hat{x}}_k^{-}+G(H^{-}{Z}_k-{\hat{x}}_k^{-})$$
其中的$G=K_{k}H$

因此后验还可写成
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k(Z_k-H{\hat{x}}_k^{-})$$
直观理解：当先验的估计误差比较大的时候$K_k$取$H^{-}$，此时后验估计的值为${\hat{x}}_{kmea}$

，当先验测量的误差比较大时$K_k$取$0$，此时后验估计的值为${\hat{x}}_k^{-}$

找一个最优的$K_k$，估计出一个准确值

用误差来量化准确这个概念
$$e_k=x_k-{\hat{x}}_k$$
$e_k$的期望为0，因此当$e_k$的方差最小时，误差最小，$trP(e_k)$

问题建模：
$$\begin{array}{rl}\underset{K_k}{\min }& trP(e_k)\end{array}$$
前置处理
$$\begin{array}{rl}P(e_k)& =\mathbb{E}(e_k{e_k}^T)\\ & =(x_k-\widehat{x_k})(x_k-\widehat{x_k}{)}^T\end{array}$$
将$(18)$和$(20)$带入$(22)$得到
$$e_k=x_k-A{\hat{x}}_{k-1}-B{u}_k-K_k(Z_k-H{\hat{x}}_k^{-})$$
带入$(14)$得到
$$\begin{array}{rl}{e}_k& =x_k-{\hat{x}}_k^{-}-K_k(H{x}_k+{\nu }_k-H{\hat{x}}_k^{-})\\ & =(I-K_{k}H)x_k-K_k{\nu }_k-{\hat{x}}_k^{-}+K_{k}H{\hat{x}}_k^{-}\\ & =(I-K_{k}H)(x_k-{\hat{x}}_k^{-})-K_k{\nu }_k\end{array}$$
把$x_k-{\hat{x}}_k^{-}$定义为先验误差$e_k^{-}$
$$e_k=(I-K_{k}H)e_k^{-}-K_k{\nu }_k$$
将式子$(24)$带入到$P(e_k)$中
$$\begin{array}{rl}P(e_k)& =\mathbb{E}(e_k{e_k}^T)\\ & =\mathbb{E}[(x_k-\widehat{x_k})(x_k-\widehat{x_k}{)}^T]\\ & =\mathbb{E}[[(I-K_{k}H)e_k^{-}-K_k{\nu }_k][(I-K_{k}H)e_k^{-}-K_k{\nu }_k{]}^T]\\ & =\mathbb{E}[(I-K_{k}H)e_k^{-}-K_k{\nu }_k][{e_k^{-}}^T(I-K_{k}H{)}^T-{\nu }_k^T{K}_k^T]\\ & =\mathbb{E}[(I-K_{k}H)e_k^{-}{e_k^{-}}^T(I-K_{k}H{)}^T-(I-K_{k}H)e_k^{-}{\nu }_k^T{K}_k^T-K_k{\nu }_k{e_k^{-}}^T(I-K_{k}H{)}^T+K_k{\nu }_k{\nu }_k^T{K}_k^T]\end{array}$$
其中$\mathbb{E}((I-K_{k}H)e_k^{-}{\nu }_k^T{K}_k^T)=(I-K_{k}H)\mathbb{E}(e_k^{-})\mathbb{E}({\nu }_k^T)K_k^T$，$\mathbb{E}(e_k^{-})=0$和$\mathbb{E}({\nu }_k^T)=0$且相互独立，因此
$$P(e_k)=\mathbb{E}[(I-K_{k}H)e_k^{-}{e_k^{-}}^T(I-K_{k}H{)}^T+K_k{\nu }_k{\nu }_k^T{K}_k^T]$$
令$P(e_k{)}^{-}=\mathbb{E}[e_k^{-}{e_k^{-}}^T]$，对$P(e_k)$进行展开
$$\begin{array}{rl}P(e_k)& =(I-K_{k}H)P(e_k{)}^{-}(I-K_{k}H{)}^T+K_k{\nu }_k{\nu }_k^T{K}_k^T\\ & =(P(e_k{)}^{-}-K_{k}HP(e_k{)}^{-})(I-K_{k}H{)}^T+K_k{\nu }_k{\nu }_k^T{K}_k^T\\ & =P(e_k{)}^{-}-P(e_k{)}^{-}{H}^T{K}_k^T-K_{k}HP(e_k{)}^{-}+K_{k}HP(e_k{)}^{-}{H}^T{K}_k^T+K_k\mathbb{E}[{\nu }_k{\nu }_k^T]K_k^T\end{array}$$
然后我们求$P(e_k)$的迹
$$\begin{array}{rl}trP(e_k)& =trP(e_k{)}^{-}-2tr(P(e_k{)}^{-}{H}^T{K}_k^T)+tr(K_{k}HP(e_k{)}^{-}{H}^T{K}_k^T)+tr(K_k\mathbb{E}[{\nu }_k{\nu }_k^T]K_k^T)\\ & =trP(e_k{)}^{-}-2tr(K_{k}HP(e_k{)}^{-})+tr(K_{k}HP(e_k{)}^{-}{H}^T{K}_k^T)+tr(K_{k}R{K}_k^T)\end{array}$$
然后求令$P(e_k)$的迹最小的$K_k$，令
$$\frac{dtrP(e_k)}{d{K}_k}=0$$

$$\begin{gathered} \frac{dtrP(e_k)}{d{K}_k}=0-2(HP(e_k{)}^{-}{)}^T+2K_{k}HP(e_k{)}^{-}{H}^T+2K_{k}R=0 \\ \to -(HP(e_k{)}^{-}{)}^T+K_{k}HP(e_k{)}^{-}{H}^T+K_{k}R=0 \\ \to K_k=\frac{(HP(e_k{)}^{-}{)}^T}{HP(e_k{)}^{-}{H}^T+R} \end{gathered}$$

注：$\frac{dtr(AB)}{dA}=B^T$，$\frac{dtr(AB{A}^T)}{dA}=A（B+B^T）$

结论

当
$$K_k=\frac{(HP(e_k{)}^{-}{)}^T}{HP(e_k{)}^{-}{H}^T+R}$$
时误差最小。

直观理解：当$R$测量的协方差很大时，$K_k$取0此时取估计的结果，当$R$很小时，K_k$取$$H^{-}$此时取测量的结果

4、卡尔曼滤波器公式推导

已知状态空间方程
$$\begin{gathered} x_k=A{x}_{k-1}+B{u}_{k-1}+{\omega }_{k-1} \\ {Z}_k=H{x}_k+{\nu }_k \end{gathered}$$
先验
$${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}$$
卡尔曼增益
$$K_k=\frac{(HP(e_k{)}^{-}{)}^T}{HP(e_k{)}^{-}{H}^T+R}$$
求后验
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k(Z_k-H{\hat{x}}_k^{-})$$
其中的$P(e_k{)}^{-}$未知，求$P(e_k{)}^{-}$
$$P(e_k{)}^{-}=\mathbb{E}[e_k^{-}{e_k^{-}}^T]$$
$e_k^{-}$定义为
$$e_k^{-}=x_k-{\hat{x}}_k^{-}$$
带入式$(34)$和$(35)$得
$$\begin{array}{rl}{e}_k^{-}& =x_k-{\hat{x}}_k^{-}\\ & =A{x}_{k-1}+B{u}_{k-1}+{\omega }_{k-1}-A{\hat{x}}_{k-1}-B{u}_{k-1}\\ & =A(x_{k-1}-{\hat{x}}_{k-1})+{\omega }_{k-1}\end{array}$$
将$e_k^{-}$带入到$P(e_k^{-})$得
$$\begin{array}{rl}P(e_k{)}^{-}& =\mathbb{E}[e_k^{-}{e_k^{-}}^T]\\ & =\mathbb{E}[(A(x_{k-1}-{\hat{x}}_{k-1})+{\omega }_{k-1})(A(x_{k-1}-{\hat{x}}_{k-1})+{\omega }_{k-1}{)}^T]\\ & =\mathbb{E}[A(x_{k-1}-{\hat{x}}_{k-1})(x_{k-1}-{\hat{x}}_{k-1}{)}^T{A}^T+A(x_{k-1}-{\hat{x}}_{k-1}){\omega }_{k-1}^T+{\omega }_{k-1}(x_{k-1}-{\hat{x}}_{k-1}{)}^T{A}^T+{\omega }_{k-1}{\omega }_{k-1}^T]\\ & =AP(e_{k-1})A^T+\mathbb{E}[A(e_{k-1}){\omega }_{k-1}^T]+\mathbb{E}[{\omega }_{k-1}(e_{k-1}{)}^T{A}^T]+\mathbb{E}[{\omega }_{k-1}{\omega }_{k-1}^T]\\ & =AP(e_{k-1})A^T+Q\end{array}$$
注：$e_{k-1}=x_{k-1}-{\hat{x}}_{k-1}$，$P(e_{k-1})=\mathbb{E}[e_{k-1}{e_{k-1}}^T]$，$\mathbb{E}[A(e_{k-1}){\omega }_{k-1}^T]=\mathbb{E}[{\omega }_{k-1}(e_{k-1}{)}^T{A}^T]=0$

，${\omega }_{k-1}$和$e_{k-1}$相互独立。

结论:

1、先预测

先验
$${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}$$
先验误差的协方差
$$P(e_k{)}^{-}=AP(e_{k-1})A^T+Q$$
2、后矫正

卡尔曼增益
$$K_k=\frac{(HP(e_k{)}^{-}{)}^T}{HP(e_k{)}^{-}{H}^T+R}$$
求后验
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k(Z_k-H{\hat{x}}_k^{-})$$
3、然后为下一次的预测准备

计算$P(e_k)$
$$\begin{array}{rl}P(e_k)& =P(e_k{)}^{-}-P(e_k{)}^{-}{H}^T{K}_k^T-K_{k}HP(e_k{)}^{-}+K_{k}HP(e_k{)}^{-}{H}^T{K}_k^T+K_k\mathbb{E}[{\nu }_k{\nu }_k^T]K_k^T\\ & =P(e_k{)}^{-}-P(e_k{)}^{-}{H}^T{K}_k^T-K_{k}HP(e_k{)}^{-}+K_k(HP(e_k{)}^{-}{H}^T+R)K_k^T\\ & =P(e_k{)}^{-}-P(e_k{)}^{-}{H}^T{K}_k^T-K_{k}HP(e_k{)}^{-}+P(e_k{)}^{-}{H}^T{K}_k^T\\ & =P(e_k{)}^{-}-K_{k}HP(e_k{)}^{-}\\ & =(I-K_{k}H)P(e_k{)}^{-}\end{array}$$
其中的$R$和$Q$矩阵未知，需要根据实际传感器调优。

5、扩展卡尔曼滤波

正态分布的随机变量经过非线性系统之后就不在服从正态分布了

对于一个非线性系统如何估计出一个准确值？？

非线性系统
$$\begin{gathered} x_k=f(x_{k-1},u_{k-1},{\omega }_{k-1}) \\ {Z}_k=h(x_k,{\nu }_k) \end{gathered}$$
其中过程噪声 ${\mathbf{w}}_{k-1}$ 被视为状态的一个附加输入，其线性化效果也需要考虑。这时对整个状态转移方程进行线性化：
$$f({\mathbf{x}}_{k-1}+\delta {\mathbf{x}}_{k-1},{\mathbf{u}}_{k-1})+{\mathbf{w}}_{k-1}\approx f({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_{k-1})+{\mathbf{A}}_{k-1}(x_{k-1}-{\hat{x}}_{k-1})+{\mathbf{W}}_{k-1}{\mathbf{w}}_{k-1}$$
其中：

• $\mathbf{W}{k-1} = \left. \frac{\partial f}{\partial \mathbf{w}} \right|{\mathbf{x} = \hat{\mathbf{x}}_{k-1}} $是对噪声的灵敏度矩阵（通常为单位矩阵，假设噪声直接叠加在状态上）。
• 其中 ${\mathbf{A}}_{k-1}$ 是状态转移函数的雅可比矩阵：${\mathbf{A}}_{k-1}={\frac{\partial f}{\partial \mathbf{x}}|}_{\mathbf{x}={\hat{\mathbf{x}}}_{k-1},\mathbf{u}={\mathbf{u}}_{k-1}}$

于是，状态预测模型可以线性化为：
$${\mathbf{x}}_k=f({\tilde{\mathbf{x}}}_{k-1},{\mathbf{u}}_{k-1},0)+{\mathbf{A}}_{k-1}({\mathbf{x}}_{k-1}-{\tilde{\mathbf{x}}}_{k-1})+{\mathbf{W}}_{k-1}{\mathbf{w}}_{k-1},$$
$f({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_{k-1},0)=\widetilde{x_k}$

然后对测量结果进行线性化

类似地，对于观测函数 $h({\mathbf{x}}_k)$，在当前预测点${\tilde{\mathbf{x}}}_k$ 附近展开：
$$Z_k=h({\tilde{\mathbf{x}}}_k,0)+{\mathbf{H}}_k({\mathbf{x}}_k-{\tilde{\mathbf{x}}}_k)+V{\nu }_k,$$
其中：
$$\begin{gathered} {\mathbf{H}}_k={\frac{\partial h}{\partial \mathbf{x}}|}_{\mathbf{x}={\tilde{\mathbf{x}}}_k} \\ {\mathbf{V}}_k={\frac{\partial h}{\partial \nu }|}_{\mathbf{x}={\tilde{\mathbf{x}}}_k} \end{gathered}$$
是观测函数的雅可比矩阵。

线性化之后

$$\begin{gathered} {\mathbf{x}}_k=f({\tilde{\mathbf{x}}}_{k-1},{\mathbf{u}}_{k-1},0)+{\mathbf{A}}_{k-1}({\mathbf{x}}_{k-1}-{\tilde{\mathbf{x}}}_{k-1})+{\mathbf{W}}_{k-1}{\omega }_{k-1}, \\ {Z}_k=h({\tilde{\mathbf{x}}}_k,0)+{\mathbf{H}}_k({\mathbf{x}}_k-{\tilde{\mathbf{x}}}_k)+V{\nu }_k, \end{gathered}$$

其中$P({\omega }_{(}k-1))=(0,Q)$，$P(W\omega )=(0,WQ{W}^T)$，$P(V\nu )=(0,V\nu V^T)$

扩展卡尔曼滤波

1、先预测

先验
$${\hat{x}}_k^{-}=f({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_{k-1},0)$$
先验误差的协方差
$$P(e_k{)}^{-}=AP(e_{k-1})A^T+WQ{W}^T$$
2、后矫正

卡尔曼增益
$$K_k=\frac{(HP(e_k{)}^{-}{)}^T}{HP(e_k{)}^{-}{H}^T+VR{V}^T}$$
求后验
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k(Z_k-h({\tilde{x}}_k,0))$$
3、然后为下一次的预测准备

计算$P(e_k)$
$$\begin{array}{rl}P(e_k)& =(I-K_{k}H)P(e_k{)}^{-}\end{array}$$