对于此前的若干篇与卡尔曼滤波有关的博文，所描述的算法都是基于过去以及当前时刻的传感器观测结果以估计当前时刻系统的状态，此为滤波算法。而在一些应用场景中，使用者对于系统状态的估计实时性要求较低，青睐于获取更准确的系统状态估计效果，此时可以利用一长段时间内获得的所有传感器观测来估计期间各个时刻的系统状态，此为平滑算法。
卡尔曼平滑算法是其中常用的一种，又称为RTS平滑——Rauch–Tung–Striebel smoother （RTSS, Rauch et al., 1965），本篇博文将详细介绍该算法。
本应用适用于服从以下条件概率分布的概率状态空间模型或称为滤波模型：对于 k = 1 , 2 , . . . k = 1,2,... k = 1 , 2 , . . .
x k ∼ p ( x k ∣ x k − 1 ) \mathbf{x}_k \sim p(\mathbf{x}_k|\mathbf{x}_{k-1}) x k ∼ p ( x k ∣ x k − 1 )
y k ∼ p ( y k ∣ x k ) \mathbf{y}_k \sim p(\mathbf{y}_k|\mathbf{x}_{k}) y k ∼ p ( y k ∣ x k )
这里
x k ∈ R n \mathbf{x}_k \in \mathbb{R}^n x k ∈ R n k k k y k ∈ R m \mathbf{y}_k \in \mathbb{R}^m y k ∈ R m k k k p ( x k ∣ x k − 1 ) p(\mathbf{x}_k|\mathbf{x}_{k-1}) p ( x k ∣ x k − 1 ) p ( y k ∣ x k ) p(\mathbf{y}_k|\mathbf{x}_{k}) p ( y k ∣ x k )
可见，该状态模型是一阶Markov链。
贝叶斯平滑的目的是在获得截止到时间 T T T k k k x k \mathbf{x}_k x k
p ( x k ∣ y 1 : T ) p(\mathbf{x}_k|\mathbf{y}_{1:T}) p ( x k ∣ y 1 : T )
其中 T > k T>k T > k x k \mathbf{x}_k x k k k k y 1 : T \mathbf{y}_{1:T} y 1 : T 1 ∼ T 1 \sim T 1 ∼ T
滤波和平滑的区别在于贝叶斯滤波利用的是过去和当前 k k k k k k 1 ∼ T 1 \sim T 1 ∼ T
推论1 （贝叶斯最优平滑公式）：用于计算任意 k k k k < T k<T k < T p ( x k ∣ y 1 : T ) p(\mathbf{x}_k|\mathbf{y}_{1:T}) p ( x k ∣ y 1 : T )
p ( x k + 1 ∣ y 1 : k ) = ∫ p ( x k + 1 ∣ x k ) p ( x k ∣ y 1 : k ) d x k 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 ( x k + 1 ∣ y 1 : k ) = ∫ p ( x k + 1 ∣ x k ) p ( x k ∣ y 1 : k ) d x k
p ( x k ∣ y 1 : T ) = p ( x k ∣ y 1 : k ) ∫ p ( x k + 1 ∣ x k ) p ( x k + 1 ∣ y 1 : T ) p ( x k + 1 ∣ y 1 : k ) d x k + 1 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 ( x k ∣ y 1 : T ) = p ( x k ∣ y 1 : k ) ∫ p ( x k + 1 ∣ y 1 : k ) p ( x k + 1 ∣ x k ) p ( x k + 1 ∣ y 1 : T ) d x k + 1
其中 p ( x k ∣ y 1 : k ) p(\mathbf{x}_{k}|\mathbf{y}_{1:k}) p ( x k ∣ y 1 : k ) k k k p ( x k + 1 ∣ y 1 : k ) p(\mathbf{x}_{k+1}|\mathbf{y}_{1:k}) p ( x k + 1 ∣ y 1 : k ) k + 1 k+1 k + 1
定义 （高斯分布） 若随机变量 x ∈ R n \mathbf{x} \in \mathbb{R}^n x ∈ R n
N ( x ∣ m , P ) = 1 ( 2 π ) n / 2 ∣ P ∣ 1 / 2 e x p ( 1 2 ( x − m ) T P − 1 ( x − m ) ) N(\mathbf{x}|\mathbf{m},\mathbf{P})=\frac{1}{(2\pi)^{n/2}\mathbf{\left | 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) N ( x ∣ m , P ) = ( 2 π ) n / 2 ∣ P ∣ 1 / 2 1 e x p ( 2 1 ( x − m ) T P − 1 ( x − m ) )
则称变量服从均值为 m ∈ R n \mathbf{m} \in \mathbb{R}^n m ∈ R n P ∈ R n × n \mathbf{P} \in \mathbb{R}^{n \times n} P ∈ R n × n ∣ P ∣ \left | \mathbf{P} \right | ∣ P ∣ P \mathbf{P} P
引理1 （高斯变量的联合分布）如果随机变量 x ∈ R n \mathbf{x} \in \mathbb{R}^n x ∈ R n y ∈ R m \mathbf{y} \in \mathbb{R}^m y ∈ R m
x ∼ N ( m , P ) \mathbf{x} \sim N(\mathbf{m},\mathbf{P}) x ∼ N ( m , P )
y ∣ x ∼ N ( H x + u , R ) \mathbf{y|x} \sim N(\mathbf{Hx+u},\mathbf{R}) y ∣ x ∼ N ( H x + u , R )
那么 x \mathbf{x} x y \mathbf{y} y y \mathbf{y} y
[ x y ] ∼ N ( [ m H m + u ] , [ P P H T H P H P H T + R ] ) \left [ \begin{matrix} \mathbf{x}\\ \mathbf{y} \end{matrix} \right ]
\sim N\left ( \left [ \begin{matrix} \mathbf{m}\\ \mathbf{Hm+u} \end{matrix} \right ] ,
\left [ \begin{matrix}
\mathbf{P} & \mathbf{PH}^T\\
\mathbf{HP} & \mathbf{HPH}^T+\mathbf{R}
\end{matrix} \right ]
\right ) [ x y ] ∼ N ( [ m H m + u ] , [ P H P P H T H P H T + R ] )
y ∼ N ( H m + u , H P H T + R ) \mathbf{y} \sim N(\mathbf{Hm+u}, \mathbf{HPH}^T+\mathbf{R}) y ∼ N ( H m + u , H P H T + R )
引理2 （高斯变量的条件分布）如果随机变量 x ∈ R n \mathbf{x} \in \mathbb{R}^n x ∈ R n y ∈ R m \mathbf{y} \in \mathbb{R}^m y ∈ R m
[ x y ] ∼ N ( [ a b ] , [ A C C T B ] ) \left [ \begin{matrix} \mathbf{x}\\ \mathbf{y} \end{matrix} \right ]
\sim N\left ( \left [ \begin{matrix} \mathbf{a}\\ \mathbf{b} \end{matrix} \right ] ,
\left [ \begin{matrix}
\mathbf{A} & \mathbf{C}\\
\mathbf{C}^T & \mathbf{B}
\end{matrix} \right ]
\right ) [ x y ] ∼ N ( [ a b ] , [ A C T C B ] )
那么 x \mathbf{x} x y \mathbf{y} y
x ∼ N ( a , A ) \mathbf{x} \sim N(\mathbf{a},\mathbf{A}) x ∼ N ( a , A )
y ∼ N ( b , B ) \mathbf{y} \sim N(\mathbf{b}, \mathbf{B}) y ∼ N ( b , B )
x ∣ y ∼ N ( a + C B − 1 ( y − b ) , A − C B − 1 C T ) \mathbf{x|y} \sim N(\mathbf{a+C} \mathbf{B}^{-1}(\mathbf{y-b}),\mathbf{A-C} \mathbf{B}^{-1}\mathbf{C}^T) x ∣ y ∼ N ( a + C B − 1 ( y − b ) , A − C B − 1 C T )
y ∣ x ∼ N ( b + C T A − 1 ( x − a ) , B − C T A − 1 C ) \mathbf{y|x} \sim N(\mathbf{b+C}^T \mathbf{A}^{-1}(\mathbf{x-a}),\mathbf{B-C}^T \mathbf{A}^{-1}\mathbf{C}) y ∣ x ∼ N ( b + C T A − 1 ( x − a ) , B − C T A − 1 C )
RTS平滑可以用于计算以下闭合解
p ( x k ∣ y 1 : T ) = N ( x k ∣ m k s , P k s ) p(\mathbf{x}_{k}|\mathbf{y}_{1:T}) =N(\mathbf{x}_{k}|\mathbf{m}_k^s,\mathbf{P}_k^s) p ( x k ∣ y 1 : T ) = N ( x k ∣ m k s , P k s )
即利用观测 y 1 : T \mathbf{y}_{1:T} y 1 : T k k k
m k + 1 − = A k m k \mathbf{m}_{k+1}^{-}=\mathbf{A}_k\mathbf{m}_k m k + 1 − = A k m k
P k − = A k P k A k T + Q k \mathbf{P}_{k}^{-}=\mathbf{A}_k\mathbf{P}_k\mathbf{A}_k^T+\mathbf{Q}_k P k − = A k P k A k T + Q k
S k − = ( H k P k − H k T + R k ) − 1 \mathbf{S}_{k}^{-}=(\mathbf{H}_k\mathbf{P}_k^{-}\mathbf{H}_k^T+\mathbf{R}_k)^{-1} S k − = ( H k P k − H k T + R k ) − 1
K k = P k − H k T S k − \mathbf{K}_{k}=\mathbf{P}_k^{-}\mathbf{H}_k^T\mathbf{S}_{k}^{-} K k = P k − H k T S k −
m k = m k − + K k ( z k − H k m k − ) \mathbf{m}_{k}=\mathbf{m}_{k}^{-}+\mathbf{K}_k(\mathbf{z}_{k}-\mathbf{H}_k\mathbf{m}_{k}^{-}) m k = m k − + K k ( z k − H k m k − )
P k = ( I − K k H k ) P k − \mathbf{P}_{k}=(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k})\mathbf{P}_{k}^{-} P k = ( I − K k H k ) P k −
后向递推（Backward Recursion）：
m k + 1 − = A k m k \mathbf{m}_{k+1}^-=\mathbf{A}_k\mathbf{m}_k m k + 1 − = A k m k
P k + 1 − = A k P k A k T + Q k \mathbf{P}_{k+1}^-=\mathbf{A}_k\mathbf{P}_k\mathbf{A}_k^T+\mathbf{Q}_k P k + 1 − = A k P k A k T + Q k
G k = P k A k T ( P k + 1 − ) − 1 \mathbf{G}_{k}=\mathbf{P}_k\mathbf{A}_k^T(\mathbf{P}_{k+1}^-)^{-1} G k = P k A k T ( P k + 1 − ) − 1
m k s = m k + G k ( m k + 1 s − m k + 1 − ) \mathbf{m}_{k}^s=\mathbf{m}_{k}+\mathbf{G}_k(\mathbf{m}_{k+1}^s-\mathbf{m}_{k+1}^-) m k s = m k + G k ( m k + 1 s − m k + 1 − )
P k s = P k + G k ( P k + 1 s − P k + 1 − ) G k T \mathbf{P}_{k}^s=\mathbf{P}_{k}+\mathbf{G}_k(\mathbf{P}_{k+1}^s-\mathbf{P}_{k+1}^-)\mathbf{G}_k^T P k s = P k + G k ( P k + 1 s − P k + 1 − ) G k T
从初始时刻1到 T T T T T T T T T T T T T T T m T \mathbf{m}_{T} m T P T \mathbf{P}_{T} P T m T s \mathbf{m}_{T}^s m T s P T s \mathbf{P}_{T}^s P T s m T = m T s \mathbf{m}_{T}=\mathbf{m}_{T}^s m T = m T s P T = P T s \mathbf{P}_{T}=\mathbf{P}_{T}^s P T = P T s
对于我们在目标跟踪上的应用，系统的状态转移是一阶Markov模型。根据一阶Markov的性质，当给定 x k + 1 \mathbf{x}_{k+1} x k + 1 x k \mathbf{x}_{k} x k y k + 1 : T \mathbf{y}_{k+1:T} y k + 1 : T p ( x k ∣ x k + 1 , y 1 : T ) = p ( x k ∣ x k + 1 , y 1 : k ) p(\mathbf{x}_k|\mathbf{x}_{k+1},\mathbf{y}_{1:T})=p(\mathbf{x}_k|\mathbf{x}_{k+1},\mathbf{y}_{1:k}) p ( x k ∣ x k + 1 , y 1 : T ) = p ( x k ∣ x k + 1 , y 1 : k )
p ( x k ∣ x k + 1 , y 1 : T ) = p ( x k ∣ x k + 1 , y 1 : k ) = p ( x k , x k + 1 ∣ y 1 : k ) p ( x k + 1 ∣ y 1 : k ) = p ( x k + 1 ∣ x k , y 1 : k ) p ( x k ∣ y 1 : k ) p ( x k + 1 ∣ y 1 : k ) = p ( x k + 1 ∣ x k ) p ( x k ∣ y 1 : k ) p ( x k + 1 ∣ y 1 : k ) 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})}
p ( x k ∣ x k + 1 , y 1 : T ) = p ( x k ∣ x k + 1 , y 1 : k ) = p ( x k + 1 ∣ y 1 : k ) p ( x k , x k + 1 ∣ y 1 : k ) = p ( x k + 1 ∣ y 1 : k ) p ( x k + 1 ∣ x k , y 1 : k ) p ( x k ∣ y 1 : k ) = p ( x k + 1 ∣ y 1 : k ) p ( x k + 1 ∣ x k ) p ( x k ∣ y 1 : k )
于是在给定观测 y 1 : T \mathbf{y}_{1:T} y 1 : T x k \mathbf{x}_{k} x k x k \mathbf{x}_{k} x k
p ( x k , x k + 1 ∣ y 1 : T ) = p ( x k ∣ x k + 1 , y 1 : T ) p ( x k + 1 ∣ y 1 : T ) = p ( x k ∣ x k + 1 , y 1 : k ) p ( x k + 1 ∣ y 1 : T ) = p ( x k + 1 ∣ x k ) p ( x k ∣ y 1 : k ) p ( x k + 1 ∣ y 1 : T ) p ( x k + 1 ∣ y 1 : k ) 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})} p ( x k , x k + 1 ∣ y 1 : T ) = p ( x k ∣ x k + 1 , y 1 : T ) p ( x k + 1 ∣ y 1 : T ) = p ( x k ∣ x k + 1 , y 1 : k ) p ( x k + 1 ∣ y 1 : T ) = p ( x k + 1 ∣ y 1 : k ) p ( x k + 1 ∣ x k ) p ( x k ∣ y 1 : k ) p ( x k + 1 ∣ y 1 : T )
其中 p ( x k + 1 ∣ y 1 : T ) p(\mathbf{x}_{k+1}|\mathbf{y}_{1:T}) p ( x k + 1 ∣ y 1 : T ) k + 1 k+1 k + 1 x k \mathbf{x}_k x k y 1 : T \mathbf{y}_{1:T} y 1 : T x k + 1 \mathbf{x}_{k+1} x k + 1 2.1节的贝叶斯平滑公式 。
利用2.3节引理1 ，给定观测 y 1 : k − 1 \mathbf{y}_{1:k-1} y 1 : k − 1 x k \mathbf{x}_k x k x k − 1 \mathbf{x}_{k-1} x k − 1
p ( x k , x k − 1 ∣ y 1 : k − 1 ) = p ( x k ∣ x k − 1 ) p ( x k − 1 ∣ y 1 : k − 1 ) = N ( x k ∣ A k − 1 x k − 1 , Q k − 1 ) N ( x k − 1 ∣ m k − 1 , P k − 1 ) = N ( [ x k − 1 x k ] ∣ m ′ , P ′ ) 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 ( \begin{bmatrix}
\mathbf{x}_{k-1}\\ \mathbf{x}_{k}
\end{bmatrix}| \mathbf{m}^{'},\mathbf{P}^{'} \right ) p ( x k , x k − 1 ∣ y 1 : k − 1 ) = p ( x k ∣ x k − 1 ) p ( x k − 1 ∣ y 1 : k − 1 ) = N ( x k ∣ A k − 1 x k − 1 , Q k − 1 ) N ( x k − 1 ∣ m k − 1 , P k − 1 ) = N ( [ x k − 1 x k ] ∣ m ′ , P ′ )
m ′ = [ m k − 1 A k − 1 m k − 1 ] \mathbf{m}^{'}=\left [ \begin{matrix} \mathbf{m}_{k-1}\\ \mathbf{A}_{k-1} \mathbf{m} _{k-1}\end{matrix} \right ] m ′ = [ m k − 1 A k − 1 m k − 1 ]
P ′ = [ P k − 1 P k − 1 A k − 1 T A k − 1 P k − 1 A k − 1 P k − 1 A k − 1 T + Q k − 1 ] \mathbf{P}^{'}=\left [ \begin{matrix}
\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{matrix} \right ] P ′ = [ P k − 1 A k − 1 P k − 1 P k − 1 A k − 1 T A k − 1 P k − 1 A k − 1 T + Q k − 1 ]
然后利用2.3节引理2 可以推出 x k \mathbf{x}_k x k
p ( x k ∣ y 1 : k − 1 ) = N ( x k ∣ m k − , P k − ) p(\mathbf{x}_{k}|\mathbf{y}_{1:k-1}) =N(\mathbf{x}_{k}|\mathbf{m}_{k}^-,\mathbf{P}_{k}^-) p ( x k ∣ y 1 : k − 1 ) = N ( x k ∣ m k − , P k − )
m k − = A k − 1 m k − 1 \mathbf{m}_{k}^-=\mathbf{A}_{k-1}\mathbf{m}_{k-1} m k − = A k − 1 m k − 1
P k − = A k − 1 P k − 1 A k − 1 T + Q k − 1 \mathbf{P}_{k}^{-}=\mathbf{A}_{k-1}\mathbf{P}_{k-1}\mathbf{A}_{k-1}^T+\mathbf{Q}_{k-1} P k − = A k − 1 P k − 1 A k − 1 T + Q k − 1
再次利用2.3节引理1 可以推出给定观测 y 1 : k − 1 \mathbf{y}_{1:k-1} y 1 : k − 1 x k \mathbf{x}_k x k y k \mathbf{y}_{k} y k
p ( x k , y k ∣ y 1 : k − 1 ) = p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 ) = N ( y k ∣ H k x k , R k ) N ( x k ∣ m k − , P k − ) = N ( [ x k y k ] ∣ m ′ ′ , P ′ ′ ) 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 ( \begin{bmatrix}
\mathbf{x}_{k}\\ \mathbf{y}_{k}
\end{bmatrix}| \mathbf{m}^{''},\mathbf{P}^{''} \right ) p ( x k , y k ∣ y 1 : k − 1 ) = p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 ) = N ( y k ∣ H k x k , R k ) N ( x k ∣ m k − , P k − ) = N ( [ x k y k ] ∣ m ′ ′ , P ′ ′ )
m ′ ′ = [ m k − H k m k − ] \mathbf{m}^{''}=\left [ \begin{matrix} \mathbf{m}_k^-\\ \mathbf{H}_k \mathbf{m} _k^-\end{matrix} \right ] m ′ ′ = [ m k − H k m k − ]
P ′ ′ = [ P k − P k − H k T H k P k − H k P k − H k T + R k ] \mathbf{P}^{''}=\left [ \begin{matrix}
\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{matrix} \right ] P ′ ′ = [ P k − H k P k − P k − H k T H k P k − H k T + R k ]
最后再利用2.3节引理2 可以推出给定观测 y 1 : k \mathbf{y}_{1:k} y 1 : k x k \mathbf{x}_k x k
p ( x k ∣ y k , y 1 : k − 1 ) = p ( x k ∣ y 1 : k ) = N ( x k ∣ m k , P 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) p ( x k ∣ y k , y 1 : k − 1 ) = p ( x k ∣ y 1 : k ) = N ( x k ∣ m k , P k )
m k = m k − + P k ′ H k T ( H k P k − H k T + R k ) − 1 ( z k − H k m k − ) \mathbf{m}_{k}=\mathbf{m}_{k}^{-}+\mathbf{P}_k^{'}\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}^{-}) m k = m k − + P k ′ H k T ( H k P k − H k T + R k ) − 1 ( z k − H k m k − )
P k = P k ′ − P k ′ H k T ( H k P k − H k T + R k ) − 1 H k P k ′ \mathbf{P}_{k}=\mathbf{P}_{k}^{'}-\mathbf{P}_k^{'}\mathbf{H}_k^T(\mathbf{H}_k\mathbf{P}_k^{-}\mathbf{H}_k^T+\mathbf{R}_k)^{-1}\mathbf{H}_{k}\mathbf{P}_{k}^{'} P k = P k ′ − P k ′ H k T ( H k P k − H k T + R k ) − 1 H k P k ′
类似于前向递推公式的推导过程，利用2.3节引理1 在给定观测 y 1 : k \mathbf{y}_{1:k} y 1 : k x k \mathbf{x}_k x k x k + 1 \mathbf{x}_{k+1} x k + 1 p ( x k , x k + 1 ∣ y 1 : k ) = p ( x k + 1 ∣ x k ) p ( x k ∣ y 1 : k ) = N ( x k + 1 ∣ A k x k , Q k ) N ( x k ∣ m k , P k ) = N ( [ x k x k + 1 ] ∣ m ~ 1 , P ~ 1 ) 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 ( \begin{bmatrix}
\mathbf{x}_{k}\\ \mathbf{x}_{k+1}
\end{bmatrix}| \widetilde{\mathbf{m}}_1,\widetilde{\mathbf{P}}_1 \right ) p ( x k , x k + 1 ∣ y 1 : k ) = p ( x k + 1 ∣ x k ) p ( x k ∣ y 1 : k ) = N ( x k + 1 ∣ A k x k , Q k ) N ( x k ∣ m k , P k ) = N ( [ x k x k + 1 ] ∣ m 1 , P 1 )
m ~ 1 = [ m k A k m k ] \widetilde{\mathbf{m}}_1=\left [ \begin{matrix} \mathbf{m}_k\\ \mathbf{A}_k \mathbf{m} _k\end{matrix} \right ] m 1 = [ m k A k m k ]
P ~ 1 = [ P k P k A k T A k P k A k P k A k T + Q k ] \widetilde{\mathbf{P}}_1=\left [ \begin{matrix}
\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{matrix} \right ] P 1 = [ P k A k P k P k A k T A k P k A k T + Q k ]
考虑到系统状态的Markov性质有 p ( x k ∣ x k + 1 , y 1 : T ) = p ( x k ∣ x k + 1 , y 1 : k ) p(\mathbf{x}_k|\mathbf{x}_{k+1},\mathbf{y}_{1:T})=p(\mathbf{x}_k|\mathbf{x}_{k+1},\mathbf{y}_{1:k}) p ( x k ∣ x k + 1 , y 1 : T ) = p ( x k ∣ x k + 1 , y 1 : k ) 2.3节引理2 有如下条件分布：
p ( x k ∣ x k + 1 , y 1 : T ) = p ( x k ∣ x k + 1 , y 1 : k ) = N ( x k ∣ m ~ 2 , P ~ 2 ) 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}| \widetilde{\mathbf{m}}_2 ,\widetilde{\mathbf{P}}_2) p ( x k ∣ x k + 1 , y 1 : T ) = p ( x k ∣ x k + 1 , y 1 : k ) = N ( x k ∣ m 2 , P 2 )
G k = P k A k T ( A k P k A k T + Q k ) − 1 \mathbf{G}_{k}=\mathbf{P}_k\mathbf{A}_k^T(\mathbf{A}_k\mathbf{P}_k\mathbf{A}_k^T+\mathbf{Q}_k)^{-1} G k = P k A k T ( A k P k A k T + Q k ) − 1
m ~ 2 = m k + G k [ m k + 1 − A k m k ] \widetilde{\mathbf{m}}_2=\mathbf{m}_{k}+\mathbf{G}_k[\mathbf{m}_{k+1}-\mathbf{A}_k\mathbf{m}_{k}] m 2 = m k + G k [ m k + 1 − A k m k ]
P ~ 2 = P k − G k ( A k P k A k T + Q k ) G k T \widetilde{\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 P 2 = P k − G k ( A k P k A k T + Q k ) G k T
因此在给定所有观测数据时，利用2.3节引理1 可以推出 x k \mathbf{x}_k x k x k + 1 \mathbf{x}_{k+1} x k + 1
p ( x k , x k + 1 ∣ y 1 : T ) = p ( x k ∣ x k + 1 , y 1 : T ) p ( x k + 1 ∣ y 1 : T ) = N ( x k ∣ m ~ 2 , P ~ 2 ) N ( x k + 1 ∣ m k + 1 s , P k + 1 s ) = N ( [ x k x k + 1 ] ∣ m ~ 3 , P ~ 3 ) 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}|\widetilde{\mathbf{m}}_2,\widetilde{\mathbf{P}}_2)N(\mathbf{x}_{k+1}|\mathbf{m}_{k+1} ^s,\mathbf{P}_{k+1}^s) \\ = N(\begin{bmatrix}
\mathbf{x}_{k}\\ \mathbf{x}_{k+1}
\end{bmatrix}|\widetilde{\mathbf{m}}_3,\widetilde{\mathbf{P}}_3) p ( x k , x k + 1 ∣ y 1 : T ) = p ( x k ∣ x k + 1 , y 1 : T ) p ( x k + 1 ∣ y 1 : T ) = N ( x k ∣ m 2 , P 2 ) N ( x k + 1 ∣ m k + 1 s , P k + 1 s ) = N ( [ x k x k + 1 ] ∣ m 3 , P 3 )
m ~ 3 = [ m k + 1 s m k + G k ( m k + 1 s − A k m k ) ] \widetilde{\mathbf{m}}_3=\left [ \begin{matrix} \mathbf{m}_{k+1}^s\\ \mathbf{m}_{k} +\mathbf{G}_k( \mathbf{m}_{k+1}^s-\mathbf{A}_k\mathbf{m}_k) \end{matrix} \right ] m 3 = [ m k + 1 s m k + G k ( m k + 1 s − A k m k ) ]
P ~ 3 = [ P k + 1 s P k + 1 s G k T G k P k + 1 s G k P k + 1 s G k T + P ~ 2 ] \widetilde{\mathbf{P}}_3=\left [ \begin{matrix}
\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+\widetilde{\mathbf{P}}_2
\end{matrix} \right ] P 3 = [ P k + 1 s G k P k + 1 s P k + 1 s G k T G k P k + 1 s G k T + P 2 ]
因此，利用2.3节引理2 可以得到 x k \mathbf{x}_k x k
p ( x k ∣ y 1 : T ) = N ( x k ∣ m k s , P k s ) p(\mathbf{x}_{k}|\mathbf{y}_{1:T}) =N(\mathbf{x}_{k}|\mathbf{m}_k^s,\mathbf{P}_k^s) p ( x k ∣ y 1 : T ) = N ( x k ∣ m k s , P k s )
m k s = m k + G k ( m k + 1 s − A k m k ) \mathbf{m}_{k}^s=\mathbf{m}_{k}+\mathbf{G}_k(\mathbf{m}_{k+1}^s-\mathbf{A}_k\mathbf{m}_k) m k s = m k + G k ( m k + 1 s − A k m k )
P k s = P k + G k ( P k + 1 s − A k P k A k T − Q k ) G k T \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 P k s = P k + G k ( P k + 1 s − A k P k A k T − Q k ) G k T
证毕。
以一正弦信号的高斯随机走动为例，信号实际波形如下图所示：
首先建立系统模型如下，状态转移方程和状态观测方程分别为：
x k = x k − 1 + q k , q k ∼ N ( 0 , Q ) x_k=x_{k-1}+q_k,q_k \sim N(0,Q) x k = x k − 1 + q k , q k ∼ N ( 0 , Q )
y k = x k + r k , r k ∼ N ( 0 , R ) y_k=x_{k}+r_k,r_k \sim N(0,R) y k = x k + r k , r k ∼ N ( 0 , R )
根据此模型，有 A = 1 A=1 A = 1 H = 1 H = 1 H = 1 Q = 0.05 Q=0.05 Q = 0 . 0 5 R = 1 R=1 R = 1
卡尔曼平滑算法其实就是卡尔曼滤波算法的加强版，包括了前向递推和后向递推两个步骤，其中前向递推的过程与卡尔曼滤波算法时一致的，而后向递推可以进一步减少估计结果的波动。算法的推导过程主要依赖于引理1和引理2的反复运用，利用贝叶斯概率理论实现系统状态的后验估计，与博文卡尔曼滤波系列一——标准卡尔曼滤波 的推导过程有些不一样，不过结果是统一的。
根据实验结果可见，对于相同的观测数据，参数相同的两个算法的处理结果存在一定差异，其中卡尔曼平滑算法虽然实时性不如卡尔曼滤波算法，但其估计得到的信号波形更为平滑。而且仔细看可以看出，卡尔曼滤波算法相对于信号真值存在微小的相位滞后，而卡尔曼平滑算法的估算结果则保持较好的相位一致性，这在实际应用中有重要价值。通过计算两个算法处理结果与信号真值的均方误差，例如上述实验中，卡尔曼滤波算法的MSE为0.0078，而卡尔曼平滑算法的MSE伪0.0025，可见，平滑算法的估计精度更高，效果更好。
在实际应用中，如果信号长度较大，可以将信号按照时间先后分成若干个重叠的时段，然后对每一段分别使用平滑算法，这样可以得到更好地效果，同时也在一定程度上减少传感器的测量时间，降低算法处理时间，提高实时性。
[1] Simo Srkk. Bayesian Filtering and Smoothing.
原创性声明：本文属于作者原创性文章，小弟码字辛苦，转载还请注明出处。谢谢~
代码下载请到本博文实验程序 。
如果有哪些地方表述的不够得体和清晰，有存在的任何问题，亦或者程序存在任何考虑不周和漏洞，欢迎评论和指正，谢谢各路大佬。
有需要相关技术支持的可咨询QQ：297461921
