扩展、统计线性化和无迹RTS平滑器

本文介绍基于线性化、统计线性化和无迹变换的RTS平滑器，分别对应EKF、SLF和ULF。
这些平滑器用于对非线性状态空间模型贝叶斯解的高斯逼近。

扩展RTS平滑器（ERTSS）

ERTSS对平滑分布的高斯逼近表达式为：由下式叠加模型：$$\begin{gathered} {\mathbf{x}}_{\mathbf{k}}=\mathbf{f}({\mathbf{x}}_{\mathbf{k}-1})+{\mathbf{q}}_{\mathbf{k}-1} \\ {\mathbf{y}}_{\mathbf{k}}=\mathbf{h}({\mathbf{x}}_{\mathbf{k}})+\mathbf{r} \end{gathered}$$扩展的RTS平滑算法如下。

扩展RTS平滑器

式中，雅可比矩阵 ${\mathbf{F}}_{\mathbf{x}}({\mathbf{m}}_{\mathbf{k}})$ 为 $\mathbf{f}(\mathbf{x})$ 在 ${\mathbf{m}}_{\mathbf{k}}$ 处的值。
上式展示的为由$\mathbf{k}$ 时刻到 $\mathbf{k}+1$ 时刻到平滑分布的递推过程。由于在最后 $\mathbf{T}$ 时刻的平滑分布于滤波分布一致。即：
$$\begin{gathered} {\mathbf{m}}_{\mathbf{T}}^{\mathbf{s}}={\mathbf{m}}_{\mathbf{T}} \\ {\mathbf{P}}_{\mathbf{T}}^{\mathbf{s}}={\mathbf{P}}_{\mathbf{T}} \end{gathered}$$故从最后时候 $\mathbf{k}=\mathbf{T}$ 开始向后递推到最初 $\mathbf{k}=0$ 时刻，这样就能够求解出所有时刻的平滑分布。

统计线性化RTS平滑器（SLRTSS）

统计线性化RTS平滑器与扩展RTS平滑器整体一致，只是用统计线性化的方法代替了一般的线性化。

SLTRSS

统计线性化RTS平滑方程为：

式中，期望值服从滤波分布${\mathbf{x}}_{\mathbf{k}}\sim \mathbf{p}({\mathbf{m}}_{\mathbf{k}},{\mathbf{P}}_{\mathbf{k}})$
非叠加型SLRTSS可参考统计线性化滤波器,只需要更改平滑方程中的前两项即可。

无迹RTS平滑器（URTSS）

无迹RTS平滑器是一种高斯逼近平滑器，由无迹变换来逼近其非线性。

叠加型URTSS：

1). 构造 $\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$ 点：
式中，参数 $\mathbf{\lambda }$ 的定义见： https://blog.csdn.net/calabash_man/article/details/105620575 相关部分。
2). 将 $\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$ 点带入动态模型：
3). 计算预测均值 ${\mathbf{m}}_{\mathbf{k}+1}^{-}$，预测协方差 ${\mathbf{P}}_{\mathbf{k}+1}^{-}$ 和互协方差 ${\mathbf{D}}_{\mathbf{k}+1}$：
式中，权值 ${\mathbf{W}}_{\mathbf{i}}^{(\mathbf{m})},{\mathbf{W}}_{\mathbf{i}}^{(\mathbf{c})}$定义见： https://blog.csdn.net/calabash_man/article/details/105620575 相关部分。
4). 计算平滑增益 ${\mathbf{G}}_{\mathbf{k}}$，平滑均值${\mathbf{m}}_{\mathbf{k}}^{\mathbf{s}}$ 和平滑协方差 ${\mathbf{P}}_{\mathbf{k}}^{\mathbf{s}}$：

以上计算从滤波结束时刻开始，即：
$$\begin{gathered} {\mathbf{m}}_{\mathbf{T}}^{\mathbf{s}}={\mathbf{m}}_{\mathbf{T}} \\ {\mathbf{P}}_{\mathbf{T}}^{\mathbf{s}}={\mathbf{P}}_{\mathbf{T}} \end{gathered}$$

从最后时候 $\mathbf{k}=\mathbf{T}$ 开始向后递推到最初 $\mathbf{k}=0$ 时刻，这样就能够求解出所有时刻的平滑分布。

非叠加型无迹RTS平滑器：

1). 构造 $\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$ 点：
式中 ${\mathbf{n}}^{{}^{\prime }}=\mathbf{n}+{\mathbf{n}}_{\mathbf{q}}$ 为 ${\mathbf{x}}_{\mathbf{k}},{\mathbf{q}}_{\mathbf{k}}$ 的维度之和。
2). 将 $\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$ 点带入动态模型：
式中，${\tilde{\mathbf{\chi }}}_{\mathbf{k}}^{(\mathbf{i}),\mathbf{x}},{\tilde{\mathbf{\chi }}}_{\mathbf{k}}^{(\mathbf{i}),\mathbf{q}}$ 分别为 ${\mathbf{x}}_{\mathbf{k}},{\mathbf{q}}_{\mathbf{k}}$ 的扩展 $\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$ 点。
3). 计算预测均值 ${\mathbf{m}}_{\mathbf{k}+1}^{-}$，预测协方差 ${\mathbf{P}}_{\mathbf{k}+1}^{-}$ 和互协方差 ${\mathbf{D}}_{\mathbf{k}+1}$：
参数 ${\mathbf{\lambda }}^{{}^{\prime }}$ 和权值${\mathbf{W}}_{\mathbf{i}}^{(\mathbf{m}{)}^{\prime }},{\mathbf{W}}_{\mathbf{i}}^{(\mathbf{c}{)}^{\prime }}$ 的定义见： https://blog.csdn.net/calabash_man/article/details/105620575 相关部分。
4). 计算平滑增益 ${\mathbf{G}}_{\mathbf{k}}$，平滑均值${\mathbf{m}}_{\mathbf{k}}^{\mathbf{s}}$ 和平滑协方差 ${\mathbf{P}}_{\mathbf{k}}^{\mathbf{s}}$：

以上计算从滤波结束时刻开始，即：
$$\begin{gathered} {\mathbf{m}}_{\mathbf{T}}^{\mathbf{s}}={\mathbf{m}}_{\mathbf{T}} \\ {\mathbf{P}}_{\mathbf{T}}^{\mathbf{s}}={\mathbf{P}}_{\mathbf{T}} \end{gathered}$$

从最后时候 $\mathbf{k}=\mathbf{T}$ 开始向后递推到最初 $\mathbf{k}=0$ 时刻，这样就能够求解出所有时刻的平滑分布。

根据已知实验模拟分析，平滑器所得结果的均方根误差均小于滤波器所得结果。且统计线性化RTS平滑器和无迹RTS平滑器的平滑结果略优于扩展RTS平滑器。