上一篇文章介绍了离散时间的批量估计，本文着重介绍离散时间的递归平滑。
上一篇位置：【状态估计】线性高斯系统的状态估计——离散时间的批量估计。
离散时间的递归平滑算法
批量优化方法给出了一个简洁漂亮的结论。它容易创建，也容易从最小二乘的角度来理解。然而，大多数时候暴力求解线性方程是非常低效的。事实上，等式左边的逆协方差矩阵有稀疏结构，可以利用这一点加速方程的求解：一次向前递推，一次向后递推。这种做法被称为典型的固定区间平滑算法。
利用批量优化结论中的稀疏结构
在上一篇中，最终的优化问题转化为了：
( H T W − 1 H ) x ^ = H T W − 1 z (H^TW^{-1}H)\hat x = H^TW^{-1}z (HTW−1H)x^=HTW−1z
其中， H T W − 1 H H^TW^{-1}H HTW−1H是一个稀疏结构：三对角块，即：
H T W − 1 H = [ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ⋱ ⋱ ⋱ ∗ ∗ ∗ ∗ ∗ ] H^TW^{-1}H=\begin{bmatrix}*&*\\*&*&*\\&*&*&*\\&&\ddots&\ddots&\ddots\\&&&*&*&*\\&&&&*&*\end{bmatrix} HTW−1H= ∗∗∗∗∗∗∗⋱∗⋱∗⋱∗∗∗∗
其中， ∗ * ∗表示非零块。
对于这种稀疏结构的线性方程，可以采用Cholesky分解，需要一次前向和后向迭代。根据Cholesky分解，可以高效地将 H T W − 1 H H^TW^{-1}H HTW−1H分解为：
H T W − 1 H = L L T H^TW^{-1}H=LL^T HTW−1H=LLT
其中， L L L称为Cholesky因子，是一个下三角矩阵。由于 H T W − 1 H H^TW^{-1}H HTW−1H是三对角块， L L L的形式如下：
L = [ ∗ ∗ ∗ ∗ ∗ ⋱ ⋱ ∗ ∗ ∗ ∗ ] L=\begin{bmatrix}*\\*&*\\&*&*\\&&\ddots&\ddots\\&&&*&*\\&&&&*&*\end{bmatrix} L= ∗∗∗∗∗⋱⋱∗∗∗∗
这个分解的复杂度是 O ( N ( K + 1 ) ) O(N(K+1)) O(N(K+1))。然后，整个优化问题变成了：
( L L T ) x ^ = H T W − 1 z (LL^T)\hat x = H^TW^{-1}z (LLT)x^=HTW−1z
首先，求解 d d d：
L d = H T W − 1 z Ld=H^TW^{-1}z Ld=HTW−1z
这也是一个 O ( N ( K + 1 ) ) O(N(K+1)) O(N(K+1))的操作，因为可以从上往下求解 d d d的每一项，再把结果代入下一行中。这个操作称为前向迭代。
然后，求解 x ^ \hat x x^：
L T x ^ = d L^T\hat x=d LTx^=d
同理，由于 L T L^T LT的特殊结构，可以从最后一行开始，逐步求解 x ^ \hat x x^的每一行元素，再代入上一行即可。这个操作称为后向迭代。
Cholesky平滑算法
上面简单的描述了Cholesky分解计算 x ^ \hat x x^的思路，这里详细的描述整个计算过程。
首先，将 H T W − 1 H H^TW^{-1}H HTW−1H进行Cholesky分解，计算 L L L：
定义 L L L的非零块：
L = [ L 0 L 10 L 1 L 21 L 2 ⋱ ⋱ L K − 1 L K − 2 L K − 1 L K − 1 L K L K ] L=\begin{bmatrix}L_0\\L_{10}&L_1\\&L_{21}&L_2\\&&\ddots&\ddots\\&&&L_{K-1}L_{K-2}&L_{K-1}\\&&&&L_{K-1}L_K&L_K\end{bmatrix} L= L0L10L1L21L2⋱⋱LK−1LK−2LK−1LK−1LKLK
利用 H T W − 1 H = L L T H^TW^{-1}H=LL^T HTW−1H=LLT，并展开 H H H、 W W W的定义，对比每块的元素：
L 0 L 0 T = P ˇ 0 − 1 + C 0 T R 0 − 1 C 0 + A 0 T Q 1 − 1 A 0 L 10 L 0 T = − Q 1 − 1 A 0 L 1 L 1 T = − L 10 L 10 T + Q 1 − 1 + C 1 T R 1 − 1 C 1 + A 1 T Q 2 − 1 A 1 L 21 L 1 T = − Q 2 − 1 A 1 . . . = . . . L K − 1 L K − 1 T = − L K − 1 , K − 2 L K − 1 , K − 2 T + Q K − 1 − 1 + C K − 1 T R K − 1 − 1 C K − 1 + A K − 1 T Q K − 1 A K − 1 L K , K − 1 L K − 1 T = − Q K − 1 A K − 1 L K L K T = − L K , K − 1 L K , K − 1 T + Q K − 1 + C K T R K − 1 C K \begin{aligned}L_0L_0^T&=\check P_0^{-1}+C_0^TR_0^{-1}C_0+A_0^TQ_1^{-1}A_0 \\ L_{10}L_0^T&=-Q_1^{-1}A_0 \\ L_1L_1^T&=-L_{10}L_{10}^T+Q_1^{-1}+C_1^TR_1^{-1}C_1+A_1^TQ_2^{-1}A_1 \\ L_{21}L_1^T&=-Q_2^{-1}A_1 \\ ... &= ... \\L_{K-1}L_{K-1}^T&=-L_{K-1,K-2}L_{K-1,K-2}^T+Q_{K-1}^{-1}+C_{K-1}^TR_{K-1}^{-1}C_{K-1}+A_{K-1}^TQ_K^{-1}A_{K-1} \\ L_{K,K-1}L_{K-1}^T&=-Q_{K}^{-1}A_{K-1} \\ L_{K}L_{K}^T&=-L_{K,K-1}L_{K,K-1}^T+Q_{K}^{-1}+C_{K}^TR_{K}^{-1}C_{K}\end{aligned} L0L0TL10L0TL1L1TL21L1T...LK−1LK−1TLK,K−1LK−1TLKLKT=Pˇ0−1+C0TR0−1C0+A0TQ1−1A0=−Q1−1A0=−L10L10T+Q1−1+C1TR1−1C1+A1TQ2−1A1=−Q2−1A1=...=−LK−1,K−2LK−1,K−2T+QK−1−1+CK−1TRK−1−1CK−1+AK−1TQK−1AK−1=−QK−1AK−1=−LK,K−1LK,K−1T+QK−1+CKTRK−1CK
可以提取一些中间量：
I k = − L k , k − 1 L k , k − 1 T + Q k − 1 + C k T R k − 1 C k I_k=-L_{k,k-1}L_{k,k-1}^T+Q_{k}^{-1}+C_{k}^TR_{k}^{-1}C_{k} Ik=−Lk,k−1Lk,k−1T+Qk−1+CkTRk−1Ck
同时定义：
I 0 = P ˇ 0 − 1 + C 0 T R 0 − 1 C 0 I_0=\check P_0^{-1}+C_0^TR_0^{-1}C_0 I0=Pˇ0−1+C0TR0−1C0
从这些式子中可以看出， L 0 L 0 T L_0L_0^T L0L0T等式可以先计算出 L 0 L_0 L0，再代入 L 10 L 0 T L_{10}L_0^T L10L0T等式可以计算出 L 10 L_{10} L10，再代入 L 1 L 1 T L_1L_1^T L1L1T等式可以计算出 L 1 L_1 L1，依此类推，直至计算出 L K L_K LK。因此，可以从上往下（前向地），在 O ( N ( K + 1 ) ) O(N(K+1)) O(N(K+1))的时间内计算出整个 L L L。
接下来，计算 L d = H T W − 1 z Ld=H^TW^{-1}z Ld=HTW−1z，把 d d d写成：
d = [ d 0 d 1 ⋮ d k ] d=\begin{bmatrix}d_0\\d_1\\\vdots\\d_k\end{bmatrix} d= d0d1⋮dk
展开矩阵乘法并比较每个块，得：
L 0 d 0 = P ˇ 0 − 1 x ˇ 0 + C 0 T R 0 − 1 y 0 − A 0 T Q 1 − 1 v 1 L 1 d 1 = − L 10 d 0 + Q 1 − 1 v 1 + C 1 T R 1 − 1 y 1 − A 1 T Q 2 − 1 v 2 . . . = . . . L K 1 d K − 1 = − L K − 1 , K − 2 d K − 2 + Q K − 1 − 1 v K − 1 + C K − 1 T R K − 1 − 1 y K − 1 − A K − 1 T Q K − 1 v K L K d K = − L K , K − 1 d K − 1 + Q K − 1 v K + C K T R K − 1 y K \begin{aligned} L_0d_0&=\check P_0^{-1}\check x_0+C_0^TR_0^{-1}y_0-A_0^TQ_1^{-1}v_1 \\ L_1d_1&=-L_{10}d_0+Q_1^{-1}v_1+C_1^TR_1^{-1}y_1-A_1^TQ_2^{-1}v_2 \\ ...&= ... \\ L_{K_1}d_{K-1}&=-L_{K-1,K-2}d_{K-2}+Q_{K-1}^{-1}v_{K-1}+C_{K-1}^TR_{K-1}^{-1}y_{K-1}-A_{K-1}^TQ_K^{-1}v_K \\ L_{K}d_{K}&=-L_{K,K-1}d_{K-1}+Q_{K}^{-1}v_{K}+C_{K}^TR_{K}^{-1}y_{K} \end{aligned} L0d0L1d1...LK1dK−1LKdK=Pˇ0−1xˇ0+C0TR0−1y0−A0TQ1−1v1=−L10d0+Q1−1v1+C1TR1−1y1−A1TQ2−1v2=...=−LK−1,K−2dK−2+QK−1−1vK−1+CK−1TRK−1−1yK−1−AK−1TQK−1vK=−LK,K−1dK−1+QK−1vK+CKTRK−1yK
可以提取一些中间量：
q k = − L k , k − 1 d k − 1 + Q k − 1 v k + C k T R k − 1 y k q_k=-L_{k,k-1}d_{k-1}+Q_{k}^{-1}v_{k}+C_{k}^TR_{k}^{-1}y_{k} qk=−Lk,k−1dk−1+Qk−1vk+CkTRk−1yk
同时定义：
q 0 = P ˇ 0 − 1 x ˇ 0 + C 0 T R 0 − 1 y 0 q_0=\check P_0^{-1}\check x_0+C_0^TR_0^{-1}y_0 q0=Pˇ0−1xˇ0+C0TR0−1y0
从这些式子中可以看出， L 0 d 0 L_0d_0 L0d0等式可以先计算出 d 0 d_0 d0，再代入 L 1 d 1 L_1d_1 L1d1等式可以计算出 d 1 d_1 d1，依此类推，直至计算出 d K d_K dK。因此，可以从上往下（前向地），在 O ( N ( K + 1 ) ) O(N(K+1)) O(N(K+1))的时间内计算出整个 d d d。
最后一步，计算 L T x ^ = d L^T\hat x=d LTx^=d，设：
x ^ = [ x ^ 0 x ^ 1 ⋮ x ^ k ] \hat x=\begin{bmatrix}\hat x_0\\\hat x_1\\\vdots\\\hat x_k\end{bmatrix} x^= x^0x^1⋮x^k
展开矩阵运算，有：
L 0 T x ^ 0 = − L 10 T x ^ 1 + d 0 L 1 T x ^ 1 = − L 21 T x ^ 2 + d 1 . . . = . . . L K − 1 T x ^ K − 1 = − L K , K − 1 T x ^ K + d K − 1 L K T x ^ K = d K \begin{aligned} L_0^T\hat x_0&=-L_{10}^T\hat x_1+d_0 \\ L_1^T\hat x_1&=-L_{21}^T\hat x_2+d_1 \\ ...&= ... \\ L_{K-1}^T\hat x_{K-1}&=-L_{K,K-1}^T\hat x_K+d_{K-1} \\ L_K^T\hat x_K&=d_K \end{aligned} L0Tx^0L1Tx^1...LK−1Tx^K−1LKTx^K=−L10Tx^1+d0=−L21Tx^2+d1=...=−LK,K−1Tx^K+dK−1=dK
从这些式子中可以看出， L K T x ^ K L_K^T\hat x_K LKTx^K等式可以先计算出 x ^ K \hat x_K x^K，再代入 L K − 1 T x ^ K − 1 L_{K-1}^T\hat x_{K-1} LK−1Tx^K−1等式可以计算出 x ^ K − 1 \hat x_{K-1} x^K−1，依此类推，直至计算出 x ^ 0 \hat x_0 x^0。因此，可以从下往上（后向地），在 O ( N ( K + 1 ) ) O(N(K+1)) O(N(K+1))的时间内计算出整个 x ^ \hat x x^。
梳理一下整个过程：
前向： k = 0 , 1 , . . . , K k=0,1,...,K k=0,1,...,K
L k − 1 L k − 1 T = I k − 1 + A k − 1 T Q k − 1 A k − 1 L k − 1 d k − 1 = q k − 1 − A k − 1 T Q k − 1 v k L k , k − 1 L k − 1 T = − Q k − 1 A k − 1 \begin{aligned}L_{k-1}L_{k-1}^T&=I_{k-1}+A_{k-1}^TQ_k^{-1}A_{k-1} \\ L_{k-1}d_{k-1}&=q_{k-1}-A_{k-1}^TQ_k^{-1}v_k\\ L_{k,k-1}L_{k-1}^T&=-Q_k^{-1}A_{k-1} \end{aligned} Lk−1Lk−1TLk−1dk−1Lk,k−1Lk−1T=Ik−1+Ak−1TQk−1Ak−1=qk−1−Ak−1TQk−1vk=−Qk−1Ak−1
其中：
I k = − L k , k − 1 L k , k − 1 T + Q k − 1 + C k T R k − 1 C k q k = − L k , k − 1 d k − 1 + Q k − 1 v k + C k T R k − 1 y k \begin{aligned}I_k&=-L_{k,k-1}L_{k,k-1}^T+Q_{k}^{-1}+C_{k}^TR_{k}^{-1}C_{k} \\ q_k&=-L_{k,k-1}d_{k-1}+Q_{k}^{-1}v_{k}+C_{k}^TR_{k}^{-1}y_{k}\end{aligned} Ikqk=−Lk,k−1Lk,k−1T+Qk−1+CkTRk−1Ck=−Lk,k−1dk−1+Qk−1vk+CkTRk−1yk
后向： k = K , K − 1 , . . . , 0 k=K,K-1,...,0 k=K,K−1,...,0
L k − 1 T x ^ k − 1 = − L k , k − 1 T x ^ k + d k − 1 L_{k-1}^T\hat x_{k-1}=-L_{k,k-1}^T\hat x_k+d_{k-1} Lk−1Tx^k−1=−Lk,k−1Tx^k+dk−1
这些值的初值：
I 0 = P ˇ 0 − 1 + C 0 T R 0 − 1 C 0 q 0 = P ˇ 0 − 1 x ˇ 0 + C 0 T R 0 − 1 y 0 x ^ K = L K − T d K \begin{aligned}I_0&=\check P_0^{-1}+C_0^TR_0^{-1}C_0 \\ q_0&=\check P_0^{-1}\check x_0+C_0^TR_0^{-1}y_0 \\ \hat x_K&=L_K^{-T}d_K\end{aligned} I0q0x^K=Pˇ0−1+C0TR0−1C0=Pˇ0−1xˇ0+C0TR0−1y0=LK−TdK
前向迭代将 { q k − 1 , I k − 1 } \{q_{k-1},I_{k-1}\} {qk−1,Ik−1}映射到了下个时刻的 { q k , I k } \{q_k,I_k\} {qk,Ik}，后向迭代将 x ^ k \hat x_k x^k映射到了上个时刻的 x ^ k − 1 \hat x_{k-1} x^k−1。
这六个递归的方程，在代数上等价于传统的RTS平滑算法；而前五个前向迭代，则等价于著名的卡尔曼滤波器。
RTS平滑算法
Cholesky推导出来的方程，代数上等价于经典的RTS平滑算法，但不是其标准方程形式。下面来进行一些转换：
从前向迭代开始， L k , k − 1 L k − 1 T L_{k,k-1}L_{k-1}^T Lk,k−1Lk−1T等式解出 L k , k − 1 L_{k,k-1} Lk,k−1，代入 I k I_k Ik，并对 L k − 1 L k − 1 T L_{k-1}L_{k-1}^T Lk−1Lk−1T进行替换：
I k = Q k − 1 − Q k − 1 A k − 1 ( I k − 1 + A k − 1 T Q k − 1 A k − 1 ) − 1 A k − 1 T Q k − 1 + C k T R k − 1 C k I_k=Q_{k}^{-1}-Q_k^{-1}A_{k-1}(I_{k-1}+A_{k-1}^TQ_k^{-1}A_{k-1})^{-1}A_{k-1}^TQ_k^{-1}+C_{k}^TR_{k}^{-1}C_{k} Ik=Qk−1−Qk−1Ak−1(Ik−1+Ak−1TQk−1Ak−1)−1Ak−1TQk−1+CkTRk−1Ck
根据SMW恒等式：
Q k − 1 − Q k − 1 A k − 1 ( I k − 1 + A k − 1 T Q k − 1 A k − 1 ) − 1 A k − 1 T Q k − 1 = ( A k − 1 I k − 1 − 1 A k − 1 T + Q k ) − 1 Q_{k}^{-1}-Q_k^{-1}A_{k-1}(I_{k-1}+A_{k-1}^TQ_k^{-1}A_{k-1})^{-1}A_{k-1}^TQ_k^{-1}=(A_{k-1}I_{k-1}^{-1}A_{k-1}^T+Q_k)^{-1} Qk−1−Qk−1Ak−1(Ik−1+Ak−1TQk−1Ak−1)−1Ak−1TQk−1=(Ak−1Ik−1−1Ak−1T+Qk)−1
定义 P ^ k , f = I k − 1 \hat P_{k,f}=I_k^{-1} P^k,f=Ik−1，这里的 f f f表示该式来自于前向迭代，那么上式可以写成两步：
P ˇ k , f = A k − 1 P ^ k − 1 , f A k − 1 T + Q k \check P_{k,f}=A_{k-1}\hat P_{k-1,f}A_{k-1}^T+Q_k Pˇk,f=Ak−1P^k−1,fAk−1T+Qk
P ^ k , f − 1 = P ˇ k , f − 1 + C k T R k − 1 C k \hat P_{k,f}^{-1}=\check P_{k,f}^{-1}+C_k^TR_k^{-1}C_k P^k,f−1=Pˇk,f−1+CkTRk−1Ck
第一个方程就是经典形式，而第二个方程是信息矩阵（协方差的逆）来写的。为了将其写成经典形式，定义增益 K k K_k Kk：
K k = P ^ k , f C k T R k − 1 K_k=\hat P_{k,f}C_k^TR_k^{-1} Kk=P^k,fCkTRk−1
将 P ^ k , f − 1 \hat P_{k,f}^{-1} P^k,f−1方程计算出 P ^ k , f \hat P_{k,f} P^k,f代入：
K k = ( P ˇ k , f − 1 + C k T R k − 1 C k ) − 1 C k T R k − 1 K_k=(\check P_{k,f}^{-1}+C_k^TR_k^{-1}C_k)^{-1}C_k^TR_k^{-1} Kk=(Pˇk,f−1+CkTRk−1Ck)−1CkTRk−1
根据SMW恒等式：
( P ˇ k , f − 1 + C k T R k − 1 C k ) − 1 C k T R k − 1 = P ˇ k , f C K T ( P ˇ k , f − 1 + C k T R k − 1 C k ) − 1 (\check P_{k,f}^{-1}+C_k^TR_k^{-1}C_k)^{-1}C_k^TR_k^{-1}=\check P_{k,f}C_K^T(\check P_{k,f}^{-1}+C_k^TR_k^{-1}C_k)^{-1} (Pˇk,f−1+CkTRk−1Ck)−1CkTRk−1=Pˇk,fCKT(Pˇk,f−1+CkTRk−1Ck)−1
因此：
K k = P ˇ k , f C K T ( P ˇ k , f − 1 + C k T R k − 1 C k ) − 1 K_k=\check P_{k,f}C_K^T(\check P_{k,f}^{-1}+C_k^TR_k^{-1}C_k)^{-1} Kk=Pˇk,fCKT(Pˇk,f−1+CkTRk−1Ck)−1
将 P ^ k , f − 1 \hat P_{k,f}^{-1} P^k,f−1方程计算出 P ˇ k , f − 1 \check P_{k,f}^{-1} Pˇk,f−1：
P ˇ k , f − 1 = P ^ k , f − 1 − C k T R k − 1 C k = P ^ k , f − 1 ( 1 − P ^ k , f C k T R k − 1 C k ) \check P_{k,f}^{-1}=\hat P_{k,f}^{-1}-C_k^TR_k^{-1}C_k=\hat P_{k,f}^{-1}(1-\hat P_{k,f}C_k^TR_k^{-1}C_k) Pˇk,f−1=P^k,f−1−CkTRk−1Ck=P^k,f−1(1−P^k,fCkTRk−1Ck)
由于 K k = P ^ k , f C k T R k − 1 K_k=\hat P_{k,f}C_k^TR_k^{-1} Kk=P^k,fCkTRk−1，直接代入：
P ˇ k , f − 1 = P ^ k , f − 1 ( 1 − K k C k ) \check P_{k,f}^{-1}=\hat P_{k,f}^{-1}(1-K_kC_k) Pˇk,f−1=P^k,f−1(1−KkCk)
因此：
P ^ k , f = ( 1 − K k C k ) P ˇ k , f \hat P_{k,f}=(1-K_kC_k)\check P_{k,f} P^k,f=(1−KkCk)Pˇk,f
这就是经典形式了。
然后将 L k − 1 d k − 1 L_{k-1}d_{k-1} Lk−1dk−1方程求解出 d k − 1 d_{k-1} dk−1， L k , k − 1 L k − 1 T L_{k,k-1}L_{k-1}^T Lk,k−1Lk−1T方程求解出 L k , k − 1 L_{k,k-1} Lk,k−1：
L k , k − 1 d k − 1 = − Q k − 1 A k − 1 ( L k − 1 L k − 1 T ) − 1 ( q k − 1 − A k − 1 T Q k − 1 v k ) L_{k,k-1}d_{k-1}=-Q_k^{-1}A_{k-1}(L_{k-1}L_{k-1}^T)^{-1}(q_{k-1}-A_{k-1}^TQ_k^{-1}v_k) Lk,k−1dk−1=−Qk−1Ak−1(Lk−1Lk−1T)−1(qk−1−Ak−1TQk−1vk)
代入 L k − 1 L k − 1 T L_{k-1}L_{k-1}^T Lk−1Lk−1T方程：
L k , k − 1 d k − 1 = − Q k − 1 A k − 1 ( I k − 1 + A k − 1 T Q k − 1 A k − 1 ) − 1 ( q k − 1 − A k − 1 T Q k − 1 v k ) L_{k,k-1}d_{k-1}=-Q_k^{-1}A_{k-1}(I_{k-1}+A_{k-1}^TQ_k^{-1}A_{k-1})^{-1}(q_{k-1}-A_{k-1}^TQ_k^{-1}v_k) Lk,k−1dk−1=−Qk−1Ak−1(Ik−1+Ak−1TQk−1Ak−1)−1(qk−1−Ak−1TQk−1vk)
将其代入 q k q_k qk方程：
q k = Q k − 1 A k − 1 ( I k − 1 + A k − 1 T Q k − 1 A k − 1 ) − 1 ( q k − 1 − A k − 1 T Q k − 1 v k ) + Q k − 1 v k + C k T R k − 1 y k = Q k − 1 A k − 1 ( I k − 1 + A k − 1 T Q k − 1 A k − 1 ) − 1 q k − 1 + ( Q k − 1 − Q k − 1 A k − 1 ( I k − 1 + A k − 1 T Q k − 1 A k − 1 ) − 1 A k − 1 T Q k − 1 ) v k + C k T R k − 1 y k \begin{aligned}q_k&=Q_k^{-1}A_{k-1}(I_{k-1}+A_{k-1}^TQ_k^{-1}A_{k-1})^{-1}(q_{k-1}-A_{k-1}^TQ_k^{-1}v_k)+Q_{k}^{-1}v_{k}+C_{k}^TR_{k}^{-1}y_{k} \\ &=Q_k^{-1}A_{k-1}(I_{k-1}+A_{k-1}^TQ_k^{-1}A_{k-1})^{-1}q_{k-1}+(Q_{k}^{-1}-Q_k^{-1}A_{k-1}(I_{k-1}+A_{k-1}^TQ_k^{-1}A_{k-1})^{-1}A_{k-1}^TQ_k^{-1})v_k+C_{k}^TR_{k}^{-1}y_{k} \end{aligned} qk=Qk−1Ak−1(Ik−1+Ak−1TQk−1Ak−1)−1(qk−1−Ak−1TQk−1vk)+Qk−1vk+CkTRk−1yk=Qk−1Ak−1(Ik−1+Ak−1TQk−1Ak−1)−1qk−1+(Qk−1−Qk−1Ak−1(Ik−1+Ak−1TQk−1Ak−1)−1Ak−1TQk−1)vk+CkTRk−1yk
根据SMW恒等式：
Q k − 1 A k − 1 ( I k − 1 + A k − 1 T Q k − 1 A k − 1 ) − 1 = ( A k − 1 I k − 1 − 1 A k − 1 T + Q k ) − 1 A k − 1 I k − 1 − 1 Q k − 1 − Q k − 1 A k − 1 ( I k − 1 + A k − 1 T Q k − 1 A k − 1 ) − 1 A k − 1 T Q k − 1 = ( A k − 1 I k − 1 − 1 A k − 1 T + Q k ) − 1 \begin{aligned}Q_k^{-1}A_{k-1}(I_{k-1}+A_{k-1}^TQ_k^{-1}A_{k-1})^{-1}&=(A_{k-1}I_{k-1}^{-1}A_{k-1}^T+Q_k)^{-1}A_{k-1}I_{k-1}^{-1}\\Q_{k}^{-1}-Q_k^{-1}A_{k-1}(I_{k-1}+A_{k-1}^TQ_k^{-1}A_{k-1})^{-1}A_{k-1}^TQ_k^{-1}&=(A_{k-1}I_{k-1}^{-1}A_{k-1}^T+Q_k)^{-1}\end{aligned} Qk−1Ak−1(Ik−1+Ak−1TQk−1Ak−1)−1Qk−1−Qk−1Ak−1(Ik−1+Ak−1TQk−1Ak−1)−1Ak−1TQk−1=(Ak−1Ik−1−1Ak−1T+Qk)−1Ak−1Ik−1−1=(Ak−1Ik−1−1Ak−1T+Qk)−1
因此：
q k = ( A k − 1 I k − 1 − 1 A k − 1 T + Q k ) − 1 A k − 1 I k − 1 − 1 q k − 1 + ( A k − 1 I k − 1 − 1 A k − 1 T + Q k ) − 1 v k + C k T R k − 1 y k = ( A k − 1 I k − 1 − 1 A k − 1 T + Q k ) − 1 ( A k − 1 I k − 1 − 1 q k − 1 + v k ) + C k T R k − 1 y k \begin{aligned}q_k&=(A_{k-1}I_{k-1}^{-1}A_{k-1}^T+Q_k)^{-1}A_{k-1}I_{k-1}^{-1}q_{k-1}+(A_{k-1}I_{k-1}^{-1}A_{k-1}^T+Q_k)^{-1}v_k+C_{k}^TR_{k}^{-1}y_{k} \\ &=(A_{k-1}I_{k-1}^{-1}A_{k-1}^T+Q_k)^{-1}(A_{k-1}I_{k-1}^{-1}q_{k-1}+v_k)+C_{k}^TR_{k}^{-1}y_{k}\end{aligned} qk=(Ak−1Ik−1−1Ak−1T+Qk)−1Ak−1Ik−1−1qk−1+(Ak−1Ik−1−1Ak−1T+Qk)−1vk+CkTRk−1yk=(Ak−1Ik−1−1Ak−1T+Qk)−1(Ak−1Ik−1−1qk−1+vk)+CkTRk−1yk
由于 P ^ k , f = I k − 1 \hat P_{k,f}=I_k^{-1} P^k,f=Ik−1，因此：
q k = ( A k − 1 P ^ k − 1 , f A k − 1 T + Q k ) − 1 ( A k − 1 P ^ k − 1 , f q k − 1 + v k ) + C k T R k − 1 y k = P ˇ k , f − 1 ( A k − 1 P ^ k − 1 , f q k − 1 + v k ) + C k T R k − 1 y k \begin{aligned}q_k&=(A_{k-1}\hat P_{k-1,f}A_{k-1}^T+Q_k)^{-1}(A_{k-1}\hat P_{k-1,f}q_{k-1}+v_k)+C_{k}^TR_{k}^{-1}y_{k}\\&=\check P_{k,f}^{-1}(A_{k-1}\hat P_{k-1,f}q_{k-1}+v_k)+C_{k}^TR_{k}^{-1}y_{k}\end{aligned} qk=(Ak−1P^k−1,fAk−1T+Qk)−1(Ak−1P^k−1,fqk−1+vk)+CkTRk−1yk=Pˇk,f−1(Ak−1P^k−1,fqk−1+vk)+CkTRk−1yk
定义 P ^ k , f − 1 x ^ k , f = q k \hat P_{k,f}^{-1}\hat x_{k,f}=q_k P^k,f−1x^k,f=qk，那么上式可以写成两步：
x ˇ k , f = A k − 1 x ^ k − 1 , f + v k P ^ k , f − 1 x ^ k , f = P ˇ k , f − 1 x ˇ k , f + C k T R k − 1 y k \begin{aligned}\check x_{k,f}&=A_{k-1}\hat x_{k-1,f}+v_k \\ \hat P_{k,f}^{-1}\hat x_{k,f}&=\check P_{k,f}^{-1}\check x_{k,f}+C_k^TR_{k}^{-1}y_k\end{aligned} xˇk,fP^k,f−1x^k,f=Ak−1x^k−1,f+vk=Pˇk,f−1xˇk,f+CkTRk−1yk
第一个方程就是经典形式，而第二个方程是信息矩阵（协方差的逆）来写的。为了将其写成经典形式，重写成：
x ^ k , f = P ^ k , f P ˇ k , f − 1 x ˇ k , f + P ^ k , f C k T R k − 1 y k \hat x_{k,f}=\hat P_{k,f}\check P_{k,f}^{-1}\check x_{k,f}+\hat P_{k,f}C_k^TR_{k}^{-1}y_k x^k,f=P^k,fPˇk,f−1xˇk,f+P^k,fCkTRk−1yk
由于 P ^ k , f P ˇ k , f − 1 = 1 − K k C k \hat P_{k,f}\check P_{k,f}^{-1}=1-K_kC_k P^k,fPˇk,f−1=1−KkCk， P ^ k , f C k T R k − 1 = K k \hat P_{k,f}C_k^TR_{k}^{-1}=K_k P^k,fCkTRk−1=Kk：
x ^ k , f = x ˇ k , f + K k ( y k − C k x ˇ k , f ) \hat x_{k,f}=\check x_{k,f}+K_k(y_k-C_k\check x_{k,f}) x^k,f=xˇk,f+Kk(yk−Ckxˇk,f)
接下来是向后迭代， L k − 1 T x ^ k − 1 L_{k-1}^T\hat x_{k-1} Lk−1Tx^k−1方程两边同时乘以 L k − 1 L_{k-1} Lk−1，再求解 x ^ k − 1 \hat x_{k-1} x^k−1：
x ^ k − 1 = ( L k − 1 L k − 1 T ) − 1 L k − 1 ( − L k , k − 1 T x ^ k + d k − 1 ) = ( L k − 1 L k − 1 T ) − 1 ( − L k − 1 L k , k − 1 T x ^ k + L k − 1 d k − 1 ) \begin{aligned}\hat x_{k-1}&=(L_{k-1}L_{k-1}^T)^{-1}L_{k-1}(-L_{k,k-1}^T\hat x_k+d_{k-1}) \\ &=(L_{k-1}L_{k-1}^T)^{-1}(-L_{k-1}L_{k,k-1}^T\hat x_k+L_{k-1}d_{k-1})\end{aligned} x^k−1=(Lk−1Lk−1T)−1Lk−1(−Lk,k−1Tx^k+dk−1)=(Lk−1Lk−1T)−1(−Lk−1Lk,k−1Tx^k+Lk−1dk−1)
代入 L k − 1 L k − 1 T L_{k-1}L_{k-1}^T Lk−1Lk−1T方程， L k − 1 d k − 1 L_{k-1}d_{k-1} Lk−1dk−1方程， L k , k − 1 L k − 1 T L_{k,k-1}L_{k-1}^T Lk,k−1Lk−1T方程：
x ^ k − 1 = ( I k − 1 + A k − 1 T Q k − 1 A k − 1 ) − 1 A k − 1 T Q k − 1 ( x ^ k − v k ) + ( I k − 1 + A k − 1 T Q k − 1 A k − 1 ) − 1 q k − 1 \hat x_{k-1}=(I_{k-1}+A_{k-1}^TQ_k^{-1}A_{k-1})^{-1}A_{k-1}^TQ_k^{-1}(\hat x_k-v_k)+(I_{k-1}+A_{k-1}^TQ_k^{-1}A_{k-1})^{-1}q_{k-1} x^k−1=(Ik−1+Ak−1TQk−1Ak−1)−1Ak−1TQk−1(x^k−vk)+(Ik−1+Ak−1TQk−1Ak−1)−1qk−1
根据SMW恒等式：
( I k − 1 + A k − 1 T Q k − 1 A k − 1 ) − 1 A k − 1 T Q k − 1 = I k − 1 − 1 A k − 1 T ( A k − 1 I k − 1 − 1 A k − 1 T + Q k ) − 1 ( I k − 1 + A k − 1 T Q k − 1 A k − 1 ) − 1 = I k − 1 − 1 − I k − 1 − 1 A k − 1 T ( A k − 1 I k − 1 − 1 A k − 1 T + Q k ) − 1 A k − 1 I k − 1 − 1 \begin{aligned}(I_{k-1}+A_{k-1}^TQ_k^{-1}A_{k-1})^{-1}A_{k-1}^TQ_k^{-1}&=I_{k-1}^{-1}A_{k-1}^T(A_{k-1}I_{k-1}^{-1}A_{k-1}^T+Q_k)^{-1} \\ (I_{k-1}+A_{k-1}^TQ_k^{-1}A_{k-1})^{-1}&=I_{k-1}^{-1}-I_{k-1}^{-1}A_{k-1}^T(A_{k-1}I_{k-1}^{-1}A_{k-1}^T+Q_k)^{-1}A_{k-1}I_{k-1}^{-1}\end{aligned} (Ik−1+Ak−1TQk−1Ak−1)−1Ak−1TQk−1(Ik−1+Ak−1TQk−1Ak−1)−1=Ik−1−1Ak−1T(Ak−1Ik−1−1Ak−1T+Qk)−1=Ik−1−1−Ik−1−1Ak−1T(Ak−1Ik−1−1Ak−1T+Qk)−1Ak−1Ik−1−1
再使用之前的符号进行化简：
x ^ k − 1 = x ^ k − 1 , f + P ^ k − 1 , f A k − 1 T P ˇ k , f − 1 ( x ^ k − x ˇ k , f ) \hat x_{k-1}=\hat x_{k-1,f}+\hat P_{k-1,f}A_{k-1}^T\check P_{k,f}^{-1}(\hat x_k-\check x_{k,f}) x^k−1=x^k−1,f+P^k−1,fAk−1TPˇk,f−1(x^k−xˇk,f)
梳理一下整个过程：
前向： k = 0 , 1 , . . . , K k=0,1,...,K k=0,1,...,K
x ˇ k , f = A k − 1 x ^ k − 1 , f + v k P ˇ k , f = A k − 1 P ^ k − 1 , f A k − 1 T + Q k K k = P ˇ k , f C K T ( P ˇ k , f − 1 + C k T R k − 1 C k ) − 1 x ^ k , f = x ˇ k , f + K k ( y k − C k x ˇ k , f ) P ^ k , f = ( 1 − K k C k ) P ˇ k , f \begin{aligned}\check x_{k,f}&=A_{k-1}\hat x_{k-1,f}+v_k \\ \check P_{k,f}&=A_{k-1}\hat P_{k-1,f}A_{k-1}^T+Q_k \\ K_k&=\check P_{k,f}C_K^T(\check P_{k,f}^{-1}+C_k^TR_k^{-1}C_k)^{-1} \\ \hat x_{k,f}&=\check x_{k,f}+K_k(y_k-C_k\check x_{k,f}) \\ \hat P_{k,f}&=(1-K_kC_k)\check P_{k,f}\end{aligned} xˇk,fPˇk,fKkx^k,fP^k,f=Ak−1x^k−1,f+vk=Ak−1P^k−1,fAk−1T+Qk=Pˇk,fCKT(Pˇk,f−1+CkTRk−1Ck)−1=xˇk,f+Kk(yk−Ckxˇk,f)=(1−KkCk)Pˇk,f
后向： k = K , K − 1 , . . . , 0 k=K,K-1,...,0 k=K,K−1,...,0
x ^ k − 1 = x ^ k − 1 , f + P ^ k − 1 , f A k − 1 T P ˇ k , f − 1 ( x ^ k − x ˇ k , f ) \hat x_{k-1}=\hat x_{k-1,f}+\hat P_{k-1,f}A_{k-1}^T\check P_{k,f}^{-1}(\hat x_k-\check x_{k,f}) x^k−1=x^k−1,f+P^k−1,fAk−1TPˇk,f−1(x^k−xˇk,f)
这些值的初值：
P ˇ 0 , f = P ˇ 0 x ˇ 0 , f = x ˇ 0 x ^ K = x ^ K , f \begin{aligned}\check P_{0,f}&=\check P_0 \\ \check x_{0,f}&=\check x_0 \\ \hat x_K&=\hat x_{K,f}\end{aligned} Pˇ0,fxˇ0,fx^K=Pˇ0=xˇ0=x^K,f