對於此前的若干篇與卡爾曼濾波有關的博文，所描述的算法都是基於過去以及當前時刻的傳感器觀測結果以估計當前時刻系統的狀態，此爲濾波算法。而在一些應用場景中，使用者對於系統狀態的估計實時性要求較低，青睞於獲取更準確的系統狀態估計效果，此時可以利用一長段時間內獲得的所有傳感器觀測來估計期間各個時刻的系統狀態，此爲平滑算法。
卡爾曼平滑算法是其中常用的一種，又稱爲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
