卡尔曼滤波系列——（六）卡尔曼平滑

1 简介

对于此前的若干篇与卡尔曼滤波有关的博文，所描述的算法都是基于过去以及当前时刻的传感器观测结果以估计当前时刻系统的状态，此为滤波算法。而在一些应用场景中，使用者对于系统状态的估计实时性要求较低，青睐于获取更准确的系统状态估计效果，此时可以利用一长段时间内获得的所有传感器观测来估计期间各个时刻的系统状态，此为平滑算法。

卡尔曼平滑算法是其中常用的一种，又称为RTS平滑——Rauch–Tung–Striebel smoother （RTSS, Rauch et al., 1965），本篇博文将详细介绍该算法。

---

2 原理

2.1 概率状态空间模型

本应用适用于服从以下条件概率分布的概率状态空间模型或称为滤波模型：对于 $k=1,2,...$

$${\mathbf{x}}_k\sim p({\mathbf{x}}_k|{\mathbf{x}}_{k-1})=N({\mathbf{x}}_k|{\mathbf{A}}_k{\mathbf{x}}_{k-1},{\mathbf{Q}}_k)$$

$${\mathbf{y}}_k\sim p({\mathbf{y}}_k|{\mathbf{x}}_k)=N({\mathbf{y}}_k|{\mathbf{H}}_k{\mathbf{x}}_k,{\mathbf{R}}_k)$$

这里

• ${\mathbf{x}}_k\in {\mathbb{R}}^n$ 表示 $k$ 时刻的系统状态；
• ${\mathbf{y}}_k\in {\mathbb{R}}^m$ 表示 $k$ 时刻的测量结果；
• $p({\mathbf{x}}_k|{\mathbf{x}}_{k-1})$ 是描述系统随机动态的动状态模型；
• $p({\mathbf{y}}_k|{\mathbf{x}}_k)$ 是描述给定系统状态时测量结果概率分布的观测模型；
• $N(\mathbf{x}|\mu ,\sigma )$表示随机变量 $\mathbf{x}$ 所服从以 $\mu$ 为均值，以 $\sigma$ 为方差的高斯分布概率密度函数；

可见，该状态模型是一阶Markov链。

2.2 贝叶斯平滑公式

贝叶斯平滑的目的是在获得截止到时间 $T$ 的所有观测后，计算 $k$ 时刻系统状态 ${\mathbf{x}}_k$ 的边缘分布，即：

$$p({\mathbf{x}}_k|{\mathbf{y}}_{1:T})$$

其中 $T>k$ ，${\mathbf{x}}_k$ 表示 $k$ 时刻的系统状态，${\mathbf{y}}_{1:T}$ 表示 $1\sim T$ 时段的所有观测结果。

滤波和平滑的区别在于贝叶斯滤波利用的是过去和当前 $k$ 时刻的观测结果，而贝叶斯平滑则同时也利用了 $k$ 时刻之后的观测结果。基于 $1\sim T$ 时段的观测，有以下后验边缘分布的推论：

推论1（贝叶斯最优平滑公式）：用于计算任意 $k$ 时刻（ $k<T$ ）平滑分布 $p({\mathbf{x}}_k|{\mathbf{y}}_{1:T})$ 的后向递归方程（贝叶斯平滑器）为：

$$p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})=\int p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k)p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})d{\mathbf{x}}_k$$

$$p({\mathbf{x}}_k|{\mathbf{y}}_{1:T})=p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})\int \frac{p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k)p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:T})}{p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})}d{\mathbf{x}}_{k+1}$$

其中 $p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})$ 代表 $k$ 时刻的滤波分布，而 $p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})$ 代表对 $k+1$ 时刻的预测分布。

2.3 两个引理

• 定义（高斯分布） 若随机变量 $\mathbf{x}\in {\mathbb{R}}^n$ 的概率密度形式如下：

$$N(\mathbf{x}|\mathbf{m},\mathbf{P})=\frac{1}{(2\pi {)}^{n/2}{\left|\mathbf{P}\right|}^{1/2}}exp\left(\frac{1}{2}{\left(\mathbf{x}-\mathbf{m}\right)}^T{\mathbf{P}}^{-1}\left(\mathbf{x}-\mathbf{m}\right)\right)$$

则称变量服从均值为 $\mathbf{m}\in {\mathbb{R}}^n$ ，协方差为 $\mathbf{P}\in {\mathbb{R}}^{n\times n}$ 的高斯分布，其中 $\left|\mathbf{P}\right|$ 表示协方差矩阵 $\mathbf{P}$ 的行列式。

• 引理1 （高斯变量的联合分布）如果随机变量 $\mathbf{x}\in {\mathbb{R}}^n$ 和变量 $\mathbf{y}\in {\mathbb{R}}^m$ 服从下述高斯分布：

$$\mathbf{x}\sim N(\mathbf{m},\mathbf{P})$$

$$\mathbf{y}|\mathbf{x}\sim N(\mathbf{H}\mathbf{x}+\mathbf{u},\mathbf{R})$$

那么 $\mathbf{x}$ 和 $\mathbf{y}$ 的联合分布以及 $\mathbf{y}$ 的边缘分布为：

$$\left[\begin{array}{c}\mathbf{x}\\ \mathbf{y}\end{array}\right]\sim N\left(\left[\begin{array}{c}\mathbf{m}\\ \mathbf{H}\mathbf{m}+\mathbf{u}\end{array}\right],\left[\begin{array}{cc}\mathbf{P}& {\mathbf{P}\mathbf{H}}^T\\ \mathbf{H}\mathbf{P}& {\mathbf{H}\mathbf{P}\mathbf{H}}^T+\mathbf{R}\end{array}\right]\right)$$

$$\mathbf{y}\sim N(\mathbf{H}\mathbf{m}+\mathbf{u},{\mathbf{H}\mathbf{P}\mathbf{H}}^T+\mathbf{R})$$

• 引理2 （高斯变量的条件分布）如果随机变量 $\mathbf{x}\in {\mathbb{R}}^n$ 和变量 $\mathbf{y}\in {\mathbb{R}}^m$ 服从下述联合高斯分布：

$$\left[\begin{array}{c}\mathbf{x}\\ \mathbf{y}\end{array}\right]\sim N\left(\left[\begin{array}{c}\mathbf{a}\\ \mathbf{b}\end{array}\right],\left[\begin{array}{cc}\mathbf{A}& \mathbf{C}\\ {\mathbf{C}}^T& \mathbf{B}\end{array}\right]\right)$$

那么 $\mathbf{x}$ 和 $\mathbf{y}$ 的边缘分布及条件分布分别为：

$$\mathbf{x}\sim N(\mathbf{a},\mathbf{A})$$

$$\mathbf{y}\sim N(\mathbf{b},\mathbf{B})$$

$$\mathbf{x}|\mathbf{y}\sim N(\mathbf{a}+\mathbf{C}{\mathbf{B}}^{-1}(\mathbf{y}-\mathbf{b}),\mathbf{A}-\mathbf{C}{\mathbf{B}}^{-1}{\mathbf{C}}^T)$$

$$\mathbf{y}|\mathbf{x}\sim N({\mathbf{b}+\mathbf{C}}^T{\mathbf{A}}^{-1}(\mathbf{x}-\mathbf{a}),{\mathbf{B}-\mathbf{C}}^T{\mathbf{A}}^{-1}\mathbf{C})$$

2.4 RTS平滑

RTS平滑可以用于计算以下闭合解

$$p({\mathbf{x}}_k|{\mathbf{y}}_{1:T})=N({\mathbf{x}}_k|{\mathbf{m}}_k^s,{\mathbf{P}}_k^s)$$

即利用观测 ${\mathbf{y}}_{1:T}$ 估计 $k$ 时刻的系统状态后验分布，该过程分成前向和后向两步：

• 前向递推（Forward Recursion）：

$${\mathbf{m}}_{k+1}^{-}={\mathbf{A}}_k{\mathbf{m}}_k$$

$${\mathbf{P}}_k^{-}={\mathbf{A}}_k{\mathbf{P}}_k{\mathbf{A}}_k^T+{\mathbf{Q}}_k$$

$${\mathbf{S}}_k^{-}=({\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\mathbf{R}}_k{)}^{-1}$$

$${\mathbf{K}}_k={\mathbf{P}}_k^{-}{\mathbf{H}}_k^T{\mathbf{S}}_k^{-}$$

$${\mathbf{m}}_k={\mathbf{m}}_k^{-}+{\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{H}}_k{\mathbf{m}}_k^{-})$$

$${\mathbf{P}}_k=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_k^{-}$$

• 后向递推（Backward Recursion）：

$${\mathbf{m}}_{k+1}^{-}={\mathbf{A}}_k{\mathbf{m}}_k$$

$${\mathbf{P}}_{k+1}^{-}={\mathbf{A}}_k{\mathbf{P}}_k{\mathbf{A}}_k^T+{\mathbf{Q}}_k$$

$${\mathbf{G}}_k={\mathbf{P}}_k{\mathbf{A}}_k^T({\mathbf{P}}_{k+1}^{-}{)}^{-1}$$

$${\mathbf{m}}_k^s={\mathbf{m}}_k+{\mathbf{G}}_k({\mathbf{m}}_{k+1}^s-{\mathbf{m}}_{k+1}^{-})$$

$${\mathbf{P}}_k^s={\mathbf{P}}_k+{\mathbf{G}}_k({\mathbf{P}}_{k+1}^s-{\mathbf{P}}_{k+1}^{-}){\mathbf{G}}_k^T$$

从初始时刻1到 $T$ 时刻完成 $T$ 次前向递推后，再由 $T$ 时刻经过 $T$ 次后向递推，完成RTS平滑过程。其中前向递推过程即为卡尔曼滤波过程，而前向递推获得的最后 $T$ 时刻的状态估计 ${\mathbf{m}}_T$ 和协方差矩阵 ${\mathbf{P}}_T$ 即为后向递推过程的初始状态估计 ${\mathbf{m}}_T^s$ 与协方差矩阵 ${\mathbf{P}}_T^s$ ，即 ${\mathbf{m}}_T={\mathbf{m}}_T^s$ ， ${\mathbf{P}}_T={\mathbf{P}}_T^s$ 。

---

3 推导

3.1 贝叶斯平滑公式的推导

对于我们在目标跟踪上的应用，系统的状态转移是一阶Markov模型。根据一阶Markov的性质，当给定 ${\mathbf{x}}_{k+1}$ 时， 状态 ${\mathbf{x}}_k$ 与观测 ${\mathbf{y}}_{k+1:T}$ 无关，即 $p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:T})=p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:k})$ ，于是利用贝叶斯公式有：

$$\begin{gathered} p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:T})=p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:k}) \\ =\frac{p({\mathbf{x}}_k,{\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})}{p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})} \\ =\frac{p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k,{\mathbf{y}}_{1:k})p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})}{p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})} \\ =\frac{p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k)p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})}{p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})} \end{gathered}$$

于是在给定观测 ${\mathbf{y}}_{1:T}$ 下， ${\mathbf{x}}_k$ 和 ${\mathbf{x}}_k$ 的联合分布为

$$\begin{gathered} p({\mathbf{x}}_k,{\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:T})=p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:T})p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:T}) \\ =p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:k})p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:T}) \\ =\frac{p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k)p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:T})}{p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})} \end{gathered}$$

其中 $p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:T})$ 即为 $k+1$ 时刻的平滑分布，而系统状态 ${\mathbf{x}}_k$ 在给定观测 ${\mathbf{y}}_{1:T}$ 的边缘分布可以通过对 ${\mathbf{x}}_{k+1}$ 的积分获得，因此便可以推出2.1节的贝叶斯平滑公式。

3.2 RTS平滑的推导

• 前向递推公式的推导：

1. 利用2.3节引理1，给定观测 ${\mathbf{y}}_{1:k-1}$ 时 ${\mathbf{x}}_k$ 和 ${\mathbf{x}}_{k-1}$ 的联合分布为：

$$\begin{gathered} p({\mathbf{x}}_k,{\mathbf{x}}_{k-1}|{\mathbf{y}}_{1:k-1}) \\ =p({\mathbf{x}}_k|{\mathbf{x}}_{k-1})p({\mathbf{x}}_{k-1}|{\mathbf{y}}_{1:k-1}) \\ =N({\mathbf{x}}_k|{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1},{\mathbf{Q}}_{k-1})N({\mathbf{x}}_{k-1}|{\mathbf{m}}_{k-1},{\mathbf{P}}_{k-1}) \\ =N\left(\left[\begin{array}{c}{\mathbf{x}}_{k-1}\\ {\mathbf{x}}_k\end{array}\right]|{\mathbf{m}}^{{}^{\prime }},{\mathbf{P}}^{{}^{\prime }}\right) \end{gathered}$$

$${\mathbf{m}}^{{}^{\prime }}=\left[\begin{array}{c}{\mathbf{m}}_{k-1}\\ {\mathbf{A}}_{k-1}{\mathbf{m}}_{k-1}\end{array}\right]$$

$${\mathbf{P}}^{{}^{\prime }}=\left[\begin{array}{cc}{\mathbf{P}}_{k-1}& {\mathbf{P}}_{k-1}{\mathbf{A}}_{k-1}^T\\ {\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}& {\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}{\mathbf{A}}_{k-1}^T+{\mathbf{Q}}_{k-1}\end{array}\right]$$

2. 然后利用2.3节引理2可以推出 ${\mathbf{x}}_k$ 的边缘分布为：

$$p({\mathbf{x}}_k|{\mathbf{y}}_{1:k-1})=N({\mathbf{x}}_k|{\mathbf{m}}_k^{-},{\mathbf{P}}_k^{-})$$

$${\mathbf{m}}_k^{-}={\mathbf{A}}_{k-1}{\mathbf{m}}_{k-1}$$

$${\mathbf{P}}_k^{-}={\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}{\mathbf{A}}_{k-1}^T+{\mathbf{Q}}_{k-1}$$

3. 再次利用2.3节引理1可以推出给定观测 ${\mathbf{y}}_{1:k-1}$ 时 ${\mathbf{x}}_k$ 和 ${\mathbf{y}}_k$ 的联合分布为：

$$\begin{gathered} p({\mathbf{x}}_k,{\mathbf{y}}_k|{\mathbf{y}}_{1:k-1}) \\ =p({\mathbf{y}}_k|{\mathbf{x}}_k)p({\mathbf{x}}_k|{\mathbf{y}}_{1:k-1}) \\ =N({\mathbf{y}}_k|{\mathbf{H}}_k{\mathbf{x}}_k,{\mathbf{R}}_k)N({\mathbf{x}}_k|{\mathbf{m}}_k^{-},{\mathbf{P}}_k^{-}) \\ =N\left(\left[\begin{array}{c}{\mathbf{x}}_k\\ {\mathbf{y}}_k\end{array}\right]|{\mathbf{m}}^{{}^{\prime \prime }},{\mathbf{P}}^{{}^{\prime \prime }}\right) \end{gathered}$$

$${\mathbf{m}}^{{}^{\prime \prime }}=\left[\begin{array}{c}{\mathbf{m}}_k^{-}\\ {\mathbf{H}}_k{\mathbf{m}}_k^{-}\end{array}\right]$$

$${\mathbf{P}}^{{}^{\prime \prime }}=\left[\begin{array}{cc}{\mathbf{P}}_k^{-}& {\mathbf{P}}_k^{-}{\mathbf{H}}_k^T\\ {\mathbf{H}}_k{\mathbf{P}}_k^{-}& {\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\mathbf{R}}_k\end{array}\right]$$

4. 最后再利用2.3节引理2可以推出给定观测 ${\mathbf{y}}_{1:k}$ 时 ${\mathbf{x}}_k$ 的条件分布为：

$$p({\mathbf{x}}_k|{\mathbf{y}}_k,{\mathbf{y}}_{1:k-1})=p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})=N({\mathbf{x}}_k|{\mathbf{m}}_k,{\mathbf{P}}_k)$$

$${\mathbf{m}}_k={\mathbf{m}}_k^{-}+{\mathbf{P}}_k^{{}^{\prime }}{\mathbf{H}}_k^T({\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\mathbf{R}}_k{)}^{-1}({\mathbf{z}}_k-{\mathbf{H}}_k{\mathbf{m}}_k^{-})$$

$${\mathbf{P}}_k={\mathbf{P}}_k^{{}^{\prime }}-{\mathbf{P}}_k^{{}^{\prime }}{\mathbf{H}}_k^T({\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\mathbf{R}}_k{)}^{-1}{\mathbf{H}}_k{\mathbf{P}}_k^{{}^{\prime }}$$

• 后向递推公式的推导：

1. 类似于前向递推公式的推导过程，利用2.3节引理1在给定观测 ${\mathbf{y}}_{1:k}$ 时， ${\mathbf{x}}_k$ 和 ${\mathbf{x}}_{k+1}$ 的联合分布为：
   $$\begin{gathered} p({\mathbf{x}}_k,{\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})=p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k)p({\mathbf{x}}_k|{\mathbf{y}}_{1:k}) \\ =N({\mathbf{x}}_{k+1}|{\mathbf{A}}_k{\mathbf{x}}_k,{\mathbf{Q}}_k)N({\mathbf{x}}_k|{\mathbf{m}}_k,{\mathbf{P}}_k) \\ =N\left(\left[\begin{array}{c}{\mathbf{x}}_k\\ {\mathbf{x}}_{k+1}\end{array}\right]|{\mathbf{m}}_1,{\mathbf{P}}_1\right) \end{gathered}$$

$${\mathbf{m}}_1=\left[\begin{array}{c}{\mathbf{m}}_k\\ {\mathbf{A}}_k{\mathbf{m}}_k\end{array}\right]$$

$${\mathbf{P}}_1=\left[\begin{array}{cc}{\mathbf{P}}_k& {\mathbf{P}}_k{\mathbf{A}}_k^T\\ {\mathbf{A}}_k{\mathbf{P}}_k& {\mathbf{A}}_k{\mathbf{P}}_k{\mathbf{A}}_k^T+{\mathbf{Q}}_k\end{array}\right]$$

2. 考虑到系统状态的Markov性质有 $p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:T})=p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:k})$ ，然后利用2.3节引理2有如下条件分布：

$$\begin{gathered} p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:T})=p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:k}) \\ =N({\mathbf{x}}_k|{\mathbf{m}}_2,{\mathbf{P}}_2) \end{gathered}$$

$${\mathbf{G}}_k={\mathbf{P}}_k{\mathbf{A}}_k^T({\mathbf{A}}_k{\mathbf{P}}_k{\mathbf{A}}_k^T+{\mathbf{Q}}_k{)}^{-1}$$

$${\mathbf{m}}_2={\mathbf{m}}_k+{\mathbf{G}}_k[{\mathbf{m}}_{k+1}-{\mathbf{A}}_k{\mathbf{m}}_k]$$

$${\mathbf{P}}_2={\mathbf{P}}_k-{\mathbf{G}}_k({\mathbf{A}}_k{\mathbf{P}}_k{\mathbf{A}}_k^T+{\mathbf{Q}}_k){\mathbf{G}}_k^T$$

3. 因此在给定所有观测数据时，利用2.3节引理1可以推出 ${\mathbf{x}}_k$ 和 ${\mathbf{x}}_{k+1}$ 的联合分布为：

$$\begin{gathered} p({\mathbf{x}}_k,{\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:T})=p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:T})p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:T}) \\ =N({\mathbf{x}}_k|{\mathbf{m}}_2,{\mathbf{P}}_2)N({\mathbf{x}}_{k+1}|{\mathbf{m}}_{k+1}^s,{\mathbf{P}}_{k+1}^s) \\ =N(\left[\begin{array}{c}{\mathbf{x}}_k\\ {\mathbf{x}}_{k+1}\end{array}\right]|{\mathbf{m}}_3,{\mathbf{P}}_3) \end{gathered}$$

$${\mathbf{m}}_3=\left[\begin{array}{c}{\mathbf{m}}_{k+1}^s\\ {\mathbf{m}}_k+{\mathbf{G}}_k({\mathbf{m}}_{k+1}^s-{\mathbf{A}}_k{\mathbf{m}}_k)\end{array}\right]$$

$${\mathbf{P}}_3=\left[\begin{array}{cc}{\mathbf{P}}_{k+1}^s& {\mathbf{P}}_{k+1}^s{\mathbf{G}}_k^T\\ {\mathbf{G}}_k{\mathbf{P}}_{k+1}^s& {\mathbf{G}}_k{\mathbf{P}}_{k+1}^s{\mathbf{G}}_k^T+{\mathbf{P}}_2\end{array}\right]$$

4. 因此，利用2.3节引理2可以得到 ${\mathbf{x}}_k$ 的边缘分布为

$$p({\mathbf{x}}_k|{\mathbf{y}}_{1:T})=N({\mathbf{x}}_k|{\mathbf{m}}_k^s,{\mathbf{P}}_k^s)$$

$${\mathbf{m}}_k^s={\mathbf{m}}_k+{\mathbf{G}}_k({\mathbf{m}}_{k+1}^s-{\mathbf{A}}_k{\mathbf{m}}_k)$$

$${\mathbf{P}}_k^s={\mathbf{P}}_k+{\mathbf{G}}_k({\mathbf{P}}_{k+1}^s-{\mathbf{A}}_k{\mathbf{P}}_k{\mathbf{A}}_k^T-{\mathbf{Q}}_k){\mathbf{G}}_k^T$$

证毕。

---

4 实验

以一正弦信号的高斯随机走动为例，信号实际波形如下图所示：

由于传感器的测量存在一定的噪声，因此测量结果波动较大，与信号的实际波形存在一定的差距，如下图所示：

若直接将传感器的测量值作为信号的实际值，显然与预期存在一定差距，因此可以利用卡尔曼滤波方法或卡尔曼平滑方法处理观测结果，从而获得更平滑稳定的波形估计。

首先建立系统模型如下，状态转移方程和状态观测方程分别为：

$$x_k=x_{k-1}+q_k,q_k\sim N(0,Q)$$

$$y_k=x_k+r_k,r_k\sim N(0,R)$$

根据此模型，有 $A=1$ , $H=1$ ，另外考虑到系统观测噪声较大，因此取 $Q=0.05$ , $R=1$ 。然后分别利用卡尔曼滤波算法卡尔曼平滑算法处理数据，得到如下结果：

---

5 结论

卡尔曼平滑算法其实就是卡尔曼滤波算法的加强版，包括了前向递推和后向递推两个步骤，其中前向递推的过程与卡尔曼滤波算法时一致的，而后向递推可以进一步减少估计结果的波动。算法的推导过程主要依赖于引理1和引理2的反复运用，利用贝叶斯概率理论实现系统状态的后验估计，与博文卡尔曼滤波系列一——标准卡尔曼滤波的推导过程有些不一样，不过结果是统一的。

根据实验结果可见，对于相同的观测数据，参数相同的两个算法的处理结果存在一定差异，其中卡尔曼平滑算法虽然实时性不如卡尔曼滤波算法，但其估计得到的信号波形更为平滑。而且仔细看可以看出，卡尔曼滤波算法相对于信号真值存在微小的相位滞后，而卡尔曼平滑算法的估算结果则保持较好的相位一致性，这在实际应用中有重要价值。通过计算两个算法处理结果与信号真值的均方误差，例如上述实验中，卡尔曼滤波算法的MSE为0.0078，而卡尔曼平滑算法的MSE伪0.0025，可见，平滑算法的估计精度更高，效果更好。

在实际应用中，如果信号长度较大，可以将信号按照时间先后分成若干个重叠的时段，然后对每一段分别使用平滑算法，这样可以得到更好地效果，同时也在一定程度上减少传感器的测量时间，降低算法处理时间，提高实时性。

---

6 参考文献

[1] Simo Srkk. Bayesian Filtering and Smoothing.

---

原创性声明：本文属于作者原创性文章，小弟码字辛苦，转载还请注明出处。谢谢~

代码下载请到本博文实验程序。

如果有哪些地方表述的不够得体和清晰，有存在的任何问题，亦或者程序存在任何考虑不周和漏洞，欢迎评论和指正，谢谢各路大佬。

有需要相关技术支持的可咨询QQ：297461921