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

本章我们将研究如何处理现实世界中的系统——这些系统往往不是线性高斯的。可以说非线性非高斯（nonlinear non-Gaussian, NLNG）系统的状态估计仍然是一个非常热门的研究课题。限于篇幅，本章仅对一些常见的处理非线性和或非高高斯系统的方法进行讲解。

首先，针对递归滤波问题，我们将介绍一种称为贝叶斯滤波的通用理论框架。我们熟知的扩展卡尔曼滤波、sigmapoint卡尔曼滤波和粒子滤波都可以看作是贝叶斯滤波的的近似。然后，我们再探讨非线性非高斯系统的批量估计问题。

当然，全部的这部分内容比较多，这里将会分为两篇文章进行论述。本文主要讨论的是：离散时间的递归估计。

离散时间的递归估计问题

问题定义

和线性高斯系统的状态估计一样，需要为估计器定义一系列的运动和观测模型。假设讨论的是离散时间情况下的时不变系统，但该系统中包含了非线性方程。定义以下运动和观测模型：

运动方程：$k=1,2,...,K$

$$x_k=f(x_{k-1},v_k,w_k)$$

观测方程：$k=0,1,2,...,K$

$$y_k=g(x_k,n_k)$$

其中，$k$为时间下标。函数$f()$为非线性的运动模型，函数$g()$为非线性的观测模型。其余变量的含义和线性高斯系统的约定一致，不同的是，并没有假设任何随机变量是高斯的。

下图描述的是系统随时间演变的图模型。从这张图上，可以观察到该系统一个非常重要的性质——马尔可夫性。

当一个随机过程在给定现在状态及所有过去状态的情况下，其未来状态的条件概率分布仅依赖于当前状态；换句话说，在给定现在状态时，未来状态与过去状态是条件独立的，那么此随机过程称为马尔可夫过程。

这里的系统就是马尔可夫过程。一旦知道$x_{k-1}$，在不需要知道任何其他过去状态的情况下，就可以向前地推地计算$x_k$。

贝叶斯滤波

线性高斯系统中，从批量式估计开始，接着是递归卡尔曼滤波器。本文中，则从递归滤波器，贝叶斯滤波开始，最后再回归到批量式方法。

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

$$p(x_k|x_0,v_{1:k},y_{0:k})$$

回忆批量式LG系统中，可以将其分解成前向递归和后向递归：

$$p(x_k|x_0,v_{1:k},y_{0:k})=\eta p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})p(x_k|v_{k+1:K},y_{k+1,K})$$

因此，关注于将前向部分转换为递归滤波器。

由于所有的观测是独立的，可以将最新的观测分解出来：

$$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_{1:k},y_{0:k-1})$$

这里用了贝叶斯公式调整了依赖关系。现在将注意力转向第二个因子，引入隐藏状态$x_{k-1}$，并对其进行积分：

$$\begin{array}{rl}p(x_k|{\check{x}}_0,v_{1:k},y_{0:k-1})& =\int p(x_k,x_{k-1}|{\check{x}}_0,v_{1:k},y_{0:k-1})d{x}_{k-1}\\ & =\int p(x_k|x_{k-1},{\check{x}}_0,v_{1:k},y_{0:k-1})p(x_{k-1}|{\check{x}}_0,v_{1:k},y_{0:k-1})d{x}_{k-1}\end{array}$$

隐藏状态的引入可以看成是边缘化的相反操作。

到这里，还没有引入任何的近似，下一步的操作非常微妙，它是递归式估计中存在许多局限性的原因。由于马尔可夫性，因此：

$$\begin{array}{rl}p(x_k|x_{k-1},{\check{x}}_0,v_{1:k},y_{0:k-1})& =p(x_k|x_{k-1},v_k)\\ p(x_{k-1}|{\check{x}}_0,v_{1:k},y_{0:k-1})& =p(x_{k-1}|{\check{x}}_0,v_{1:k-1},y_{0:k-1})\end{array}$$

因此：

$$\begin{array}{rl}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_{1:k},y_{0:k-1})\\ & =\eta p(y_k|x_k)\int p(x_k|x_{k-1},v_k)p(x_{k-1}|{\check{x}}_0,v_{1:k-1},y_{0:k-1})d{x}_{k-1}\end{array}$$

可以看出：$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})$是后验置信度，$p(y_k|x_k)$利用$g()$进行更新，$p(x_k|x_{k-1},v_k)$利用$f()$进行预测，$p(x_{k-1}|{\check{x}}_0,v_{1:k-1},y_{0:k-1})$是先验置信度。此式具有预测——校正的形式。

在预测阶段，先验置信度通过输入$v_k$和运动模型$f()$在时间上进行前向传播；在校正阶段，则通过观测$y_k$和观测模型$g()$来更新预测估计状态，并得到后验置信度。

贝叶斯滤波虽然精确，但也仅仅是一个精美的数学产物：除了线性高斯的情况外，在实际中它基本上不可能实现。主要原因有两个：

1. 概率密度函数存在于无限维的空间中，因此需要无限的存储空间/参数来完全表示置信度。为了克服这种问题，需要将这个置信度大致地表示出来：一种是将该函数近似为高斯（即只关心一阶矩和二阶矩），另一种是使用有限数量的随机样本来近似；

2. 贝叶斯滤波器的积分在计算上十分耗时，因此需要无限的计算资源来计算它。为了克服这种问题，必须对积分进行近似：一种是对运动和观测模型进行线性化，另一种是使用蒙特卡罗积分。

扩展卡尔曼滤波

如果将置信度和噪声限制为高斯分布，并且对运动模型和观测模型进行线性化，计算贝叶斯滤波中的积分（以及归一化积），将得到著名的扩展卡尔曼滤波（EKF）。

为了推导EKF，首先将$x_k$的置信度函数限制为高斯分布：

$$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})=N({\hat{x}}_k,{\hat{P}}_k)$$

其中，${\hat{x}}_k$为均值，${\hat{P}}_k$为协方差。接下来，假设噪声变量$w_k$和$n_k$也是高斯的：

$$\begin{array}{rl}{w}_k& \sim N(0,Q_k)\\ n_k& \sim N(0,R_k)\end{array}$$

请注意，高斯PDF经过非线性函数变换后，可能变成非高斯的。对于噪声变量而言，这种情况也是存在的。换句话说，非线性运动和观测模型可能会对$w_k$和$n_k$造成影响。它们不一定是在非线性函数后以加法的形式存在，即：

$$\begin{array}{rl}{x}_k& =f(x_{k-1},v_k)+w_k\\ y_k& =g(x_k)+n_k\end{array}$$

而是包含在非线性函数之内，即：

$$\begin{array}{rl}{x}_k& =f(x_{k-1},v_k,w_k)\\ y_k& =g(x_k,n_k)\end{array}$$

然而，可以通过线性化将其恢复为加性噪声的形式。

由于$g()$和$f()$的非线性特征，无法计算得到贝叶斯滤波中积分的闭式解，转而使用线性化的方法，在当前状态估计的均值处展开，对运动和观测模型进行线性化。

$$\begin{array}{rl}f(x_{k-1},v_k,w_k)& \approx {\check{x}}_k+F_{k-1}(x_{k-1}-{\hat{x}}_{k-1})+w_k^{{}^{\prime }}\\ g(x_k,n_k)& \approx {\check{y}}_k+G_k(x_k-{\check{x}}_k)+n_k^{{}^{\prime }}\end{array}$$

其中，

$$\begin{array}{rl}{\check{x}}_k& =f({\hat{x}}_{k-1},v_k,0)\\ F_{k-1}& =\frac{\partial f(x_{k-1},v_k,w_k)}{\partial x_{k-1}}{|}_{{\hat{x}}_{k-1},v_k,0}\\ w_k^{{}^{\prime }}& =w_k\frac{\partial f(x_{k-1},v_k,w_k)}{\partial w_k}{|}_{{\hat{x}}_{k-1},v_k,0}\end{array}$$

$$\begin{array}{rl}{\check{y}}_k& =g({\check{x}}_k,0)\\ G_k& =\frac{\partial g(x_k,n_k)}{\partial x_k}{|}_{{\check{x}}_k,0}\\ n_k^{{}^{\prime }}& =n_k\frac{\partial g(x_k,n_k)}{\partial n_k}{|}_{{\check{x}}_k,0}\end{array}$$

给定过去的状态和最新输入，则当前状态$x_k$的统计学特性为：

$$\begin{array}{rl}{x}_k& \approx {\check{x}}_k+F_{k-1}(x_{k-1}-{\hat{x}}_{k-1})+w_k^{{}^{\prime }}\\ E[x_k]& \approx {\check{x}}_k+F_{k-1}(x_{k-1}-{\hat{x}}_{k-1})+E[w_k^{{}^{\prime }}]\\ E[(x_k-E[x_k])(x_k-E[x_k]{)}^T]& \approx E[w_k^{{}^{\prime }}(w_k^{{}^{\prime }}{)}^T]\\ 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 }})\end{array}$$

给定当前状态，则当前观测$y_k$的统计学特性为：

$$\begin{array}{rl}{y}_k& \approx {\check{y}}_k+G_k(x_k-{\check{x}}_k)+n_k^{{}^{\prime }}\\ E[y_k]& \approx {\check{y}}_k+G_k(x_k-{\check{x}}_k)+E[n_k^{{}^{\prime }}\\ E[(y_k-E[y_k])(y_k-E[y_k]{)}^T]& \approx E[n_k^{{}^{\prime }}(n_k^{{}^{\prime }}{)}^T]\\ p(y_k|x_k)& \approx N({\check{y}}_k+G_k(x_k-{\check{x}}_k),R_k^{{}^{\prime }})\end{array}$$

将上面的等式代入被贝叶斯滤波中，则可以得到：

$$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}|{\check{x}}_0,v_{1:k-1},y_{0:k-1})d{x}_{k-1}$$

则：

$$N({\hat{x}}_k,{\hat{P}}_k)=\eta N({\check{y}}_k+G_k(x_k-{\check{x}}_k),R_k^{{}^{\prime }})\times \int N({\check{x}}_k+F_{k-1}(x_{k-1}-{\hat{x}}_{k-1}),Q_k^{{}^{\prime }})N({\hat{x}}_{k-1},{\hat{P}}_{k-1})d{x}_{k-1}$$

利用将服从高斯分布的变量传入非线性函数的线性化之后，其积分仍然是高斯的：

$$N({\hat{x}}_k,{\hat{P}}_k)=\eta N({\check{y}}_k+G_k(x_k-{\check{x}}_k),R_k^{{}^{\prime }})\times N({\check{x}}_k,F_{k-1}{\hat{P}}_{k-1}{F}_{k-1}^T+Q_k^{{}^{\prime }})$$

现在只剩下两个高斯PDF的归一化积，经过一系列的代数化简，可以得到：

$$N({\hat{x}}_k,{\hat{P}}_k)=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 }}))$$

对比左右两侧：

预测：

$$\begin{array}{rl}{\check{x}}_k& =f({\hat{x}}_{k-1},v_k,0)\\ {\check{P}}_k& =F_{k-1}{\hat{P}}_{k-1}{F}_{k-1}^T+Q_k^{{}^{\prime }}\end{array}$$

卡尔曼增益：

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

更新：

$$\begin{array}{rl}{\hat{x}}_k& ={\check{x}}_k+K_k(y_k-g({\check{x}}_k,0))\\ {\hat{P}}_k& =(1-K_k{G}_k){\check{P}}_k\end{array}$$

这就是EKF的经典递归方程，可以从 { x ^ k − 1 , P ^ k − 1 } \{\hat x_{k-1},\hat P_{k-1}\} {x^k−1​,P^k−1​}计算出 { x ^ k , P ^ k } \{\hat x_k,\hat P_k\} {x^k​,P^k​}。这与线性高斯的主要区别：

• 通过非线性的运动和观测模型来传递估计的均值；

• 噪声协方差$Q_k^{{}^{\prime }}$和$R_k^{{}^{\prime }}$中包含了雅可比矩阵，这是因为允许噪声应用于非线性模型中。

需要注意的是，EKF并不能保证在所有的非线性系统中能够充分发挥作用。EKF的主要问题在于，其线性化的工作点是估计状态的均值，而不是真实状态。这一点微小的差异可能导致EKF在某些情况下快速地发散。有时EKF的估计虽然没有什么明显异常，但常常是有偏或不一致的，更经常是两者都有。

广义高斯滤波

贝叶斯滤波的迷人之处在于它具有精确的表达，可以采用不同的近似形式和处理，推导出一些可实现的滤波器。不过假设估计的状态是高斯的，就存在更清晰的推导方式。

一般来说，先从$k-1$时刻的高斯先验开始：

$$p(x_{k-1}|{\check{x}}_0,v_{1:k-1},y_{0:k-1})=N({\hat{x}}_{k-1},{\hat{P}}_{k-1})$$

通过非线性运动模型$f()$在时间上向前传递，得到$k$时刻的高斯先验：

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

这是预测步骤，结合了最新的输入$v_k$。

对于校正步骤，写出在时刻$k$的状态和最新测量的联合高斯分布：

$$p(x_k,y_k|{\check{x}}_0,v_{1:k},y_{0:k-1})=N(\left[\begin{array}{c}{\mu }_{x,k}\\ {\mu }_{y,k}\end{array}\right],\left[\begin{array}{cc}{\Sigma }_{xx,k}& {\Sigma }_{xy,k}\\ {\Sigma }_{yx,k}& {\Sigma }_{yy,k}\end{array}\right])$$

然后得到$x_k$的条件高斯密度函数（即后验概率）：

$$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})=N({\mu }_{x,k}+{\Sigma }_{xy,k}{\Sigma }_{yy,k}^{-1}(y_k-{\mu }_{y,k}),{\Sigma }_{xx,k}-{\Sigma }_{xy,k}{\Sigma }_{yy,k}^{-1}{\Sigma }_{yx,k})$$

其中，${\mu }_{y,k}$通过非线性观测模型$g()$来计算。这里，可以写出广义高斯滤波中校正步骤的方程：

$$\begin{array}{rl}{K}_k& ={\Sigma }_{xy,k}{\Sigma }_{yy,k}^{-1}\\ {\hat{x}}_k& ={\check{x}}_k+K_k(y_k-{\mu }_{y,k})\\ {\hat{P}}_k& ={\check{P}}_k-K_k{\Sigma }_{xy,k}^T\end{array}$$

其中，令${\mu }_{x,k}={\check{x}}_k$，${\Sigma }_{xx,k}={\check{P}}_k$，$K_k$为卡尔曼增益。然而，除非运动和观测模型是线性的，否则无法计算所需的剩余的变量：${\mu }_{y,k}$，${\Sigma }_{yy,k}$和${\Sigma }_{xy,k}$。这是因为将高斯PDF代入非线性函数中通常会成为非高斯的。因此，在这个截断需要考虑对齐进行近似。

迭代扩展卡尔曼滤波

基于EKF的内容，接下来完成**迭代扩展卡尔曼滤波（IEKF）**的推导。其中预测步骤相当直接，与EKF基本相同，因此不再赘述。但需要注意的是，在$k$时刻的先验为：

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

包含了$v_k$。

校正步骤则会更有意思一些。非线性观测模型为：

$$y_k=g(x_k,n_k)$$

对其中任意一个点$x_{op,k}$进行线性化，可得：

$$g(x_k,n_k)\approx y_{op,k}+G_k(x_k-x_{op,k})+n_k^{{}^{\prime }}$$

其中：

$$\begin{array}{rl}{y}_{op,k}& =g(x_{op,k},0)\\ G_k& =\frac{\partial g(x_k,n_k)}{\partial x_k}{|}_{x_{op,k},0}\\ n_k^{{}^{\prime }}& =n_k\frac{\partial g(x_k,n_k)}{\partial n_k}{|}_{x_{op,k},0}\end{array}$$

注意，观测模型和雅可比矩阵均在$x_{op,k}$处计算。

使用上面这种线性化的模型，可以将时刻$k$处的状态和测量的联合概率近似为高斯分布：

$$\begin{array}{rl}p(x_k,y_k|{\check{x}}_0,v_{1:k},y_{0:k})& \approx N(\left[\begin{array}{c}{\mu }_{x,k}\\ {\mu }_{y,k}\end{array}\right],\left[\begin{array}{cc}{\Sigma }_{xx,k}& {\Sigma }_{xy,k}\\ {\Sigma }_{yx,k}& {\Sigma }_{yy,k}\end{array}\right])\\ & =N(\left[\begin{array}{c}{\check{x}}_k\\ y_{op,k}+G_k({\check{x}}_k-x_{op,k})\end{array}\right],\left[\begin{array}{cc}{\check{P}}_k& {\check{P}}_k{G}_k^T\\ G_k{\check{P}}_k& G_k{\check{P}}_k{G}_k^T+R_k^{{}^{\prime }}\end{array}\right])\end{array}$$

如果测量值$y_k$一致，可以得到$x_k$的条件高斯密度函数（即后验概率）：

$$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})=N({\mu }_{x,k}+{\Sigma }_{xy,k}{\Sigma }_{yy,k}^{-1}(y_k-{\mu }_{y,k}),{\Sigma }_{xx,k}-{\Sigma }_{xy,k}{\Sigma }_{yy,k}^{-1}{\Sigma }_{yx,k})$$

那么，校正步骤的方程：

$$\begin{array}{rl}{K}_k& ={\Sigma }_{xy,k}{\Sigma }_{yy,k}^{-1}\\ {\hat{x}}_k& ={\check{x}}_k+K_k(y_k-{\mu }_{y,k})\\ {\hat{P}}_k& ={\check{P}}_k-K_k{\Sigma }_{xy,k}^T\end{array}$$

将矩${\mu }_{y,k}$、${\Sigma }_{yy,k}$和${\Sigma }_{xy,k}$代入，可以得到：

$$\begin{array}{rl}{K}_k& ={\check{P}}_k{G}_k^T(G_k{\check{P}}_k{G}_k^T+R_k^{{}^{\prime }}{)}^{-1}\\ {\hat{x}}_k& ={\check{x}}_k+K_k(y_k-y_{op,k}-G_k({\check{x}}_k-x_{op,k}))\\ {\hat{P}}_k& =(1-K_k{G}_k){\check{P}}_k\end{array}$$

这些方程中的卡尔曼增益和校正方程和EKF的非常相似：唯一的区别在于线性化的工作点。如果将线性化的工作点设置为预测先验的均值，即$x_{op,k}={\check{x}}_k$，那么两者是完全相同的。

然而，当迭代地进行校正步骤的计算，并且在每一次迭代中将工作点设置为上一次迭代的后验均值，将会得到更好的结果：

$$x_{op,k}⟵{\hat{x}}_k$$

在第一次迭代中，令$x_{op,k}={\check{x}}_k$。这使得能够对更好的估计进行线性化，从而改进每次迭代的近似程度。在迭代的过程中，若$x_{op,k}$的改变足够小的时候就终止迭代。

从MAP角度看IEKF

一个重要的问题：EKF、IEKF和全贝叶斯后验之间的关系是什么？

可以发现，IEKF对应于全后验概率的（局部）极大值；换句话说，它是一个MAP估计。另一方面，由于EKF的校正部分没有迭代，它可能远离局部最大值；实际上很难说清楚它与全后验概率的关系。

其他将PDF传入非线性函数的方法

在推导EKF和IEKF时，通过在非线性模型的工作点处进行线性化的方式将PDF传递进非线性函数中。这当然是一种可行的方法，但还存在其他的方法。通常的方法有：蒙特卡罗方法（暴力的）、线性化方法（EKF，IEKF采用的）、sigma point无迹变换。

蒙特卡罗方法

蒙特卡罗方法本质上是一种暴力的方法。根据输入的概率密度采集大量样本，接着通过非线性函数将每一个样本精确地进行转换，最后从转换的样本中构建输出的概率密度。

笼统地说，大数定律确保了当样本数量接近无穷大时，这种做法将会使结果收敛到正确的值。

这种方法明显存在的问题就是，它可能非常低效，特别是在高维问题上。除了这个明显的缺点，这个方法也存在着一些优点：

• 适用于任何PDF，而不仅仅是高斯分布；

• 可以处理任何类型的非线性函数，不要求可微，连续，甚至不需要知道其数学形式；

• 这是一个任意时间的算法，即计算时间可以随要求进行调整，只需要调整采样点数量。

另一个值得一提的地方是，输出概率密度的均值，和输入概率密度的均值通过非线性变换后的值，是不同的。

线性化方法

通过非线性函数传递高斯PDF最常见的方法就是线性化，严格地说，均值实际上是通过非线性函数精确地传递的，而协方差则是近似地通过非线性函数的线性化版本。通常，线性化过程的工作点是PDF的均值。这个过程是非常不准确的，原因如下：

• 通过非线性函数的高斯PDF的结果不会是另一个高斯PDF，线性化方法只保留了后验PDF的均值和协方差，丢弃了高阶矩，是一种近似；

• 简单地将先验PDF的均值经过非线性变换来逼近真实输出PDF的均值；

• 通过线性化非线性函数来近似真实输出PDF的协方差；

• 线性化的工作点通常不是先验PDF的真实均值，而是对输入PDF均值的估计。

线性化另一个缺点是，必须解析/数值地计算非线性函数的雅可比矩阵。

尽管有着种种近似和缺点，但是如果函数只是轻微的非线性，并且输入是高斯的，那么线性化方法可以说是一种简单易懂并且易于实现的方法。线性化方法的一个优点是：线性化的操作实际上是可逆的（如果非线性函数是局部可逆的）。也就是说，可以将输出PDF通过非线性函数的逆来精确地恢复输入PDF。但是这对其它通过非线性函数传递PDF的方法来说，并不都成立，因为它们并不像线性化方法那样做出相同的近似。

sigma point变换

从某种意义上说，当输入概率密度大致为高斯时，sigma point或无迹变换是蒙特卡罗方法和线性化方法的折中。它比线性化方法更准确，除了计算开销稍大一些。蒙特卡罗仍然是最准确的方法，但在大多数情况下，其计算开销令人望而生畏。

sigmapoint变换是一系列的变换方法。一般来说，任何一个sigma point变换的版本，都是在输入概率密度均值的基础版本上添加一个附加样本。

对称采样策略的sigma point变换，具体步骤如下：

1. 根据输入概率密度$N({\mu }_x,{\Sigma }_{xx})$计算出$2L+1$个sigma point：

$$\begin{array}{rl}L{L}^T& ={\Sigma }_{xx}\\ x_0& ={\mu }_x\\ x_i& ={\mu }_x+\sqrt{l+\kappa }{L}_{co{l}_i}\\ x_{i+L}& ={\mu }_x-\sqrt{l+\kappa }{L}_{co{l}_i}\end{array}$$

其中，$l=dim({\mu }_x)$，$i=1,2,...,l$。$\kappa$是比例系数，用以调节sigma点的分散情况，它的选取仅影响三阶及以上的高阶矩带来的偏差；对于高斯分布，影响四阶及以上的高阶矩。

$$\begin{array}{rl}{\mu }_x& =\sum _{i=0}^{2L}{w}_i{x}_i\\ {\Sigma }_{xx}& =\sum _{i=0}^{2L}{w}_i(x_i-{\mu }_x)(x_i-{\mu }_x{)}^T\end{array}$$

其中，

$$\begin{array}{rl}{w}_0& =\frac{\kappa }{l+\kappa }\\ w_i& =\frac{1}{2(l+\kappa )}\end{array}$$

其中，$i=1,2,...,2l$。

2. 把每个sigma point单独代入非线性函数$g()$中：

$$y_i=g(x_i)$$

3. 输出概率的均值${\mu }_y$、协方差${\Sigma }_{yy}$通过下面的式子计算：

$$\begin{array}{rl}{\mu }_y& =\sum _{i=0}^{2L}{w}_i{y}_i\\ {\Sigma }_{yy}& =\sum _{i=0}^{2L}{w}_i(y_i-{\mu }_y)(y_i-{\mu }_y{)}^T\end{array}$$

4. 最终得到输出概率密度$N({\mu }_y,{\Sigma }_{yy})$。

一般来说，$l+\kappa =3$。

由于$\kappa$的取值可正可负，当$l>3$时，$\kappa$的取值为负，此时就无法确保协方差矩阵的半正定性。当然，在实际应用中，$\kappa$的选取并不一定要满足$l+\kappa =3$这个条件，只是这样的话，对称采样对后验分布的捕捉精度会略微下降。

但不管如何，对称采样下的sigma点对任意非线性状态后验分布的近似精度可达泰勒2阶，对高斯分布可达3阶。当满足$l+\kappa =3$时，对高斯分布可达4阶。

对称采样过程中，随着$l$的增加，sigma点到中心的距离会越来越远，产生了采样的非局部效应，即sigma采样点所携带的均值特征越来越少，从而导致无法准确计算后验分布。另外，如果$\kappa$为负，则会导致无法确保协方差的半正定性。为了消除采样的非局部效应，理论上$L+\kappa$的取值越小越好。

以二维高斯PDF为例，最终获取到的sigma point大致为：

为了使对$\kappa$的选取不受过多的限制，可以选择比例修正对称采样策略的sigma point变换。与对称采样的差别主要体现在采样上：

$$\begin{array}{rl}L{L}^T& ={\Sigma }_{xx}\\ x_0& ={\mu }_x\\ x_i& ={\mu }_x+\sqrt{l+\lambda }{L}_{co{l}_i}\\ x_{i+L}& ={\mu }_x-\sqrt{l+\lambda }{L}_{co{l}_i}\end{array}$$

其中，$l=dim({\mu }_x)$，$i=1,2,...,l$。

$$\begin{array}{rl}{\mu }_x& =\sum _{i=0}^{2L}{w}_i^m{x}_i\\ {\Sigma }_{xx}& =\sum _{i=0}^{2L}{w}_i^c(x_i-{\mu }_x)(x_i-{\mu }_x{)}^T\end{array}$$

其中，

$$\begin{array}{rl}{w}_0^m& =\frac{\lambda }{l+\lambda }\\ w_i^m& =\frac{1}{2(l+\lambda )}\\ w_0^c& =\frac{\lambda }{l+\lambda }+1+\beta -{\alpha }^2\\ w_i^c& =\frac{1}{2(l+\lambda )}\end{array}$$

其中，$i=1,2,...,2l$，$\lambda ={\alpha }^2(L+\kappa )-l$。$\kappa$仍为比例参数。

一般情况下，$l+\kappa =3$，但需要确保协方差矩阵的半正定性。$\alpha$是正值的比例缩放因子，$1\le \alpha \le 1$，它的取值会影响sigma点的分散程度，从而影响后验分布的捕捉精度。当系统非线性程度严重时，$\alpha$可以取一个非常小的正值，以避免采样点的非局域效应的影响。$\beta$是一个非负的权系数，它的取值会影响协方差的近似精度。对于高斯分布，$\beta$的最佳选择是$\beta =2$。

这样的好处是，在对称采样中，对于$l>3$时，为了保证协方差矩阵的半正定性，$\kappa$不能为负，但是$\kappa >0$时，sigma点就变得分散，影响采样的非局部效应。而比例修正对称采样，${\alpha }^2(l+\kappa )$多乘了一个${\alpha }^2$系数，此时可以使得sigma点变得聚集，提高后验分布的精度。

相比于线性化方法，这种方法具有许多优点：

• 通过对输入密度进行近似，避免了线性化方法中非线性函数的雅可比矩阵（解析或数值）的计算；

• 仅使用标准线性代数运算（Cholesky分解、外积、矩阵求和）；

• 不要求非线性函数光滑和可微，计算代价和线性化方法差不多。

粒子滤波

采集大量的样本就可以近似地描述PDF，再将每个样本代入非线性函数中进行重新组合，可以获得PDF转换后的近似值。

粒子滤波是唯一一种能够处理非高斯噪声、非线性观测模型和运动模型的实用技术。其实用之处在于它很容易实现：甚至不需要知道$f()$和$g()$的解析表达式，也不需要求得它们的偏导。

粒子滤波器有很多版本，我们介绍一个基础版本，然后指出从哪些地方可以推出一些变化。这里采用的方法是重要性采样（sample importance resampling）。主要步骤如下：

1. 从由先验和运动噪声的联合概率密度中抽取$M$个样本：

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

其中，$m$为唯一的粒子序号。实际上，可以根据联合概率密度的每个因子分开来抽样。

2. 使用$v_k$得到后验PDF的预测。这可以将每个先验粒子和噪声样本代入非线性运动模型：

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

这些新的预测粒子共同近似刻画了概率密度$p(x_k|{\check{x}}_0,v_{1:k},y_{1:k-1})$。

3. 结合$y_k$对后验概率进行校正，主要分两步：

• 根据每个粒子的期望后验和预测后验的收敛程度，对每个粒子赋予权重$w_{k,m}$：

$$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})$$

其中，$\eta$为归一化系数。在实际中，通常使用非线性模型来模拟期望的传感器读数${\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})$，其中等式右边的概率密度已知（比如高斯分布）。

• 根据赋予的权重，对每个粒子进行重要性采样：

x ^ k , m ⟵ { x ˇ k , m , w k , m } \hat x_{k,m}\longleftarrow \{\check x_{k,m},w_{k,m}\} x^k,m​⟵{xˇk,m​,wk,m​}

sigma point卡尔曼滤波

UKF就是在非线性的运动和观测模型中，使用sigma point变换来传递PDF。sigma point变换上面提到了两种：对称采样策略、比例修正对称采样策略。这里以对称采样策略为例。

预测步骤是sigma point变换的直接应用，因为这一步只是将先验通过运动模型向前进行传递。即：将先验置信度 { x ^ k − 1 , P ^ k − 1 } \{\hat x_{k-1},\hat P_{k-1}\} {x^k−1​,P^k−1​}转换为 { x ˇ k , P ˇ k } \{\check x_k,\check P_k\} {xˇk​,Pˇk​}：

1. 先验置信度和运动噪声都有不确定性，将它们按以下方式堆叠在一起：

$$\begin{array}{rl}{\mu }_z& =\left[\begin{array}{c}{\hat{x}}_{k-1}\\ 0\end{array}\right]\\ {\Sigma }_{zz}& =\left[\begin{array}{cc}{\hat{P}}_{k-1}& 0\\ 0& Q_k\end{array}\right]\end{array}$$

可以看到 { μ z , Σ z z } \{\mu_z,\Sigma_{zz}\} {μz​,Σzz​}仍然是高斯形式。

2. 将 { μ z , Σ z z } \{\mu_z,\Sigma_{zz}\} {μz​,Σzz​}转化为sigma point表示：

$$\begin{array}{rl}L{L}^T& ={\Sigma }_{zz}\\ z_0& ={\mu }_z\\ z_i& ={\mu }_z+\sqrt{l+\kappa }{L}_{co{l}_i}\\ z_{i+L}& ={\mu }_z-\sqrt{l+\kappa }{L}_{co{l}_i}\end{array}$$

其中，$l=dim({\mu }_z)$，$i=1,2,...,l$。

3. 对每个sigma point展开为状态和运动噪声的形式：

$$z_i=\left[\begin{array}{c}{\hat{x}}_{k-1,i}\\ w_{k,i}\end{array}\right]$$

接着将每个sigma point带入非线性运动模型进行精确求解：

$${\check{x}}_{k,i}=f({\hat{x}}_{k-1,i},v_k,w_{k,i})$$

其中，$i=0,1,...,2l$。

4. 将转换后的sigma point重新组合成预测置信度：

$$\begin{array}{rl}{\check{x}}_k& =\sum _{i=0}^{2L}{w}_i{\check{x}}_{k,i}\\ {\check{P}}_k& =\sum _{i=0}^{2L}{w}_i({\check{x}}_{k,i}-{\check{x}}_k)({\check{x}}_{k,i}-{\check{x}}_k{)}^T\end{array}$$

其中，

$$\begin{array}{rl}{w}_0& =\frac{\kappa }{l+\kappa }\\ w_i& =\frac{1}{2(l+\kappa )}\end{array}$$

其中，$i=1,2,...,2l$。

校正部分稍微复杂一些。广义高斯滤波中校正步骤的方程：

$$\begin{array}{rl}{K}_k& ={\Sigma }_{xy,k}{\Sigma }_{yy,k}^{-1}\\ {\hat{x}}_k& ={\check{x}}_k+K_k(y_k-{\mu }_{y,k})\\ {\hat{P}}_k& ={\check{P}}_k-K_k{\Sigma }_{xy,k}^T\end{array}$$

使用sigma point可以得到更优的${\mu }_{y,k}$、${\Sigma }_{yy,k}$和${\Sigma }_{xy,k}$的估计：

1. 预测置信度和观测噪声都不确定，将他们按以下方式堆叠在一起：

$$\begin{array}{rl}{\mu }_z& =\left[\begin{array}{c}{\check{x}}_k\\ 0\end{array}\right]\\ {\Sigma }_{zz}& =\left[\begin{array}{cc}{\check{P}}_k& 0\\ 0& R_k\end{array}\right]\end{array}$$

可以看到 { μ z , Σ z z } \{\mu_z,\Sigma_{zz}\} {μz​,Σzz​}仍然是高斯形式。

2. 将 { μ z , Σ z z } \{\mu_z,\Sigma_{zz}\} {μz​,Σzz​}转化为sigma point表示：

$$\begin{array}{rl}L{L}^T& ={\Sigma }_{zz}\\ z_0& ={\mu }_z\\ z_i& ={\mu }_z+\sqrt{l+\kappa }{L}_{co{l}_i}\\ z_{i+L}& ={\mu }_z-\sqrt{l+\kappa }{L}_{co{l}_i}\end{array}$$

其中，$l=dim({\mu }_z)$，$i=1,2,...,l$。

3. 对每个sigma point展开为状态和运动噪声的形式：

$$z_i=\left[\begin{array}{c}{\check{x}}_{k,i}\\ n_{k,i}\end{array}\right]$$

接着将每个sigma point带入非线性运动模型进行精确求解：

$${\check{y}}_{k,i}=g({\check{x}}_{k,i},n_{k,i})$$

其中，$i=0,1,...,2l$。

4. 将转换后的sigma point重新组合成最终的结果：

$$\begin{array}{rl}{\mu }_{y,k}& =\sum _{i=0}^{2L}{w}_i{\check{y}}_{k,i}\\ {\Sigma }_{yy,k}& =\sum _{i=0}^{2L}{w}_i({\check{y}}_{k,i}-{\mu }_{y,k})({\check{y}}_{k,i}-{\mu }_{y,k}{)}^T\\ {\Sigma }_{xy,k}& =\sum _{i=0}^{2L}{w}_i({\check{x}}_{k,i}-{\check{x}}_k)({\check{y}}_{k,i}-{\mu }_{y,k}{)}^T\end{array}$$

其中，

$$\begin{array}{rl}{w}_0& =\frac{\kappa }{l+\kappa }\\ w_i& =\frac{1}{2(l+\kappa )}\end{array}$$

其中，$i=1,2,...,2l$。

UKF的两个优点是：

• 不需要任何解析形式的导数；

• 仅使用了基本的线性代数运算。

滤波器分类

总结一下递归式非线性系统状态估计的几种方法。

贝叶斯滤波位于分类的最顶层。用高斯分布估计PDF就是高斯滤波，用大量随机样本估计PDF就是粒子滤波。在高斯滤波中，在线性化运动和测量模型中传递PDF就是EKF，对PDF进行确定性采样，在非线性运动和测量模型中传递就是UKF。

另外，迭代在滤波方法中作用也很明显。若没有选代，EKF和UKF都很难与全贝叶斯后验概率联系起来。然而，IEKF方法收敛于MAP的解（即模），而IUKF方法则收敛于全后验概率的均值。