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

三种平滑方式：

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

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

1.正向滤波（forward）

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

2.反向滤波（backward）

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

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

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

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

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

5、RTS区间平滑算法

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

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

6、平滑精度比较

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