1. 问题背景
在运动目标跟踪中，我们常常只能观测到带噪声的位置，例如 GPS、雷达或视觉测量。
假设一个物体近似做一维匀速运动。真实状态包括：
xk=[pkvk] x_k= \begin{bmatrix} p_k\\ v_k \end{bmatrix} xk=[pkvk]
其中：
- pkp_kpk：第 kkk 时刻的位置
- vkv_kvk：第 kkk 时刻的速度
但传感器通常只能观测位置：
yk=pk+rk y_k = p_k + r_k yk=pk+rk
其中 rkr_krk 是观测噪声。
Kalman filter 可以根据过去和当前观测估计当前状态：
p(xk∣y1:k) p(x_k\mid y_{1:k}) p(xk∣y1:k)
而 RTS smoother 想估计的是：
p(xk∣y1:T) p(x_k\mid y_{1:T}) p(xk∣y1:T)
也就是说，它会利用未来观测修正过去状态。
2. 匀速运动状态空间模型
设采样间隔：
Δt=1 \Delta t = 1 Δt=1
匀速运动满足：
pk=pk−1+vk−1Δt p_k = p_{k-1} + v_{k-1}\Delta t pk=pk−1+vk−1Δt
vk=vk−1 v_k = v_{k-1} vk=vk−1
写成矩阵形式：
xk=Fxk−1+qk x_k = F x_{k-1} + q_k xk=Fxk−1+qk
其中：
F=[1101] F= \begin{bmatrix} 1 & 1\\ 0 & 1 \end{bmatrix} F=[1011]
过程噪声：
qk∼N(0,Q) q_k\sim \mathcal N(0,Q) qk∼N(0,Q)
它表示模型不是完美匀速，可能存在轻微加速度、扰动或建模误差。
观测模型为：
yk=Hxk+rk y_k = Hx_k + r_k yk=Hxk+rk
其中：
H=[10] H= \begin{bmatrix} 1 & 0 \end{bmatrix} H=[10]
观测噪声：
rk∼N(0,R) r_k\sim \mathcal N(0,R) rk∼N(0,R)
3. Kalman Filter 前向递推
RTS smoother 的第一步是先做 Kalman filter。
Kalman filter 每一步分为两部分：
- 预测 prediction
- 更新 update
3.1 状态预测
x^k∣k−1=Fx^k−1∣k−1 \hat x_{k|k-1}=F\hat x_{k-1|k-1} x^k∣k−1=Fx^k−1∣k−1
物理意义是：
根据上一时刻的位置和速度，用匀速运动模型预测当前状态。
如果：
x^k−1∣k−1=[p^k−1v^k−1] \hat x_{k-1|k-1}= \begin{bmatrix} \hat p_{k-1}\\ \hat v_{k-1} \end{bmatrix} x^k−1∣k−1=[p^k−1v^k−1]
那么：
x^k∣k−1=[1101][p^k−1v^k−1]=[p^k−1+v^k−1v^k−1] \hat x_{k|k-1}= \begin{bmatrix} 1&1\\ 0&1 \end{bmatrix} \begin{bmatrix} \hat p_{k-1}\\ \hat v_{k-1} \end{bmatrix}= \begin{bmatrix} \hat p_{k-1}+\hat v_{k-1}\\ \hat v_{k-1} \end{bmatrix} x^k∣k−1=[1011][p^k−1v^k−1]=[p^k−1+v^k−1v^k−1]
也就是：
p^k=p^k−1+v^k−1 \hat p_k = \hat p_{k-1}+\hat v_{k-1} p^k=p^k−1+v^k−1
v^k=v^k−1 \hat v_k = \hat v_{k-1} v^k=v^k−1
3.2 协方差预测
Pk∣k−1=FPk−1∣k−1F⊤+Q P_{k|k-1}=FP_{k-1|k-1}F^\top+Q Pk∣k−1=FPk−1∣k−1F⊤+Q
这里 PPP 是状态估计误差协方差。
它衡量我们对状态估计的不确定性。
如果：
x∼N(μ,P) x\sim \mathcal N(\mu,P) x∼N(μ,P)
经过线性变换：
z=Fx z=Fx z=Fx
则：
Cov(z)=FPF⊤ \mathrm{Cov}(z)=FPF^\top Cov(z)=FPF⊤
所以：
FPk−1∣k−1F⊤ FP_{k-1|k-1}F^\top FPk−1∣k−1F⊤
表示上一时刻的不确定性通过运动模型传播到当前时刻。
再加上过程噪声：
Q Q Q
表示运动模型本身引入的新不确定性。
3.3 Kalman 增益
Kk=Pk∣k−1H⊤(HPk∣k−1H⊤+R)−1 K_k=P_{k|k-1}H^\top \left( HP_{k|k-1}H^\top+R \right)^{-1} Kk=Pk∣k−1H⊤(HPk∣k−1H⊤+R)−1
Kalman gain 决定：
当前观测来了以后，我们应该相信模型预测多一点，还是相信观测多一点。
其中：
HPk∣k−1H⊤ HP_{k|k-1}H^\top HPk∣k−1H⊤
是预测状态投影到观测空间后的不确定性。
R R R
是观测噪声。
如果 RRR 很小，说明观测很准，KkK_kKk 会较大。
如果 RRR 很大，说明观测很吵，KkK_kKk 会较小。
3.4 状态更新
x^k∣k=x^k∣k−1+Kk(yk−Hx^k∣k−1) \hat x_{k|k} = \hat x_{k|k-1} + K_k \left( y_k-H\hat x_{k|k-1} \right) x^k∣k=x^k∣k−1+Kk(yk−Hx^k∣k−1)
其中：
yk−Hx^k∣k−1 y_k-H\hat x_{k|k-1} yk−Hx^k∣k−1
称为 innovation，也叫残差。
它表示：
实际观测值与预测观测值之间的差异。
如果观测位置比预测位置大，残差为正，估计会被往上修正。
如果观测位置比预测位置小，残差为负，估计会被往下修正。
3.5 协方差更新
Pk∣k=(I−KkH)Pk∣k−1 P_{k|k}=(I-K_kH)P_{k|k-1} Pk∣k=(I−KkH)Pk∣k−1
观测提供了新信息，因此更新后的不确定性通常会下降。
4. RTS Smoother 后向递推
Kalman filter 得到的是：
p(xk∣y1:k) p(x_k\mid y_{1:k}) p(xk∣y1:k)
RTS smoother 要得到：
p(xk∣y1:T) p(x_k\mid y_{1:T}) p(xk∣y1:T)
也就是用完整观测序列修正每个历史状态。
RTS smoother 从最后时刻 TTT 开始往前递推。
初始化：
x^T∣Ts=x^T∣T \hat x_{T|T}^{s}=\hat x_{T|T} x^T∣Ts=x^T∣T
PT∣Ts=PT∣T P_{T|T}^{s}=P_{T|T} PT∣Ts=PT∣T
因为最后一个时刻没有未来信息，所以滤波结果和平滑结果相同。
4.1 RTS 平滑增益
RTS smoother 的平滑增益为：
Gk=Pk∣kF⊤Pk+1∣k−1 G_k=P_{k|k}F^\top P_{k+1|k}^{-1} Gk=Pk∣kF⊤Pk+1∣k−1
它的物理意义是：
未来状态被修正以后，这个修正应该以多大程度反向影响当前状态。
为什么是这个形式？
前向滤波后：
xk∣y1:k∼N(x^k∣k,Pk∣k) x_k\mid y_{1:k} \sim \mathcal N(\hat x_{k|k},P_{k|k}) xk∣y1:k∼N(x^k∣k,Pk∣k)
由状态方程：
xk+1=Fxk+qk x_{k+1}=Fx_k+q_k xk+1=Fxk+qk
可知：
xk+1∣y1:k∼N(x^k+1∣k,Pk+1∣k) x_{k+1}\mid y_{1:k} \sim \mathcal N(\hat x_{k+1|k},P_{k+1|k}) xk+1∣y1:k∼N(x^k+1∣k,Pk+1∣k)
其中：
x^k+1∣k=Fx^k∣k \hat x_{k+1|k}=F\hat x_{k|k} x^k+1∣k=Fx^k∣k
Pk+1∣k=FPk∣kF⊤+Q P_{k+1|k}=FP_{k|k}F^\top+Q Pk+1∣k=FPk∣kF⊤+Q
由于 xkx_kxk 和 xk+1x_{k+1}xk+1 相关，它们的交叉协方差为：
Cov(xk,xk+1∣y1:k)=Pk∣kF⊤ \mathrm{Cov}(x_k,x_{k+1}\mid y_{1:k}) = P_{k|k}F^\top Cov(xk,xk+1∣y1:k)=Pk∣kF⊤
对联合高斯分布使用条件期望公式：
E[xk∣xk+1,y1:k]=x^k∣k+Pk∣kF⊤Pk+1∣k−1(xk+1−x^k+1∣k) \mathbb E[x_k\mid x_{k+1},y_{1:k}] = \hat x_{k|k} + P_{k|k}F^\top P_{k+1|k}^{-1} (x_{k+1}-\hat x_{k+1|k}) E[xk∣xk+1,y1:k]=x^k∣k+Pk∣kF⊤Pk+1∣k−1(xk+1−x^k+1∣k)
于是自然得到：
Gk=Pk∣kF⊤Pk+1∣k−1 G_k=P_{k|k}F^\top P_{k+1|k}^{-1} Gk=Pk∣kF⊤Pk+1∣k−1
所以 GkG_kGk 本质上是：
xk 与 xk+1 的相关性xk+1 的不确定性 \frac{ x_k \text{ 与 } x_{k+1} \text{ 的相关性} }{ x_{k+1} \text{ 的不确定性} } xk+1 的不确定性xk 与 xk+1 的相关性
4.2 平滑状态更新
x^k∣T=x^k∣k+Gk(x^k+1∣T−x^k+1∣k) \hat x_{k|T} = \hat x_{k|k} + G_k \left( \hat x_{k+1|T} - \hat x_{k+1|k} \right) x^k∣T=x^k∣k+Gk(x^k+1∣T−x^k+1∣k)
其中：
x^k∣k \hat x_{k|k} x^k∣k
是前向滤波结果。
x^k+1∣k \hat x_{k+1|k} x^k+1∣k
是在第 kkk 时刻对第 k+1k+1k+1 时刻的预测。
x^k+1∣T \hat x_{k+1|T} x^k+1∣T
是第 k+1k+1k+1 时刻的平滑结果，它已经利用了未来观测。
关键项是：
x^k+1∣T−x^k+1∣k \hat x_{k+1|T}-\hat x_{k+1|k} x^k+1∣T−x^k+1∣k
它表示：
原来从 kkk 预测到 k+1k+1k+1 的结果，和后来利用全部数据得到的更准结果之间的差异。
RTS smoother 把这个差异通过 GkG_kGk 反向传播给 xkx_kxk。
一句话：
过去的平滑估计=过去的滤波估计+未来修正量的反向传播 \boxed{ \text{过去的平滑估计} = \text{过去的滤波估计} + \text{未来修正量的反向传播} } 过去的平滑估计=过去的滤波估计+未来修正量的反向传播
4.3 平滑协方差更新
Pk∣T=Pk∣k+Gk(Pk+1∣T−Pk+1∣k)Gk⊤ P_{k|T} = P_{k|k} + G_k \left( P_{k+1|T}-P_{k+1|k} \right) G_k^\top Pk∣T=Pk∣k+Gk(Pk+1∣T−Pk+1∣k)Gk⊤
因为平滑使用了更多观测，一般有：
Pk+1∣T⪯Pk+1∣k P_{k+1|T}\preceq P_{k+1|k} Pk+1∣T⪯Pk+1∣k
所以：
Pk+1∣T−Pk+1∣k P_{k+1|T}-P_{k+1|k} Pk+1∣T−Pk+1∣k
通常是负半定矩阵。
因此：
Pk∣T⪯Pk∣k P_{k|T}\preceq P_{k|k} Pk∣T⪯Pk∣k
物理意义是：
未来信息减少了第 k+1k+1k+1 时刻的不确定性，这个不确定性减少量会通过状态动力学反向传递给第 kkk 时刻。
5. 数值例子：一维匀速运动
设：
Δt=1 \Delta t=1 Δt=1
F=[1101] F= \begin{bmatrix} 1&1\\ 0&1 \end{bmatrix} F=[1011]
H=[10] H= \begin{bmatrix} 1&0 \end{bmatrix} H=[10]
初始状态：
x^0∣0=[01] \hat x_{0|0} = \begin{bmatrix} 0\\ 1 \end{bmatrix} x^0∣0=[01]
初始协方差：
P0∣0=[1001] P_{0|0} = \begin{bmatrix} 1&0\\ 0&1 \end{bmatrix} P0∣0=[1001]
过程噪声：
Q=[0.1000.1] Q= \begin{bmatrix} 0.1&0\\ 0&0.1 \end{bmatrix} Q=[0.1000.1]
观测噪声：
R=1 R=1 R=1
观测值：
y1=1.2,y2=2.0,y3=2.9 y_1=1.2,\quad y_2=2.0,\quad y_3=2.9 y1=1.2,y2=2.0,y3=2.9
6. 前向 Kalman Filter 计算
6.1 k=1k=1k=1
预测状态：
x^1∣0=Fx^0∣0=[1101][01]=[11] \hat x_{1|0} = F\hat x_{0|0} = \begin{bmatrix} 1&1\\ 0&1 \end{bmatrix} \begin{bmatrix} 0\\ 1 \end{bmatrix} = \begin{bmatrix} 1\\ 1 \end{bmatrix} x^1∣0=Fx^0∣0=[1011][01]=[11]
预测协方差：
P1∣0=FP0∣0F⊤+Q=[2.1111.1] P_{1|0} = FP_{0|0}F^\top+Q = \begin{bmatrix} 2.1&1\\ 1&1.1 \end{bmatrix} P1∣0=FP0∣0F⊤+Q=[2.1111.1]
观测预测：
Hx^1∣0=1 H\hat x_{1|0}=1 Hx^1∣0=1
残差：
y1−Hx^1∣0=1.2−1=0.2 y_1-H\hat x_{1|0}=1.2-1=0.2 y1−Hx^1∣0=1.2−1=0.2
Kalman 增益：
K1=P1∣0H⊤(HP1∣0H⊤+R)−1=[2.11](2.1+1)−1=[0.67740.3226] K_1 = P_{1|0}H^\top (HP_{1|0}H^\top+R)^{-1} = \begin{bmatrix} 2.1\\ 1 \end{bmatrix} (2.1+1)^{-1} = \begin{bmatrix} 0.6774\\ 0.3226 \end{bmatrix} K1=P1∣0H⊤(HP1∣0H⊤+R)−1=[2.11](2.1+1)−1=[0.67740.3226]
状态更新：
x^1∣1=[11]+[0.67740.3226]0.2=[1.13551.0645] \hat x_{1|1} = \begin{bmatrix} 1\\ 1 \end{bmatrix} + \begin{bmatrix} 0.6774\\ 0.3226 \end{bmatrix} 0.2 = \begin{bmatrix} 1.1355\\ 1.0645 \end{bmatrix} x^1∣1=[11]+[0.67740.3226]0.2=[1.13551.0645]
物理意义：
观测位置 1.21.21.2 比预测位置 1.01.01.0 大，所以位置被上调。由于位置偏大也可能意味着速度偏大，因此速度也被上调。
6.2 k=2k=2k=2
预测状态：
x^2∣1=Fx^1∣1=[1101][1.13551.0645]=[2.20001.0645] \hat x_{2|1} = F\hat x_{1|1} = \begin{bmatrix} 1&1\\ 0&1 \end{bmatrix} \begin{bmatrix} 1.1355\\ 1.0645 \end{bmatrix} = \begin{bmatrix} 2.2000\\ 1.0645 \end{bmatrix} x^2∣1=Fx^1∣1=[1011][1.13551.0645]=[2.20001.0645]
预测协方差：
P2∣1=FP1∣1F⊤+Q=[2.20001.10001.10000.8774] P_{2|1} = FP_{1|1}F^\top+Q = \begin{bmatrix} 2.2000&1.1000\\ 1.1000&0.8774 \end{bmatrix} P2∣1=FP1∣1F⊤+Q=[2.20001.10001.10000.8774]
残差：
y2−Hx^2∣1=2.0−2.2=−0.2 y_2-H\hat x_{2|1} = 2.0-2.2=-0.2 y2−Hx^2∣1=2.0−2.2=−0.2
Kalman 增益：
K2=[2.21.1](2.2+1)−1=[0.68750.3438] K_2 = \begin{bmatrix} 2.2\\ 1.1 \end{bmatrix} (2.2+1)^{-1} = \begin{bmatrix} 0.6875\\ 0.3438 \end{bmatrix} K2=[2.21.1](2.2+1)−1=[0.68750.3438]
状态更新：
x^2∣2=[2.21.0645]+[0.68750.3438](−0.2)=[2.06250.9958] \hat x_{2|2} = \begin{bmatrix} 2.2\\ 1.0645 \end{bmatrix} + \begin{bmatrix} 0.6875\\ 0.3438 \end{bmatrix} (-0.2) = \begin{bmatrix} 2.0625\\ 0.9958 \end{bmatrix} x^2∣2=[2.21.0645]+[0.68750.3438](−0.2)=[2.06250.9958]
物理意义：
观测位置 2.02.02.0 比预测位置 2.22.22.2 小，所以位置和速度都被下调。
6.3 k=3k=3k=3
预测状态：
x^3∣2=Fx^2∣2=[3.05830.9958] \hat x_{3|2} = F\hat x_{2|2} = \begin{bmatrix} 3.0583\\ 0.9958 \end{bmatrix} x^3∣2=Fx^2∣2=[3.05830.9958]
预测协方差：
P3∣2=[1.97430.84300.84300.5993] P_{3|2} = \begin{bmatrix} 1.9743&0.8430\\ 0.8430&0.5993 \end{bmatrix} P3∣2=[1.97430.84300.84300.5993]
残差：
y3−Hx^3∣2=2.9−3.0583=−0.1583 y_3-H\hat x_{3|2} = 2.9-3.0583=-0.1583 y3−Hx^3∣2=2.9−3.0583=−0.1583
Kalman 增益：
K3=[1.97430.8430](1.9743+1)−1=[0.66380.2834] K_3 = \begin{bmatrix} 1.9743\\ 0.8430 \end{bmatrix} (1.9743+1)^{-1} = \begin{bmatrix} 0.6638\\ 0.2834 \end{bmatrix} K3=[1.97430.8430](1.9743+1)−1=[0.66380.2834]
状态更新：
x^3∣3=[3.05830.9958]+[0.66380.2834](−0.1583)=[2.95320.9509] \hat x_{3|3} = \begin{bmatrix} 3.0583\\ 0.9958 \end{bmatrix} + \begin{bmatrix} 0.6638\\ 0.2834 \end{bmatrix} (-0.1583) = \begin{bmatrix} 2.9532\\ 0.9509 \end{bmatrix} x^3∣3=[3.05830.9958]+[0.66380.2834](−0.1583)=[2.95320.9509]
7. 前向滤波结果
| 时刻 | 滤波位置 | 滤波速度 |
|---|---|---|
| 1 | 1.1355 | 1.0645 |
| 2 | 2.0625 | 0.9958 |
| 3 | 2.9532 | 0.9509 |
这些是：
x^k∣k=E[xk∣y1:k] \hat x_{k|k}=E[x_k\mid y_{1:k}] x^k∣k=E[xk∣y1:k]
只用了当前及过去观测。
8. 后向 RTS Smoother 计算
初始化：
x^3∣T=x^3∣3=[2.95320.9509] \hat x_{3|T}=\hat x_{3|3} = \begin{bmatrix} 2.9532\\ 0.9509 \end{bmatrix} x^3∣T=x^3∣3=[2.95320.9509]
8.1 从 k=2k=2k=2 回推
RTS 平滑增益：
G2=P2∣2F⊤P3∣2−1 G_2 = P_{2|2}F^\top P_{3|2}^{-1} G2=P2∣2F⊤P3∣2−1
代入数值后：
G2=[0.6947−0.40370.17840.5821] G_2 = \begin{bmatrix} 0.6947&-0.4037\\ 0.1784&0.5821 \end{bmatrix} G2=[0.69470.1784−0.40370.5821]
未来修正量：
x^3∣T−x^3∣2=[2.95320.9509]−[3.05830.9958]=[−0.1051−0.0449] \hat x_{3|T}-\hat x_{3|2} = \begin{bmatrix} 2.9532\\ 0.9509 \end{bmatrix} - \begin{bmatrix} 3.0583\\ 0.9958 \end{bmatrix} = \begin{bmatrix} -0.1051\\ -0.0449 \end{bmatrix} x^3∣T−x^3∣2=[2.95320.9509]−[3.05830.9958]=[−0.1051−0.0449]
这说明：
站在全局角度看，第 3 步原来的预测位置和速度都偏高。
反向修正：
G2(x^3∣T−x^3∣2)≈[−0.0549−0.0449] G_2 \left( \hat x_{3|T}-\hat x_{3|2} \right) \approx \begin{bmatrix} -0.0549\\ -0.0449 \end{bmatrix} G2(x^3∣T−x^3∣2)≈[−0.0549−0.0449]
所以：
x^2∣T=x^2∣2+G2(x^3∣T−x^3∣2)=[2.06250.9958]+[−0.0549−0.0449]=[2.00760.9509] \hat x_{2|T} = \hat x_{2|2} + G_2 \left( \hat x_{3|T}-\hat x_{3|2} \right) = \begin{bmatrix} 2.0625\\ 0.9958 \end{bmatrix} + \begin{bmatrix} -0.0549\\ -0.0449 \end{bmatrix} = \begin{bmatrix} 2.0076\\ 0.9509 \end{bmatrix} x^2∣T=x^2∣2+G2(x^3∣T−x^3∣2)=[2.06250.9958]+[−0.0549−0.0449]=[2.00760.9509]
8.2 从 k=1k=1k=1 回推
RTS 平滑增益：
G1=P1∣1F⊤P2∣1−1 G_1 = P_{1|1}F^\top P_{2|1}^{-1} G1=P1∣1F⊤P2∣1−1
数值结果：
G1=[0.7255−0.54190.15270.6946] G_1 = \begin{bmatrix} 0.7255&-0.5419\\ 0.1527&0.6946 \end{bmatrix} G1=[0.72550.1527−0.54190.6946]
未来修正量：
x^2∣T−x^2∣1=[2.00760.9509]−[2.20001.0645]=[−0.1924−0.1136] \hat x_{2|T}-\hat x_{2|1} = \begin{bmatrix} 2.0076\\ 0.9509 \end{bmatrix} - \begin{bmatrix} 2.2000\\ 1.0645 \end{bmatrix} = \begin{bmatrix} -0.1924\\ -0.1136 \end{bmatrix} x^2∣T−x^2∣1=[2.00760.9509]−[2.20001.0645]=[−0.1924−0.1136]
反向修正：
G1(x^2∣T−x^2∣1)≈[−0.0780−0.1083] G_1 \left( \hat x_{2|T}-\hat x_{2|1} \right) \approx \begin{bmatrix} -0.0780\\ -0.1083 \end{bmatrix} G1(x^2∣T−x^2∣1)≈[−0.0780−0.1083]
所以：
x^1∣T=x^1∣1+G1(x^2∣T−x^2∣1)=[1.13551.0645]+[−0.0780−0.1083]=[1.05750.9562] \hat x_{1|T} = \hat x_{1|1} + G_1 \left( \hat x_{2|T}-\hat x_{2|1} \right) = \begin{bmatrix} 1.1355\\ 1.0645 \end{bmatrix} + \begin{bmatrix} -0.0780\\ -0.1083 \end{bmatrix} = \begin{bmatrix} 1.0575\\ 0.9562 \end{bmatrix} x^1∣T=x^1∣1+G1(x^2∣T−x^2∣1)=[1.13551.0645]+[−0.0780−0.1083]=[1.05750.9562]
9. 最终结果对比
| 时刻 | 滤波位置 | 滤波速度 | 平滑位置 | 平滑速度 |
|---|---|---|---|---|
| 1 | 1.1355 | 1.0645 | 1.0575 | 0.9562 |
| 2 | 2.0625 | 0.9958 | 2.0076 | 0.9509 |
| 3 | 2.9532 | 0.9509 | 2.9532 | 0.9509 |
可以看到：
- 最后时刻不变，因为没有未来信息
- 早期位置和速度都被修正
- 速度估计变得更一致
观测序列：
1.2,2.0,2.9 1.2,\quad 2.0,\quad 2.9 1.2,2.0,2.9
整体看速度接近：
2.9−1.22=0.85 \frac{2.9-1.2}{2}=0.85 22.9−1.2=0.85
前向滤波在第 1 步时只看到 y1=1.2y_1=1.2y1=1.2，因此速度估计偏高：
v1∣1=1.0645 v_{1|1}=1.0645 v1∣1=1.0645
但 RTS smoother 看到完整轨迹后，把它修正为：
v1∣T=0.9562 v_{1|T}=0.9562 v1∣T=0.9562
10. RTS Smoother 的物理意义总结
Kalman filter 是"边走边估计"：
x^k∣k=E[xk∣y1:k] \hat x_{k|k}=E[x_k\mid y_{1:k}] x^k∣k=E[xk∣y1:k]
它只能利用过去和当前信息。
RTS smoother 是"事后复盘"：
x^k∣T=E[xk∣y1:T] \hat x_{k|T}=E[x_k\mid y_{1:T}] x^k∣T=E[xk∣y1:T]
它利用了未来信息。
在匀速运动模型中，未来位置观测不仅能修正过去位置，也能修正过去速度。
RTS smoother 的三条核心公式：
Gk=Pk∣kF⊤Pk+1∣k−1 G_k=P_{k|k}F^\top P_{k+1|k}^{-1} Gk=Pk∣kF⊤Pk+1∣k−1
x^k∣T=x^k∣k+Gk(x^k+1∣T−x^k+1∣k) \hat x_{k|T} = \hat x_{k|k} + G_k \left( \hat x_{k+1|T} - \hat x_{k+1|k} \right) x^k∣T=x^k∣k+Gk(x^k+1∣T−x^k+1∣k)
Pk∣T=Pk∣k+Gk(Pk+1∣T−Pk+1∣k)Gk⊤ P_{k|T} = P_{k|k} + G_k \left( P_{k+1|T}-P_{k+1|k} \right) G_k^\top Pk∣T=Pk∣k+Gk(Pk+1∣T−Pk+1∣k)Gk⊤
可以理解为：
RTS smoother=把未来状态的修正量，按照运动模型和状态相关性，反向分配给过去状态。 \boxed{ \text{RTS smoother} = \text{把未来状态的修正量，按照运动模型和状态相关性，反向分配给过去状态。} } RTS smoother=把未来状态的修正量，按照运动模型和状态相关性，反向分配给过去状态。
这就是它在轨迹后处理、GPS 平滑、机器人定位和目标跟踪中非常有用的原因。