【状态估计】非线性非高斯系统的状态估计——离散时间的批量估计

上一篇文章介绍了离散时间的递归估计，本文着重介绍离散时间的批量估计。

上一篇位置：【状态估计】非线性非高斯系统的状态估计——离散时间的递归估计。

离散时间的批量估计问题

最大后验估计

目标函数

利用高斯-牛顿法来解决估计问题的非线性版本，这种优化方法也可以认为是MAP方法。

首先建立最小化的目标函数，然后考虑如何解决它。

构建目标函数，优化变量为：

$$x=\left[\begin{array}{c}{x}_0\\ x_1\\ ⋮\\ x_K\end{array}\right]$$

即需要估计整条轨迹。

对于非线性情况，定于相对于先验和测量的误差为：

$$\begin{array}{rl}{e}_{v,0}(x)& ={\check{x}}_0-x_0\\ e_{v,k}(x)& =f(x_{k-1},v_k,0)-x_k\end{array}$$

其中，$k=1,2,...,K$。

$$e_{y,k}(x)=y_k-g(x_k,0)$$

其中，$k=0,1,...,K$。

它们对目标函数的贡献为：

$$\begin{array}{rl}{J}_{v,k}(x)& =\frac{1}{2}{e}_{v,k}(x{)}^T{W}_{v,k}^{-1}{e}_{v,k}(x)\\ J_{y,k}(x)& =\frac{1}{2}{e}_{y,k}(x{)}^T{W}_{y,k}^{-1}{e}_{y,k}(x)\end{array}$$

那么完整的代价函数为：

$$J(x)=\sum _{k=0}^K(J_{v,k}(x)+J_{y,k}(x))$$

通常，可以把$W_{v,k}$和$W_{y,k}$简单的认为是对称正定的权重矩阵。通过设置权重矩阵和测量噪声的协方差相关联，则最小化目标函数等同于最大化状态的联合似然函数。

同时，定义：

$$\begin{array}{rl}e(x)& =\left[\begin{array}{c}{e}_v(x)\\ e_y(x)\end{array}\right]\\ e_v(x)& =\left[\begin{array}{c}{e}_{v,0}(x)\\ e_{v,1}(x)\\ ⋮\\ e_{v,K}(x)\end{array}\right]\\ e_y(x)& =\left[\begin{array}{c}{e}_{y,0}(x)\\ e_{y,1}(x)\\ ⋮\\ e_{y,K}(x)\end{array}\right]\end{array}$$

$$\begin{array}{rl}W& =diag(W_v,W_y)\\ W_v& =diag(W_{v,0},W_{v,1},...,W_{v,K})\\ W_y& =diag(W_{y,0},W_{y,1},...,W_{y,K})\end{array}$$

因此，目标函数可以写成：

$$J(x)=\frac{1}{2}e(x{)}^T{W}^{-1}e(x)$$

定义一个修改版本的误差项：

$$u(x)=Le(x)$$

其中，$L^{T}L=W^{-1}$。使用这些定义，可以得到更简单的目标函数：

$$J(x)=\frac{1}{2}u(x{)}^{T}u(x)$$

这正是二次型的形式，但不是关于设定变量$x$的二次型。目标是最小化目标函数，得到最优参数$\hat{x}$：

$$\hat{x}=argmin(J(x))$$

可以使用许多非线性优化的方法来求解这个二次型的表达式。最经典的方法是高斯牛顿优化方法，但还有许多其他的选择。

牛顿法

牛顿法是指，以迭代的方式，不断用二次函数来近似目标函数，朝着二次近似极小值移动的方法。假设自变量的初始估计，或者说工作点为$x_{op}$，那么可对原函数$J()$在工作点附近进行二阶泰勒展开：

$$J(x_{op}+\delta x)\approx J(x_{op})+(\frac{\partial J(x)}{\partial x}{|}_{x_{op}})\delta x+\frac{1}{2}\delta x^T(\frac{{\partial }^{2}J(x)}{\partial x\partial x^T}{|}_{x_{op}})\delta x$$

其中，$\delta x$表示相对于初始估计$x_{op}$的微小增量，一阶偏导称为雅可比矩阵，二阶偏导称为海塞矩阵。注意，海塞矩阵必须是正定的，才能判断该二次近似的极小值存在，才能使用牛顿法。

下一步是找到$\delta x$的值，最小化该二次近似。令$\delta x$的导数为0：

$$\frac{\partial J(x_{op}+\delta x)}{\partial \delta x}=(\frac{\partial J(x)}{\partial x}{|}_{x_{op}})+\delta x^T(\frac{{\partial }^{2}J(x)}{\partial x\partial x^T}{|}_{x_{op}})=0$$

化简，求得：

$$(\frac{{\partial }^{2}J(x)}{\partial x\partial x^T}{|}_{x_{op}})\delta x=-(\frac{\partial J(x)}{\partial x}{|}_{x_{op}}{)}^T$$

当海塞矩阵可逆时（必定可逆，因为前面假设为正定的），可以得到方程的解，然后根据下面的公式来更新工作点：

$$x_{op}⟵x_{op}+\delta x$$

不停地迭代上述过程，直到$\delta x$变得足够小为止。对于牛顿法，有几点需要注意：

• 局部收敛，这意味着当初始估计已经足够接近解时，不断地改进可以保证结果收敛到一个解；

• 收敛速度是二次的（比简单梯度下降收敛得快得多）；

• 海塞矩阵的计算可能很复杂，使得牛顿法在现实中应用存在困难。

高斯牛顿法

最优化目标函数，对$u()$进行泰勒展开，而不是对$J()$进行泰勒展开：

$$u(x_{op}+\delta x)\approx u(x_{op})+(\frac{\partial u(x)}{\partial x}{|}_{x_{op}})\delta x$$

将其代入$J()$中，则：

$$J(x_{op}+\delta x)\approx \frac{1}{2}(u(x_{op})+(\frac{\partial u(x)}{\partial x}{|}_{x_{op}})\delta x{)}^T(u(x_{op})+(\frac{\partial u(x)}{\partial x}{|}_{x_{op}})\delta x)$$

针对$\delta x$最小化：

$$\frac{\partial J(x_{op}+\delta x)}{\partial \delta x}=(u(x_{op})+(\frac{\partial u(x)}{\partial x}{|}_{x_{op}})\delta x{)}^T(\frac{\partial u(x)}{\partial x}{|}_{x_{op}})=0$$

化简，求得：

$$(\frac{\partial u(x)}{\partial x}{|}_{x_{op}}{)}^T(\frac{\partial u(x)}{\partial x}{|}_{x_{op}})\delta x=-(\frac{\partial u(x)}{\partial x}{|}_{x_{op}}{)}^{T}u(x_{op})$$

高斯牛顿法的另一种推导方式

从牛顿法到高斯牛顿法，主要就是海塞矩阵的近似。那么我们看：

$$J(x)=\frac{1}{2}u(x{)}^{T}u(x)$$

它的雅可比矩阵为：

$$\frac{\partial J(x)}{\partial x}{|}_{x_{op}}=u(x_{op}{)}^T(\frac{\partial u(x)}{\partial x}{|}_{x_{op}})$$

海塞矩阵为：

$$\frac{{\partial }^{2}J(x)}{\partial x\partial x^T}{|}_{x_{op}}=(\frac{\partial u(x)}{\partial x}|x_{x_{op}}{)}^T(\frac{\partial u(x)}{\partial x}{|}_{x_{op}})+\sum _{i=1}^M{u}_i(x_{op})(\frac{{\partial }^2{u}_i(x)}{\partial x\partial x^T}{|}_{x_{op}})$$

其中：$u(x)=(u_1(x),u_2(x),...,u_M(x)$。

注意到在海塞矩阵的表达式中，我们可以假设在$J$的极小值附近，第二项相对于第一项是很小的。直观上看，在极小值附近$u_i(x)$的值应该是很小的（理想情况下为零）。因此在忽略了包含二阶导的项时，海塞可以近似为：

$$\frac{{\partial }^{2}J(x)}{\partial x\partial x^T}{|}_{x_{op}}\approx (\frac{\partial u(x)}{\partial x}|x_{x_{op}}{)}^T(\frac{\partial u(x)}{\partial x}{|}_{x_{op}})$$

将雅可比矩阵和海塞矩阵的近似，带入到牛顿法的公式，可以得到：

$$(\frac{\partial u(x)}{\partial x}{|}_{x_{op}}{)}^T(\frac{\partial u(x)}{\partial x}{|}_{x_{op}})\delta x=-(\frac{\partial u(x)}{\partial x}{|}_{x_{op}}{)}^{T}u(x_{op})$$

这个上面的推导结果保持一致。

高斯牛顿法的改进

由于高斯牛顿法不能保证收敛（因为对海塞矩阵进行了近似），可以使用两个实际的方法对其进行改进：

1. 一旦计算出最优增量$\delta x$，则实际的更新为：

$$x_{op}⟵x_{op}+\alpha \delta x$$

其中，$\alpha \in [0,1]$为自定义的参数。在实际上，常常通过线搜索的方式求得$\alpha$的最优值。该方法能够有效的原因在于，$\delta x$是下降方向；而只是调整了在该方向上的行进距离，使得收敛性质更加鲁棒（而不是更快）。

2. 可以使用列文伯格-马夸尔特（LM）改进高斯牛顿法：

$$((\frac{\partial u(x)}{\partial x}{|}_{x_{op}}{)}^T(\frac{\partial u(x)}{\partial x}{|}_{x_{op}})+\lambda D)\delta x=-(\frac{\partial u(x)}{\partial x}{|}_{x_{op}}{)}^{T}u(x_{op})$$

其中，$D$为正定对角矩阵。当$D=1$时，随着$\lambda \ge 0$变大，海塞矩阵近似所占的比重相对较小，此时：

$$\delta x=-\frac{1}{\lambda }(\frac{\partial u(x)}{\partial x}{|}_{x_{op}}{)}^{T}u(x_{op})$$

即最速下降（即负梯度）中非常小的一个步长。当$\lambda =0$时，则恢复为通常的高斯牛顿更新。

LM法通过缓慢增加$\lambda$的值，可以在海塞矩阵近似较差或病态的情况下工作。

关于误差项的高斯牛顿法

前面提到：

$$\begin{array}{rl}J(x)& =\frac{1}{2}u(x{)}^{T}u(x)\\ u(x)& =Le(x)\end{array}$$

其中，$L^{T}L=W^{-1}$是一个常量。

将上式代入高斯牛顿的更新方程中，可以得到关于误差项$e(x)$的更新：

$$(LH{)}^{T}LH\delta x=-(LH{)}^{T}Le(x_{op})$$

其中，

$$\begin{array}{rl}H& =-\frac{\partial e(x)}{\partial x}{|}_{x_{op}}=\left[\begin{array}{ccccc}1& & & & \\ -F_0& 1& & & \\ & -F_1& \ddots & & \\ & & \ddots & 1& \\ & & & -F_{K-1}& 1\\ G_0& & & & \\ & G_1& & & \\ & & G_2& & \\ & & & \ddots & \\ & & & & G_K\end{array}\right]\\ F_{k-1}& =\frac{\partial f(x_{k-1},v_k,w_k)}{\partial x_{k-1}}{|}_{x_{op},k-1,v_k,0}\\ G_k& =\frac{\partial g(x_k,n_k)}{\partial x_k}{|}_{x_{op},k,0}\end{array}$$

化简可得：

$$(H^T{W}^{-1}H)\delta x=H^T{W}^{-1}e(x_{op})$$

贝叶斯推断

从贝叶斯推断的角度也可以得到相同的更新方程。

MAP方法是通过定义目标函数，通过高斯-牛顿法得到$(H^T{W}^{-1}H)\delta x=H^T{W}^{-1}e(x_{op})$。当然，也可以使用捷径，对$e(x)$进行线性化，再使用线性高斯系统的方法进行推导，最后会得到相同的结果。

贝叶斯推断，也可以使用线性化的方法进行讨巧，再证明得到。这里就不赘述了。

讨论

如果把EKF看作是全非线性高斯牛顿方法的近似，那么它的表现是不尽如人意的。主要原因是，EKF没有迭代至收敛的过程，其雅可比矩阵也只计算一次（可能远离最优估计）。从本质上看，EKF可以做得比单次高斯牛顿迭代更好，因为它没有一次性计算所有的雅可比矩阵。即EKF中，一部分雅可比计算是在运动先验中，另一部分在观测中，但是观测部分的雅可比是在推导运动先验之后再计算的。而单次高斯牛顿迭代中，两部分雅可比是一起计算的。

EKF的主要缺陷在于缺少迭代的过程。目前，在提升EKF的表现方面已经有不少的工作，包括IEKF。IEKF的问题在于，它仍然依赖于马尔可夫假设。它仅在一个时刻上进行了迭代，而非在整个轨迹上。

不过高斯牛顿的批量式的估计也存在一些问题。它必须离线运行，且不是一个恒定时间的算法。而EKF既可以是在线方法，也可以是恒定时间方法。所谓的滑动窗口滤波器（SWF），则是在由多个时间步长组成的窗口内进行选代，并且将这个窗口进行滑动，从而达到在线和恒定时间的实现。

原文地址：https://blog.csdn.net/qq_38410730/article/details/140180040

免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！