卡尔曼滤波系列推导

机器人的状态估计

1.线性高斯系统的状态估计

离散时间的批量估计

• 贝叶斯推断：从状态的先验概率密度函数出发，通过初始状态、输入、运动方程和观测数据，计算状态的后验概率密度函数。

• 最大后验估计：用优化理论，寻找给定信息下的最大后验估计。

• 运动方程 ，观测方程如下：

  运动方程：$$x_k=A_{k-1}{x}_{k-1}+v_k+{\omega }_k,k=1,.....K\text{\hspace{1em}(1)}$$

  观测方程： $$y_k=C_k{x}_k+n_k,k=0....K\text{\hspace{1em}(2)}$$

最大后验估计（MAP）

目的 $$\hat{x}=\underset{x}{\mathrm{argmax}}p(x|v,y)\text{\hspace{1em}(3)}$$

由贝叶斯定理，将（3）式写为，$$\hat{x}=\arg \underset{x}{\max }p(y|x)p(x|v)\text{\hspace{1em}(4)}$$

进行因子分解：

​ $p(y|x)=\prod _{k=0}^{K}p(y_k|x_k)$

​ $p(x|v)=p(x_0|{\check{x}}_0)\prod _{k=1}^{K}p(x_k|,x_{k-1},v_k)$

对公式(4) $\hat{x}$ 取对数,去掉一些与x值无关的项。得到$J_{v,k}和J_{y,k}$ 称为平方马氏距离。并得到目标函数 $J(x)$ 。

$$J(x)=\sum _{k=0}^K{J}_{V,K}(x)+J_{y,k}(x)\text{\hspace{1em}(5)}$$

此时问题转化为了 $$\check{x}=\arg \underset{x}{\min }J(x)\text{\hspace{1em}(6)}$$

这是一个无约束的优化问题 ，式子中所有的项都是x的二次形式，所以把所有的数据排成一列，即提升形式 。

$$(H^T{W}^{-1}H)\hat{x}=H^T{W}^{-1}z\text{\hspace{1em}(7)}$$

批量最小二乘解。

$z=\left[\begin{array}{c}\check{x}\\ v_1\\ ⋮\\ v_k\\ ---\\ y_0\\ ⋮\\ y_K\end{array}\right]$ $x=\left[\begin{array}{c}{x}_0\\ ⋮\\ x_K\end{array}\right]$ $H=\left[\begin{array}{cccc}1& & & \\ -A_0& 1& & \\ & \ddots & \ddots & \\ & & -A_{K-1}& 1\\ -& -& -& -\\ C_0& & & \\ & C_1& & \\ & & \ddots & \\ & & & C_K\end{array}\right]$

$W=\left[\begin{array}{ccccccccc}{\check{P}}_0& & & & |& & & & \\ & Q_1& & & |& & & & \\ & & \ddots & & |& & & & \\ & & & Q_K& |& & & & \\ -& -& -& -& -& -& -& -& -\\ & & & & |& R_0& & & \\ & & & & |& & R_1& & \\ & & & & |& & & \ddots & \\ & & & & |& & & & R_K\end{array}\right]$

贝叶斯推断

目的：如何建立后验概率$p(x|v,y)$ 。

将运动方程（1）写为提升形式： $x=A(v+w)$ （将$x_1,x_2,....x_k$ 递归代入）

提升之后，均值：$\check{x}=E(x)=Av$ ，协方差：$\check{P}=AQ{A}^T$

那么先验就可以简洁的写出：$$p(x|v)=N(\check{x},\check{P})=N(Av,AQ{A}^T)\text{\hspace{1em}(8)}$$

观测方程(2)写为提升形式：$y=Cx+n$

则此时，状态、观测的联合概率密度函数可以写为：$$p(x,y|v)=N(\left[\begin{array}{c}\check{x}\\ C\check{x}\end{array}\right],\left[\begin{array}{cc}\check{P}& \check{P}{C}^T\\ C\check{P}& C\check{P}{C}^T+R\end{array}\right])\text{\hspace{1em}(9)}$$

（因为已知 $v$ ，所以 $x$ 的先验分布也已经确定了）

可将$p(x,y|v)$分解为： $$p(x,y|v)=p(x|y,v)\times p(y|v)\text{\hspace{1em}(10)}$$

我们关心 $p(x|y,v)$ 表示了全贝叶斯的后验概率公式。根据高斯概率联合密度可写为： $$p(x|y,v)=N(\check{x}+\check{P}{C}^T(C\check{P}{C}^T+R^{-1}{)}^{-1}(y-C\check{x}),\check{P}-\check{P}{C}^T(C\check{P}{C}^T+R{)}^{-1}C\check{P})\text{\hspace{1em}(11)}$$

根据**SMW（矩阵求逆引理）**恒等式：

$$(A^{-1}+B{D}^{-1}C{)}^{-1}\equiv A-AB(D+CAB{)}^{-1}CA$$

$(D+CAB{)}^{-1}\equiv D^{-1}-D^{-1}C(A^{-1}+B{D}^{-1}C{)}^{-1}B{D}^{-1}$

$AB(D+CAB{)}^{-1}\equiv (A^{-1}+B{D}^{-1}C{)}^{-1}B{D}^{-1}$

$(D+CAB{)}^{-1}CA\equiv D^{-1}C(A^{-1}+B{D}^{-1}C{)}^{-1}$

可将式（11）转换成：

$$p(x|y，v)=N⟨(\check{P}+C^T{R}^{-1}C{)}^{-1}({\check{P}}^{-1}\check{x+C^T{R}^{-1}y}),({\check{P}}^{-1}+C^T{R}^{-1}C{)}^{-1}⟩\text{\hspace{1em}(12)}$$

上述公式，前面的一项为 $\hat{x}$ ，后面的一项为 $\hat{P}$ 。

$(\check{P}+C^T{R}^{-1}C{)}^{-1}({\check{P}}^{-1}\check{x+C^T{R}^{-1}y})=\hat{x}$

$({\check{P}}^{-1}+C^T{R}^{-1}C{)}^{-1}=\hat{P}$

变形为：

$(\check{P}+C^T{R}^{-1}C)\hat{x}={\hat{P}}^{-1}\hat{x}={\check{P}}^{-1}\check{x+C^T{R}^{-1}y}$

此时将 $\check{x}=Av$ 和$\check{P}=(AQ{A}^T{)}^{-1}=A^{-T}{Q}^{-1}{A}^{-1}$ 代入上式: $$(A^{-T}{Q}^{-1}{A}^{-1}{C}^T{R}^{-1}C)\hat{x}={\hat{P}}^{-1}\hat{x}=A^{-T}{Q}^{-1}v+C^T{R}^{-1}y\text{\hspace{1em}(13)}$$

定义 $z=\left[\begin{array}{c}v\\ y\end{array}\right]$ $H=\left[\begin{array}{c}{A}^{-1}\\ C\end{array}\right]$ $W=\left[\begin{array}{cc}Q& \\ & R\end{array}\right]$

则公式(13)写为: $$(H^T{W}^{-1}H)\hat{x}=H^T{W}^{-1}z\text{\hspace{1em}(14)}$$ 。

离散时间的递归平滑算法

Cholesky 平滑算法

由公式（14）得出: $H^T{W}^{-1}H$ 是一个三对角块，采用一种方式Cholesky分解 需要一次前向和后向迭代，可以将三对角块分解为：

​ $$H^T{W}^{-1}H=L{L}^T\text{\hspace{1em}(15)}$$

求解公式（16）得到 d：

​ $$Ld=H^T{W}^{-1}z\text{\hspace{1em}(16)}$$

由公式（14）知： $$d=L^T\hat{x}\text{\hspace{1em}(17)}$$ 求解得到 $\hat{x}$ 。

前向过程为(15)、(16)，后向过程为（17）先求 $X_K->X_{K-1}->....$。

综合前向和后向两步，我们综合的写为:

前向：$k=1.....K$

$$L_{k-1}{L}_{k-1}^T=I_{k-1}+A_{k-1}^T{Q}_k^{-1}{A}_{k-1}\text{\hspace{1em}(18a)}$$

$$L_{k,k-1}{L}_{k_1}^T=-Q_k^{-1}{A}_{k-1}\text{\hspace{1em}(18b)}$$

​$$L_{k-1}{d}_{k-1}=q_{k-1}-A_{k-1}^T{Q}_k^{-1}{v}_k\text{\hspace{1em}(18c)}$$

$$I_k=-L_{k,k-1}{L}_{k,k-1}^T+Q_k^{-1}+C_k^T{R}_k^{-1}{C}_k\text{\hspace{1em}(18d)}$$

$$q_k=-L_{k,k-1}{d}_{k-1}+Q_k^{-1}{v}_k+C_k^T{R}_k^{-1}{y}_k\text{\hspace{1em}(18e)}$$

后向：$k=K.....1$

$$L_{K-1}^T{\hat{x}}_{k-1}=-L_{k,k-1}^T\hat{x}+d_{k-1}\text{\hspace{1em}(18f)}$$

这六个递归方程，在代数上等价于传统的Rauch-Tung-Striebel 平滑算法；而五个前向迭代，则等价于著名的卡尔曼滤波器。

Rauch-Tung-Striebel 平滑算法

Cholesky 平滑算法不是平滑算法的标准方程形式。

将公式（18b）中的 $L_{k,k-1}$ 解出，和（18a）一起代入公式（18d）中，***得出 $I_k$*** 。

令 ${\hat{P}}_{k,f}=I_K^{-1}$

在$I_k$ 中 $${\check{P}}_{k,f}^{-1}=A_{k-1}{\hat{P}}_{k-1,f}{A}_{k-1}^T+Q_k\text{\hspace{1em}(19a)}$$

$$I_k={\hat{P}}_{k,f}^{-1}={\check{P}}_{k,f}^{-1}+C^T{R}_k^{-1}{C}_k\text{\hspace{1em}(19)}$$

此时定义卡尔曼增益 $K_k$ : $$K_k={\hat{P}}_{k,f}{C}_k^T{R}_k^{-1}\text{\hspace{1em}(20)}$$

代入公式（19）得到：

$$K_k={\check{P}}_{k,f}{C}^T(C_K{\check{P}}_{k,f}{C}_k^T+R_k{)}^{-1}\text{\hspace{1em}(21)}$$

再将（21）代入（19）中，解出 $${\check{P}}_{k,f}^{-1}={\hat{P}}_{k,f}^{-1}(1-K_k{C}_k)$$ ,整理得到经典的卡尔曼滤波协方差更新步骤： $${\hat{P}}_{k,f}=(1-K_k{C}_k){\check{P}}_{k,f}\text{\hspace{1em}(22)}$$

接着求出 $q_k$ ,令 ${\hat{P}}_{k,f}^{-1}{x}_{k,f}=q_k$ ,同时使用到了公式（19a），化简整理得到：

​ $${\check{x}}_{k,f}=A_{K-1}{\hat{x}}_{k-1,f}+v_k\text{\hspace{1em}(23a)}$$

$${\hat{P}}_{k,f}^{-1}{\hat{x}}_{k,f}={\check{P}}_{k,f}^{-1}{\check{x}}_{k,f}+C_k^T{R}_k^{-1}{y}_k\text{\hspace{1em}(23)}$$

对公式（23）两边同乘 ${\hat{P}}_{k,f}$ 得到，

$${\hat{x}}_{k,f}={\check{x}}_{k,f}+K_k(y_k-C_k{\check{x}}_{k,f})\text{\hspace{1em}(24)}$$

最后进行后向迭代求出 ${\hat{x}}_{k-1}$ ，整理公式得：

$${\hat{x}}_{k-1}={\hat{x}}_{k,f-1}+{\check{P}}_{k-1}{A}_{k-1}^T{\check{P}}_{k,f}^{-1}({\hat{x}}_k-{\check{x}}_{k,f})\text{\hspace{1em}(25)}$$

这是传统后向平滑算法得形式。（这是对均值的修正）

公式（19a），（23a），（23），（21），（22），（24）,(25)：

前向：$k=1.....K$

​ $${\check{P}}_{k,f}^{-1}=A_{k-1}{\hat{P}}_{k-1,f}{A}_{k-1}^T+Q_k\text{\hspace{1em}(26)}$$

​ $${\check{x}}_{k,f}=A_{K-1}{\hat{x}}_{k-1,f}+v_k\text{\hspace{1em}(27)}$$

​ $$K_k={\check{P}}_{k,f}{C}^T(C_K{\check{P}}_{k,f}{C}_k^T+R_k{)}^{-1}\text{\hspace{1em}(28)}$$

​ $${\hat{P}}_{k,f}=(1-K_k{C}_k){\check{P}}_{k,f}\text{\hspace{1em}(29)}$$

$${\hat{x}}_{k,f}={\check{x}}_{k,f}+K_k(y_k-C_k{\check{x}}_{k,f})\text{\hspace{1em}(30)}$$

后向：$k=K.....1$

$${\hat{x}}_{k-1}={\hat{x}}_{k,f-1}+{\check{P}}_{k-1}{A}_{k-1}^T{\check{P}}_{k,f}^{-1}({\hat{x}}_k-{\check{x}}_{k,f})\text{\hspace{1em}(31)}$$

前五个前向迭代过程被称为卡尔曼滤波器。这六个公式表达的RTS平滑算法。

RTS用到了所有的数据，用未来的数据来修饰当前的变量，即它是非因果的。

2. 非线性非高斯系统的状态估计

离散时间的递归估计问题

贝叶斯滤波

贝叶斯滤波仅使用过去以及当前的测量，来构造一个完整的PDF来刻画当前的状态。

$$p(x_k|x_0,v_{1:k},y_{0:k})\text{\hspace{1em}(32)}$$

贝叶斯滤波是LG系统下分解的前向过程。

贝叶斯滤波器：

$$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})=\eta p(y_k|x_k)p(x_k|{\check{x}}_0,v_{0:k},y_{0:k-1})\text{\hspace{1em}(33)}$$

其中，$$p(x_k|{\check{x}}_0,v_{0:k},y_{0:k-1})=\int p(x_k|x_{k-1},v_k)p(x_{k-1}|v_{1:k},y_{0:k-1})d{x}_{k-1}\text{\hspace{1em}(34)}$$

则 $$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})=\eta p(y_k|x_k)\int p(x_k|x_{k-1},v_k)p(x_{k-1}|v_{1:k},y_{0:k-1})d{x}_{k-1}\text{\hspace{1em}(35)}$$

公式中，首项是观测过程，第二项是运动方程进行预测，第三项是已经知道的先验置信度。

扩展卡尔曼滤波

我们假设$x_k$ 的置信度函数限制为高斯分布： $$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})\sim N(\hat{x},{\hat{P}}_k)\text{\hspace{1em}(36)}$$

进行线性化：

$$x_k={\check{x}}_k+F_{k-1}(x_{k-1}-{\hat{x}}_{k-1})+w^{\prime }\text{\hspace{1em}(37)}$$

$$y_k\approx \check{y}+G_{k-1}(x_k-\check{x})+n_k^{\prime }\text{\hspace{1em}(38)}$$

对$x_k$ 求均值和方差，得到

​ $$p(x_k|x_{k-1},v_k)\approx N({\check{x}}_k+F_{k-1}(x_{k-1}-{\hat{x}}_{k-1}),Q_k^{\prime })\text{\hspace{1em}(39)}$$

$$p(y_k|x_k)\approx N({\check{y}}_k+G_k(x_k-{\hat{x}}_{k-1}{)}^{\prime },R_k^{\prime })\text{\hspace{1em}(40)}$$

贝叶斯滤波器 $$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})=\eta p(y_k|x_k)\int p(x_k|x_{k-1},v_k)p(x_{k-1}|v_{1:k},y_{0:k-1})d{x}_{k-1}\sim N({\check{x}}_k+K_k(y_k-{\check{y}}_k),(1-K_k{G}_k)(F_{k-1}{\hat{P}}_{k-1}{F}_{k-1}^T+Q_k^{\prime }))$$

此时EKF的经典递归更新方程如下：

预测：$$\check{P}=F_{k-1}{\hat{P}}_{k-1}{F}_{k-1}^T+Q_k^{\prime }\text{\hspace{1em}(41)}$$

​ $$\check{x}=f({\hat{x}}_{k-1},v_k,0)\text{\hspace{1em}(42)}$$

卡尔曼增益：

$$K_k={\check{P}}_k{G}_k^T(G_k{\check{P}}_k{G}_k^T+R_k^{\prime }{)}^{-1}\text{\hspace{1em}(43)}$$

更新： $${\hat{P}}_k=(1-K_k{G}_k){\check{P}}_k\text{\hspace{1em}(44)}$$

$${\hat{x}}_k={\check{x}}_k+K_k(y_k-g({\check{x}}_k,0))\text{\hspace{1em}(45)}$$

$y_k-g({\check{x}}_k,0)$ 为更新量 。估计的均值 。猜测是为了缩小误差

广义高斯滤波

使用了2.2.3节的联合斯概率密度函数 。写出联合分布，然后使用高斯推断。

$p(x_k,y_k|{\check{x}}_0,v_{1:k},y_{0:k-1})$ 的联合分布。

$p(x_k,y_k|{\check{x}}_0,v_{1:k},y_{0:k-1})=p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})p(y_k|{\check{x}}_0,v_{1:k},y_{0:k-1})$

可以写出$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})$ 的均值和方差。即为广义高斯滤波。之后对应找到：${\hat{x}}_k、{\hat{P}}_k、K_k$ 。但是如果我们不进行线性化，方差和均值不能求出来，所以需要对其进行近似。

迭代扩展卡尔曼滤波

相比较广义高斯滤波，对模型进行如线性化即扩展卡尔曼滤波，之后进行了广义高斯滤波，进行代入联合分布的均值、方差中的各项值。

不同点：公式（37）、（38）中的 ${\check{x}}_k、{\check{y}}_k$ 换为了我们所选区的点 $x_{op}$ 。

所以我们需要进行迭代计算，$x_{op}\leftarrow {\hat{x}}_k$ 每一次迭代将工作点设置为上一次迭代的后验均值，即 ${\hat{x}}_k$ 。

EKF只进行了一次线性化。

1. IEKF的均值是一个MAP解（找最优），
2. 全贝叶斯估计p(x|y)、MAP解与真实x之间是有偏的。
3. IEKF和MAP的解优于EKF

蒙特卡洛方法

对数据分布进行大量的采样，计算非线性变换的值，再用变换后的值构建输出分布。

1. 高斯分布的非线性变换本身不再是高斯分布，只保留了那个分布的一、二阶矩。
2. 非线性变换本身存在误差
3. 线性化点理论上是x均值，但实际是x均值的估计值，也可能是个错误的值
4. y均值= g(x均值)是成问题。

Sigma Point变换

SP变换，或无迹变换是线性化方法和蒙特卡洛方法的折中。

核心思想：选定输入分布的几个点（Sigma Point），计算这几个点的非线性变换，用他的结构构建输出分布。

1.根据输入概率，计算出2L+1个sigmapoint：

​ $$L{L}^T=\sum _{xx}\text{\hspace{1em}(46)}$$

​ $$x_0={\mu }_x\text{\hspace{1em}(47)}$$

​ $$x_i={\mu }_x+\sqrt{L+k}co{l}_{i}L\text{\hspace{1em}(48)}$$

$$x_{i+L}={\mu }_x-\sqrt{L+k}co{l}_{i}L\text{\hspace{1em}(49)}$$

2.这样的样本点满足：

​ ${\mu }_x={\sum }_{i=0}^{2L}{\alpha }_i{x}_i$

​ $$\sum _{xx}=\sum _{i=0}^{2L}{\alpha }_i(x_i-{\mu }_i)(x_i-{\mu }_i)$$

其中： $${\alpha }_i=\{\begin{array}{l}\frac{k}{L+k}i=0;\\ \frac{k}{2(L+k)}其他\end{array}$$

3.对Sigma Point进行非线性变换，得到： $y_i=g(x_i),i=0,...,2L$

4. 用y的结果构建输出高斯分布：

均值： ${\mu }_y={\sum }_{i=0}^{2L}{\alpha }_i{y}_i$

协方差： ${\sum }_{yy}={\sum }_{i=0}^{2L}{\alpha }_i(y_i-{\mu }_y)(y_i-{\mu }_y{)}^T$

Sigma Point的好处：

• 不用计算线性化雅可比矩阵；
• 仅使用了表针矩阵加法乘法和Cholesky分解；
• 对非线性函数的要求很少（不要求光滑和可微）

线性化的方法对比蒙特卡洛法，均值有偏，而且方差过小了。

粒子滤波

粒子滤波是唯一一种可以处理NLNG系统的实用技术。

粒子滤波的流程：

1.采样：从先验与运动噪声中采样M个样本：

$$\left[\begin{array}{c}{\hat{x}}_{k-1,m}\\ w_{k,m}\end{array}\right]\leftarrow p(x_{k-1}|{\check{x}}_0,v_{1:k-1},y_{1:k-1})p(w_k)\text{\hspace{1em}(50)}$$

2.使用运动方程得到预测分布：

$${\check{x}}_{k,m}=f({\hat{x}}_{k-1,m},v_k,w_{k,m)}\text{\hspace{1em}(56)}$$

3.使用观测方程进行比较：

• 为每个粒子计算权重：

  $$w_{k,m}=\frac{p({\check{x}}_{k,m}|{\check{x}}_0,v_{1:k},y_{1:k})}{p({\check{x}}_{k,m}|{\check{x}}_0,v_{1:k},y_{1:k-1})}=\eta p(y_k|{\check{x}}_{k,m})\text{\hspace{1em}(57)}$$

  在实践中，通常使用非线性观测模型来模拟期望的传感器读数 ${\check{y}}_{k,m}$ :

  ​ $${\check{y}}_{k,m}=g({\check{x}}_{k,m},0)$$

  假设$$p(y_k|{\check{x}}_{k,m})=p(y_k|{\check{y}}_{k,m})\text{\hspace{1em}(58)}$$

• 对粒子进行重要性采样（Sample importance resampling)

  ​ x ^ k , m ← 重 要 性 采 样 { x ˇ k , m , w k , m } \hat{x}_{k,m}\leftarrow _{}^{重要性采样} \{\check{x}_{k,m},w_{k,m}\} x^k,m​←重要性采样​{xˇk,m​,wk,m​}

重采样(轮盘赌)：$${\beta }_m=\frac{{\sum }_{n=1}^m{w}_n}{{\sum }_{l=1}^M{w}_l}$$

• 三自由度定位使用几百个粒子即可；
• 粒子数量也可动态设置；
• 重采样可以每隔一段时间做一次

Sigma Point 卡尔曼滤波

SPKF,无迹卡尔曼滤波。

整个过程分为预测步骤和校正步骤：

预测步骤：

• 将先验置信度和运动噪声堆叠在一起，进行SP。
• 对SP后的结果展开为状态和噪声的形式，代入非线性运动模型进行求解；
• 构建高斯分布。带公式求均值，方差

校正步骤：

• 使用了广义高斯滤波，联合概率高斯分布。

  $p(x_k,y_k|{\check{x}}_0,v_{1:k},y_{0:k-1})=p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})p(y_k|{\check{x}}_0,v_{1:k},y_{0:k-1})$ 可以写出$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})$ 的均值和方差。

• 将预测置信度和观测噪声堆叠在一起，进行SP。

• 对SP后的结果展开为状态和噪声的形式，代入非线性观测模型进行求解；

• 构建高斯分布。带公式求均值，方差

• 最后代入广义高斯滤波的公式中。

完全不需要求导；

甚至不需要运动和观测方程得解析形式，视为黑盒模型；

迭代Sigma Point 卡尔曼滤波

迭代扩展卡尔曼滤波，进行多次线性化。

迭代Sigma Point 卡尔曼滤波，进行多次广义高斯滤波。

EKF和SPKF 都不太理想，迭代起来就比较与真值接近。

ISPKF收敛于均值，而MAP收敛与模。

离散时间的批量估计

最大后验估计（MAP）

从上一讲内容中我们已经知道批量解可以等价于最小二乘问题

流程：定义优化变量、优化目标函数。

• 优化变量： $x_0,x_1,....x_K$
• **优化目标函数：**运动与观测的误差，定义目标函数（误差的马氏范数）、整体优化目标。

解最优值得两种方法：

• 牛顿法：

• 对最终的优化函数进行线性化： $j(x)对x求导$

  线性化以后的函数再对$\delta x进行求导$，令导数为零，求得最优$\delta x$ 。

  不断迭代：$x_{op}=x_{op}+\delta x$

• 高斯牛顿法：

• Levernberg-Marquardt方法：

最终的结果：$(H^T{W}^{-1}H)\delta x=H^T{W}^{-1}e(x_{o}p)$

贝叶斯推断

流程：

线性化方程，提升形式，计算先验均值方差，写出联合分布概率密度，用高斯推断计算出后验均值和方差，代入公式推出来结果。

贝叶斯推断和MAP基本上是一样的，不过显式给出来了协方差。

注：

• 相比于MAP，EKF得问题主要有两个过程，没有迭代过程，依赖马尔可夫假设；
• EKF基本上和单词的MAP迭代一样（实际上稍微由于单词迭代）
• 相比于各种针对EKF得改进，实际当中马尔科夫性是一个更本质得约束
• 也有Sliding Window Filter这种介于批量和递归之间的滤波方法。