1. 问题背景

在运动目标跟踪中，我们常常只能观测到带噪声的位置，例如 GPS、雷达或视觉测量。

假设一个物体近似做一维匀速运动。真实状态包括：

$$x_k=\left[\begin{array}{c}{p}_k\\ v_k\end{array}\right]$$

其中：

• $p_k$：第 $k$ 时刻的位置
• $v_k$：第 $k$ 时刻的速度

但传感器通常只能观测位置：

$$y_k=p_k+r_k$$

其中 $r_k$ 是观测噪声。

Kalman filter 可以根据过去和当前观测估计当前状态：

$$p(x_k\mid y_{1:k})$$

而 RTS smoother 想估计的是：

$$p(x_k\mid y_{1:T})$$

也就是说，它会利用未来观测修正过去状态。

---

2. 匀速运动状态空间模型

设采样间隔：

$$\Delta t=1$$

匀速运动满足：

$$p_k=p_{k-1}+v_{k-1}\Delta t$$

$$v_k=v_{k-1}$$

写成矩阵形式：

$$x_k=F{x}_{k-1}+q_k$$

其中：

$$F=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]$$

过程噪声：

$$q_k\sim \mathcal{N}(0,Q)$$

它表示模型不是完美匀速，可能存在轻微加速度、扰动或建模误差。

观测模型为：

$$y_k=H{x}_k+r_k$$

其中：

$$H=\left[\begin{array}{cc}1& 0\end{array}\right]$$

观测噪声：

$$r_k\sim \mathcal{N}(0,R)$$

---

3. Kalman Filter 前向递推

RTS smoother 的第一步是先做 Kalman filter。

Kalman filter 每一步分为两部分：

1. 预测 prediction
2. 更新 update

---

3.1 状态预测

$${\hat{x}}_{k|k-1}=F{\hat{x}}_{k-1|k-1}$$

物理意义是：

根据上一时刻的位置和速度，用匀速运动模型预测当前状态。

如果：

$${\hat{x}}_{k-1|k-1}=\left[\begin{array}{c}{\hat{p}}_{k-1}\\ {\hat{v}}_{k-1}\end{array}\right]$$

那么：

$${\hat{x}}_{k|k-1}=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\hat{p}}_{k-1}\\ {\hat{v}}_{k-1}\end{array}\right]=\left[\begin{array}{c}{\hat{p}}_{k-1}+{\hat{v}}_{k-1}\\ {\hat{v}}_{k-1}\end{array}\right]$$

也就是：

$${\hat{p}}_k={\hat{p}}_{k-1}+{\hat{v}}_{k-1}$$

$${\hat{v}}_k={\hat{v}}_{k-1}$$

---

3.2 协方差预测

$$P_{k|k-1}=F{P}_{k-1|k-1}{F}^{\top }+Q$$

这里 $P$ 是状态估计误差协方差。

它衡量我们对状态估计的不确定性。

如果：

$$x\sim \mathcal{N}(\mu ,P)$$

经过线性变换：

$$z=Fx$$

则：

$$\mathrm{Cov}(z)=FP{F}^{\top }$$

所以：

$$F{P}_{k-1|k-1}{F}^{\top }$$

表示上一时刻的不确定性通过运动模型传播到当前时刻。

再加上过程噪声：

$$Q$$

表示运动模型本身引入的新不确定性。

---

3.3 Kalman 增益

$$K_k=P_{k|k-1}{H}^{\top }{\left(H{P}_{k|k-1}{H}^{\top }+R\right)}^{-1}$$

Kalman gain 决定：

当前观测来了以后，我们应该相信模型预测多一点，还是相信观测多一点。

其中：

$$H{P}_{k|k-1}{H}^{\top }$$

是预测状态投影到观测空间后的不确定性。

$$R$$

是观测噪声。

如果 $R$ 很小，说明观测很准，$K_k$ 会较大。

如果 $R$ 很大，说明观测很吵，$K_k$ 会较小。

---

3.4 状态更新

$${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k\left(y_k-H{\hat{x}}_{k|k-1}\right)$$

其中：

$$y_k-H{\hat{x}}_{k|k-1}$$

称为 innovation，也叫残差。

它表示：

实际观测值与预测观测值之间的差异。

如果观测位置比预测位置大，残差为正，估计会被往上修正。

如果观测位置比预测位置小，残差为负，估计会被往下修正。

---

3.5 协方差更新

$$P_{k|k}=(I-K_{k}H)P_{k|k-1}$$

观测提供了新信息，因此更新后的不确定性通常会下降。

---

4. RTS Smoother 后向递推

Kalman filter 得到的是：

$$p(x_k\mid y_{1:k})$$

RTS smoother 要得到：

$$p(x_k\mid y_{1:T})$$

也就是用完整观测序列修正每个历史状态。

RTS smoother 从最后时刻 $T$ 开始往前递推。

初始化：

$${\hat{x}}_{T|T}^s={\hat{x}}_{T|T}$$

$$P_{T|T}^s=P_{T|T}$$

因为最后一个时刻没有未来信息，所以滤波结果和平滑结果相同。

---

4.1 RTS 平滑增益

RTS smoother 的平滑增益为：

$$G_k=P_{k|k}{F}^{\top }{P}_{k+1|k}^{-1}$$

它的物理意义是：

未来状态被修正以后，这个修正应该以多大程度反向影响当前状态。

为什么是这个形式？

前向滤波后：

$$x_k\mid y_{1:k}\sim \mathcal{N}({\hat{x}}_{k|k},P_{k|k})$$

由状态方程：

$$x_{k+1}=F{x}_k+q_k$$

可知：

$$x_{k+1}\mid y_{1:k}\sim \mathcal{N}({\hat{x}}_{k+1|k},P_{k+1|k})$$

其中：

$${\hat{x}}_{k+1|k}=F{\hat{x}}_{k|k}$$

$$P_{k+1|k}=F{P}_{k|k}{F}^{\top }+Q$$

由于 $x_k$ 和 $x_{k+1}$ 相关，它们的交叉协方差为：

$$\mathrm{Cov}(x_k,x_{k+1}\mid y_{1:k})=P_{k|k}{F}^{\top }$$

对联合高斯分布使用条件期望公式：

$$\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})$$

于是自然得到：

$$G_k=P_{k|k}{F}^{\top }{P}_{k+1|k}^{-1}$$

所以 $G_k$ 本质上是：

$$\frac{x_k\text{ 与 }{x}_{k+1}\text{ 的相关性}}{x_{k+1}\text{ 的不确定性}}$$

---

4.2 平滑状态更新

$${\hat{x}}_{k|T}={\hat{x}}_{k|k}+G_k\left({\hat{x}}_{k+1|T}-{\hat{x}}_{k+1|k}\right)$$

其中：

$${\hat{x}}_{k|k}$$

是前向滤波结果。

$${\hat{x}}_{k+1|k}$$

是在第 $k$ 时刻对第 $k+1$ 时刻的预测。

$${\hat{x}}_{k+1|T}$$

是第 $k+1$ 时刻的平滑结果，它已经利用了未来观测。

关键项是：

$${\hat{x}}_{k+1|T}-{\hat{x}}_{k+1|k}$$

它表示：

原来从 $k$ 预测到 $k+1$ 的结果，和后来利用全部数据得到的更准结果之间的差异。

RTS smoother 把这个差异通过 $G_k$ 反向传播给 $x_k$。

一句话：

$$\boxed{\text{过去的平滑估计}=\text{过去的滤波估计}+\text{未来修正量的反向传播}}$$

---

4.3 平滑协方差更新

$$P_{k|T}=P_{k|k}+G_k\left(P_{k+1|T}-P_{k+1|k}\right)G_k^{\top }$$

因为平滑使用了更多观测，一般有：

$$P_{k+1|T}⪯P_{k+1|k}$$

所以：

$$P_{k+1|T}-P_{k+1|k}$$

通常是负半定矩阵。

因此：

$$P_{k|T}⪯P_{k|k}$$

物理意义是：

未来信息减少了第 $k+1$ 时刻的不确定性，这个不确定性减少量会通过状态动力学反向传递给第 $k$ 时刻。

---

5. 数值例子：一维匀速运动

设：

$$\Delta t=1$$

$$F=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]$$

$$H=\left[\begin{array}{cc}1& 0\end{array}\right]$$

初始状态：

$${\hat{x}}_{0|0}=\left[\begin{array}{c}0\\ 1\end{array}\right]$$

初始协方差：

$$P_{0|0}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$$

过程噪声：

$$Q=\left[\begin{array}{cc}0.1& 0\\ 0& 0.1\end{array}\right]$$

观测噪声：

$$R=1$$

观测值：

$$y_1=1.2,y_2=2.0,y_3=2.9$$

---

6. 前向 Kalman Filter 计算

6.1 $k=1$

预测状态：

$${\hat{x}}_{1|0}=F{\hat{x}}_{0|0}=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]\left[\begin{array}{c}0\\ 1\end{array}\right]=\left[\begin{array}{c}1\\ 1\end{array}\right]$$

预测协方差：

$$P_{1|0}=F{P}_{0|0}{F}^{\top }+Q=\left[\begin{array}{cc}2.1& 1\\ 1& 1.1\end{array}\right]$$

观测预测：

$$H{\hat{x}}_{1|0}=1$$

残差：

$$y_1-H{\hat{x}}_{1|0}=1.2-1=0.2$$

Kalman 增益：

$$K_1=P_{1|0}{H}^{\top }(H{P}_{1|0}{H}^{\top }+R{)}^{-1}=\left[\begin{array}{c}2.1\\ 1\end{array}\right](2.1+1{)}^{-1}=\left[\begin{array}{c}0.6774\\ 0.3226\end{array}\right]$$

状态更新：

$${\hat{x}}_{1|1}=\left[\begin{array}{c}1\\ 1\end{array}\right]+\left[\begin{array}{c}0.6774\\ 0.3226\end{array}\right]0.2=\left[\begin{array}{c}1.1355\\ 1.0645\end{array}\right]$$

物理意义：

观测位置 $1.2$ 比预测位置 $1.0$ 大，所以位置被上调。由于位置偏大也可能意味着速度偏大，因此速度也被上调。

---

6.2 $k=2$

预测状态：

$${\hat{x}}_{2|1}=F{\hat{x}}_{1|1}=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]\left[\begin{array}{c}1.1355\\ 1.0645\end{array}\right]=\left[\begin{array}{c}2.2000\\ 1.0645\end{array}\right]$$

预测协方差：

$$P_{2|1}=F{P}_{1|1}{F}^{\top }+Q=\left[\begin{array}{cc}2.2000& 1.1000\\ 1.1000& 0.8774\end{array}\right]$$

残差：

$$y_2-H{\hat{x}}_{2|1}=2.0-2.2=-0.2$$

Kalman 增益：

$$K_2=\left[\begin{array}{c}2.2\\ 1.1\end{array}\right](2.2+1{)}^{-1}=\left[\begin{array}{c}0.6875\\ 0.3438\end{array}\right]$$

状态更新：

$${\hat{x}}_{2|2}=\left[\begin{array}{c}2.2\\ 1.0645\end{array}\right]+\left[\begin{array}{c}0.6875\\ 0.3438\end{array}\right](-0.2)=\left[\begin{array}{c}2.0625\\ 0.9958\end{array}\right]$$

物理意义：

观测位置 $2.0$ 比预测位置 $2.2$ 小，所以位置和速度都被下调。

---

6.3 $k=3$

预测状态：

$${\hat{x}}_{3|2}=F{\hat{x}}_{2|2}=\left[\begin{array}{c}3.0583\\ 0.9958\end{array}\right]$$

预测协方差：

$$P_{3|2}=\left[\begin{array}{cc}1.9743& 0.8430\\ 0.8430& 0.5993\end{array}\right]$$

残差：

$$y_3-H{\hat{x}}_{3|2}=2.9-3.0583=-0.1583$$

Kalman 增益：

$$K_3=\left[\begin{array}{c}1.9743\\ 0.8430\end{array}\right](1.9743+1{)}^{-1}=\left[\begin{array}{c}0.6638\\ 0.2834\end{array}\right]$$

状态更新：

$${\hat{x}}_{3|3}=\left[\begin{array}{c}3.0583\\ 0.9958\end{array}\right]+\left[\begin{array}{c}0.6638\\ 0.2834\end{array}\right](-0.1583)=\left[\begin{array}{c}2.9532\\ 0.9509\end{array}\right]$$

---

7. 前向滤波结果

时刻	滤波位置	滤波速度
1	1.1355	1.0645
2	2.0625	0.9958
3	2.9532	0.9509

这些是：

$${\hat{x}}_{k|k}=E[x_k\mid y_{1:k}]$$

只用了当前及过去观测。

---

8. 后向 RTS Smoother 计算

初始化：

$${\hat{x}}_{3|T}={\hat{x}}_{3|3}=\left[\begin{array}{c}2.9532\\ 0.9509\end{array}\right]$$

---

8.1 从 $k=2$ 回推

RTS 平滑增益：

$$G_2=P_{2|2}{F}^{\top }{P}_{3|2}^{-1}$$

代入数值后：

$$G_2=\left[\begin{array}{cc}0.6947& -0.4037\\ 0.1784& 0.5821\end{array}\right]$$

未来修正量：

$${\hat{x}}_{3|T}-{\hat{x}}_{3|2}=\left[\begin{array}{c}2.9532\\ 0.9509\end{array}\right]-\left[\begin{array}{c}3.0583\\ 0.9958\end{array}\right]=\left[\begin{array}{c}-0.1051\\ -0.0449\end{array}\right]$$

这说明：

站在全局角度看，第 3 步原来的预测位置和速度都偏高。

反向修正：

$$G_2\left({\hat{x}}_{3|T}-{\hat{x}}_{3|2}\right)\approx \left[\begin{array}{c}-0.0549\\ -0.0449\end{array}\right]$$

所以：

$${\hat{x}}_{2|T}={\hat{x}}_{2|2}+G_2\left({\hat{x}}_{3|T}-{\hat{x}}_{3|2}\right)=\left[\begin{array}{c}2.0625\\ 0.9958\end{array}\right]+\left[\begin{array}{c}-0.0549\\ -0.0449\end{array}\right]=\left[\begin{array}{c}2.0076\\ 0.9509\end{array}\right]$$

---

8.2 从 $k=1$ 回推

RTS 平滑增益：

$$G_1=P_{1|1}{F}^{\top }{P}_{2|1}^{-1}$$

数值结果：

$$G_1=\left[\begin{array}{cc}0.7255& -0.5419\\ 0.1527& 0.6946\end{array}\right]$$

未来修正量：

$${\hat{x}}_{2|T}-{\hat{x}}_{2|1}=\left[\begin{array}{c}2.0076\\ 0.9509\end{array}\right]-\left[\begin{array}{c}2.2000\\ 1.0645\end{array}\right]=\left[\begin{array}{c}-0.1924\\ -0.1136\end{array}\right]$$

反向修正：

$$G_1\left({\hat{x}}_{2|T}-{\hat{x}}_{2|1}\right)\approx \left[\begin{array}{c}-0.0780\\ -0.1083\end{array}\right]$$

所以：

$${\hat{x}}_{1|T}={\hat{x}}_{1|1}+G_1\left({\hat{x}}_{2|T}-{\hat{x}}_{2|1}\right)=\left[\begin{array}{c}1.1355\\ 1.0645\end{array}\right]+\left[\begin{array}{c}-0.0780\\ -0.1083\end{array}\right]=\left[\begin{array}{c}1.0575\\ 0.9562\end{array}\right]$$

---

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$$

整体看速度接近：

$$\frac{2.9-1.2}{2}=0.85$$

前向滤波在第 1 步时只看到 $y_1=1.2$，因此速度估计偏高：

$$v_{1|1}=1.0645$$

但 RTS smoother 看到完整轨迹后，把它修正为：

$$v_{1|T}=0.9562$$

---

10. RTS Smoother 的物理意义总结

Kalman filter 是“边走边估计”：

$${\hat{x}}_{k|k}=E[x_k\mid y_{1:k}]$$

它只能利用过去和当前信息。

RTS smoother 是“事后复盘”：

$${\hat{x}}_{k|T}=E[x_k\mid y_{1:T}]$$

它利用了未来信息。

在匀速运动模型中，未来位置观测不仅能修正过去位置，也能修正过去速度。

RTS smoother 的三条核心公式：

$$G_k=P_{k|k}{F}^{\top }{P}_{k+1|k}^{-1}$$

$${\hat{x}}_{k|T}={\hat{x}}_{k|k}+G_k\left({\hat{x}}_{k+1|T}-{\hat{x}}_{k+1|k}\right)$$

$$P_{k|T}=P_{k|k}+G_k\left(P_{k+1|T}-P_{k+1|k}\right)G_k^{\top }$$

可以理解为：

$$\boxed{\text{RTS smoother}=\text{把未来状态的修正量，按照运动模型和状态相关性，反向分配给过去状态。}}$$

这就是它在轨迹后处理、GPS 平滑、机器人定位和目标跟踪中非常有用的原因。