11-卡尔曼滤波原理（十一）区间平滑：前向滤波、反向滤波、双向区间平滑、RTS平滑.md

最优预测、估计与平滑之间的关系：

三种平滑方式：

函数模型和随机模型 $$ \left{\begin{array} { l } { \boldsymbol { X } _ { k } = \boldsymbol { \Phi } _ { k / k - 1 } \boldsymbol { X } _ { k - 1 } + \boldsymbol { \Gamma } _ { k - 1 } \boldsymbol { W } _ { k - 1 } } \ { \boldsymbol { Z } _ { k } = \boldsymbol { H } _ { k } \boldsymbol { X } _ { k } + \boldsymbol { V } _ { k } } \end{array} \quad \left{\begin{array}{ll} \mathrm{E}\left[\boldsymbol{W}{k}\right]=\mathbf{0}, & \mathrm{E}\left[\boldsymbol{W}{k} \boldsymbol{W}{j}^{\mathrm{T}}\right]=\boldsymbol{Q}{k} \delta_{k j} \ \mathrm{E}\left[\boldsymbol{V}{k}\right]=\mathbf{0}, & \mathrm{E}\left[\boldsymbol{V}{k} \boldsymbol{V}{j}^{\mathrm{T}}\right]=\boldsymbol{R}{k} \delta_{k j} \ \mathrm{E}\left[\boldsymbol{W}{k} \boldsymbol{V}{j}^{\mathrm{T}}\right]=\mathbf{0} & \end{array}\right.\right. $$ 将量测序列分成两段：$\bar{Z}{M}=\underbrace{\left[\boldsymbol{Z}{1} \boldsymbol{Z}{2} \cdots \boldsymbol{Z}{j}\right.}{\overline{\boldsymbol{Z}}{\mathrm{1} \cdot j}} \underbrace{\mathbf{Z}{j+1} \boldsymbol{Z}{j+2} \cdots \boldsymbol{Z}{M}}{\bar{Z}_{j+1+M}}]^{\mathrm{T}}$

先看固定点平滑，固定区间平滑只是将点的范围扩大了，将点前后平移即可

1.正向滤波（forward）

通过前面一段做正向滤波，就是普通的Kalman滤波，下标都加了 $f$ 表示正向，： $$ \left{\begin{array}{l} \hat{\boldsymbol{X}}{f, k / k-1}=\boldsymbol{\Phi}{k / k-1} \hat{\boldsymbol{X}}{f, k-1} \ \boldsymbol{P}{f, k / k-1}=\boldsymbol{\Phi}{k / k-1} \boldsymbol{P}{f, k-1} \boldsymbol{\Phi}{k / k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}{k-1} \boldsymbol{Q}{k-1} \boldsymbol{\Gamma}{k-1}^{\mathrm{T}} \ \boldsymbol{K}{f, k}=\boldsymbol{P}{f, k / k-1} \boldsymbol{H}{k}^{\mathrm{T}}\left(\boldsymbol{H}{k} \boldsymbol{P}{f, k / k-1} \boldsymbol{H}{k}^{\mathrm{T}}+\boldsymbol{R}{k}\right)^{-1} \quad k=1,2, \cdots, j \ \hat{\boldsymbol{X}}{f, k}=\hat{\boldsymbol{X}}{f, k / k-1}+\boldsymbol{K}{f, k}\left(\boldsymbol{Z}{k}-\boldsymbol{H}{k} \hat{\boldsymbol{X}}{f, k / k-1}\right) \ \boldsymbol{P}{f, k}=\left(\boldsymbol{I}-\boldsymbol{K}{f, k} \boldsymbol{H}{k}\right) \boldsymbol{P}{f, k / k-1} \end{array}\right. $$ 求得 $j$ 时刻估计 $\hat{\boldsymbol{X}}{f, j}, \boldsymbol{P}_{f, j}$

2.反向滤波（backward）

从后往前推，需要对Kalman滤波模型改写：把状态转移矩阵变为求逆的形式，表示由后时刻预测前时刻： $$ \left{\begin{array}{l}\boldsymbol{X}{k}={\color{red}\boldsymbol{\Phi}{k / k-1}} \boldsymbol{X}{k-1}+\boldsymbol{\Gamma}{k-1} \boldsymbol{W}{k-1} \ \boldsymbol{Z}{k}=\boldsymbol{H}{k} \boldsymbol{X}{k}+\boldsymbol{V}{k}\end{array} \Longrightarrow\left{\begin{array}{l}\boldsymbol{X}{k}={\color{red}\boldsymbol{\Phi}{k+1 / k}^{-1}} \boldsymbol{X}{k+1}-{\color{red}\boldsymbol{\Phi}{k+1 / k}^{-1}} \boldsymbol{\Gamma}{k} \boldsymbol{W}{k} \ \boldsymbol{Z}{k}=\boldsymbol{H}{k} \boldsymbol{X}{k}+\boldsymbol{V}{k}\end{array}\right.\right. $$ 令 $\boldsymbol{\Phi}{k+1 / k}^{}=\boldsymbol{\Phi}{k+1 / k}^{-1}$ ，$\boldsymbol{\Gamma}{k}^{}=-\boldsymbol{\Phi}{k+1 / k}^{-1} \boldsymbol{\Gamma}{k}$ ，$\boldsymbol{W}{k+1}^{*}=\boldsymbol{W}{k}$ ，得新的函数模型： $$ \left{\begin{array}{l} \boldsymbol{X}{k}=\boldsymbol{\Phi}{k+1 / k}^{} \boldsymbol{X}{k+1}+\boldsymbol{\Gamma}{k}^{} \boldsymbol{W}{k+1}^{*} \ \boldsymbol{Z}{k}=\boldsymbol{H}{k} \boldsymbol{X}{k}+\boldsymbol{V}{k} \end{array}\right. $$ 对新的函数模型做Kalman滤波： $$ \left{\begin{array}{l} \hat{\boldsymbol{X}}{b, k / k+1}=\boldsymbol{\Phi}{k / k+1}^{*} \hat{\boldsymbol{X}}{b, k+1} \ \boldsymbol{P}{b, k / k+1}=\boldsymbol{\Phi}{k / k+1}^{} \boldsymbol{P}{b, k+1}\left(\boldsymbol{\Phi}{k / k+1}^{}\right)^{\mathrm{T}}+\boldsymbol{\Gamma}{k}^{*} \boldsymbol{Q}{k} \boldsymbol{\Gamma}{k}^{*} \ \boldsymbol{K}{b, k}=\boldsymbol{P}{b, k / k+1} \boldsymbol{H}{k}^{\mathrm{T}}\left(\boldsymbol{H}{k} \boldsymbol{P}{b, k / k+1} \boldsymbol{H}{k}^{\mathrm{T}}+\boldsymbol{R}{k}\right)^{-1} \quad k=M-1, M-2, \cdots, j+1 \ \hat{\boldsymbol{X}}{b, k}=\hat{\boldsymbol{X}}{b, k / k+1}+\boldsymbol{K}{b, k}\left(\boldsymbol{Z}{k}-\boldsymbol{H}{k} \hat{\boldsymbol{X}}{b, k / k+1}\right) \ \boldsymbol{P}{b, k}=\left(\boldsymbol{I}-\boldsymbol{K}{b, k} \boldsymbol{H}{k}\right) \boldsymbol{P}{b, k / k+1} \end{array}\right. $$ 求得 $j$ 时刻反向一步预测 $\hat{\boldsymbol{X}}{b, j / j+1}, \boldsymbol{P}{b, j / j+1}$

3、j 时刻固定点信息融合(smoothing)

将利用前面观测值正向滤波得到的观测值 $\hat{\boldsymbol{X}}{f, j}, \boldsymbol{P}{f, j}$ ，和反向滤波得到的观测值 $\hat{\boldsymbol{X}}{b, j / j+1}, \boldsymbol{P}{b, j / j+1}$ 做信息融合（加权平均），并且认为两段滤波的结果不相关： $$ \begin{array}{c} \left{\begin{array} { l } { \hat { \boldsymbol { X } } _ { f , j } = \boldsymbol { X } _ { j } + \boldsymbol { \Delta } _ { f , j } } \ { \hat { \boldsymbol { X } } _ { b , j / j + 1 } = \boldsymbol { X } _ { j } + \boldsymbol { \Delta } _ { b , j / j + 1 } } \end{array} \quad \left{\begin{array}{l} \boldsymbol{\Delta}{f, j} \sim \mathrm{N}\left(\mathbf{0}, \boldsymbol{P}{f, j}\right), \ \boldsymbol{\Delta}{b, j / j+1} \sim \mathrm{N}\left(\mathbf{0}, \boldsymbol{P}{b, j / j+1}\right), \ \operatorname{cov}\left(\boldsymbol{U}{f, j} \boldsymbol{\Delta}{b, j / j+1}^{\mathrm{T}}\right)=\mathbf{0} \end{array}\right.\right. \ \Longrightarrow\left{\begin{array}{l} \boldsymbol{P}{s, j}=\left(\boldsymbol{P}{f, j}^{-1}+\boldsymbol{P}{b, j / j+1}^{-1}\right)^{-1} \ \hat{\boldsymbol{X}}{s, j}=\boldsymbol{P}{s, j}\left(\boldsymbol{P}{b, j / j+1}^{-1} \hat{\boldsymbol{X}}{f, j}+\boldsymbol{P}{f, j}^{-1} \hat{\boldsymbol{X}}_{b, j / j+1}\right) \end{array}\right. \end{array} $$

4、基于正反向滤波的区间平滑

1. 由前往后正向滤波, 获得并存储 $\hat{\boldsymbol{X}}{f, j}, \boldsymbol{P}{f, j}(j=1,2, \cdots, M)$;
2. 由后往前反向滤波, 获得 $\hat{\boldsymbol{X}}{b, j / j+1}, \boldsymbol{P}{b, j / j+1}$, 融合获得 $\hat{\boldsymbol{X}}{s, j}, \boldsymbol{P}{s, j}$;
3. 当 $(j=M-1, M-2, \cdots, 1)$ 完成整个区间平滑。

5、RTS区间平滑算法

H.Rauch, F.Tung, C.Striebel, 1965 ，推导比较复杂

不需要时间更新，先由前往后正向滤波, 获得并存储 $\boldsymbol{\Phi}{j / j-1}^{\mathrm{T}}, \hat{\boldsymbol{X}}{f, j / j-1}$, $\boldsymbol{P}{f, j / j-1}, \hat{\boldsymbol{X}}{f, j}, \boldsymbol{P}{f, j}(j=1,2, \cdots, M)$ 再按由后往前的量测顺序执行如下RTS算法： $$ \left{\begin{array}{ll}\boldsymbol{K}{s, k}=\boldsymbol{P}{f, k} \boldsymbol{\Phi}{k+1 / k}^{\mathrm{T}} \boldsymbol{P}{f, k+1 / k}^{-1} & \text { 初值 } \hat{\boldsymbol{X}}{s, M}=\hat{\boldsymbol{X}}{f, M}, \boldsymbol{P}{s, M}=\boldsymbol{P}{f, M} \ \hat{\boldsymbol{X}}{s, k}=\hat{\boldsymbol{X}}{f, k}+\boldsymbol{K}{s, k}\left(\hat{\boldsymbol{X}}{s, k+1}-\hat{\boldsymbol{X}}{f, k+1 / k}\right) & k=M-1, M-2, \cdots, 1 \ \boldsymbol{P}{s, k}=\boldsymbol{P}{f, k}+\boldsymbol{K}{s, k}\left(\boldsymbol{P}{s, k+1}-\boldsymbol{P}{f, k+1 / k}\right) \boldsymbol{K}{s, k}^{\mathrm{T}} & \end{array}\right. $$ 计算量和正反向滤波的区间平滑差不多，存储量增加不少

6、平滑精度比较

• 可平滑性问题：受系统噪声影响的状态才具可平滑性。随机常数就没有可平滑性。
• 滤波要存的数据量比较大，可能几个G，工程上双向滤波+ P 阵对角线加权平均，可有效降低存储量。

我的理解，RTS和双向滤波等价，比单向滤波精度高，全高斯白噪声理想情况精度高一倍；双向滤波是从前往后算一遍，存下状态向量、协方差，再从后往前算一遍，前后的结果取加权平均；RTS也是先从前往后算一遍，存的量更多，但从后往前算可以直接出平滑后的结果。