论文阅读-Fast and Adaptive Perception and Planning for UAVs in Dynamic Cluttered Environments

“FAPP: Fast and Adaptive Perception and Planning for UAVs in Dynamic Cluttered Environments” (Lu 等, 2025, p. 1) (pdf)

动态环境感知

FAPP 采用几何聚类+运动估计结合的点云分割方法，既不依赖GPU，也能区分快速动态与静态目标

1. 把点云从传感器局部坐标系 BBB 变换到全局坐标系 WWW
2. 建立一个增量 KD 树（I-KD Tree）来维护最近 FFF 帧的点集合 ξ\xiξ,点集合通过增删操作动态更新
   1. 即 ξ⊆{WPj}j∈[k−F,k−1]\xi \subseteq \{ W P_j \}_{j\in[k-F,k-1]}ξ⊆{WPj​}j∈[k−F,k−1]​。使用 I-KD Tree 可以使 ξ\xiξ 保持合适的大小并在 3D 空间上较均匀分布，从而较好地表示最近几帧扫描得到的障碍物空间分布信息。
   2. ❓为什么要让点在 ξ\xiξ 中“均匀分布”?❓
      1. 如果历史点云过密（比如一个静止墙在原地被重复扫描 100 次），则 KD 树查询的距离几乎为 0，会掩盖其他动态物体；
      2. 如果过稀，则动态检测不稳定。
   3. 为什么用KD tree
      • KD Tree 是一种空间索引结构，用来快速做最近邻搜索
        • 查询时：给一个点 p，快速找到 ξ 中离 p 最近的点
      • I-KD Tree增量式 KD 树，当新的帧点云到来时，不是**重新建树，**而是“增量更新”已有的 KD 树，把新点插入，保留了“历史点云的结构信息”，可以用来判断一个点是不是新出现的、或者物体是否在移动。
   4. ❓为什么要保存多帧（而不是只上一帧）？
      1. 保存多帧可以让“静态结构”在时间上积累密度，而“动态物体”会因为移动而形成稀疏或拖尾现象。
   5. ❓KD Tree 中的内容，注意不是ξ ?
      1. ξ 中包含多个时间戳的点云（混在一起的），而不是分开的
      2. 这几个帧的点云如果合在一起形成稠密点簇,KD Tree 查询的最近距离 dn≈0d_n ≈ 0dn​≈0，说明“它在过去多帧都出现过 → 静态”
3. 区分动态/静态点云:先对当前帧点云 WPkW P_kWPk​ 使用 DBSCAN 聚类,得到 mmm 个簇 Ck={C1,…,Cm}C_k=\{C_1,\dots,C_m\}Ck​={C1​,…,Cm​}。对每个簇，可分为三类,（以阈值 h1,h2h_1,h_2h1​,h2​ 判定）5. 参数选择
   1. Case 1（持续移动物体）；移动：T1>h1T_1 > h_1T1​>h1​ 且 T2<h2T_2 < h_2T2​<h2​。
      1. 若一个簇是真正移动的物体，其当前点到历史点云的最近距离较大（大 T1T_1T1​），并且由于它自己在每一帧上移动的投影相对一致，所以 dnd_ndn​ 的分布较均匀（小 T2T_2T2​）。因此当 T1T_1T1​ 大且 T2T_2T2​ 小时判断为连续移动（Case 1）。
   2. Case 2（静态物体）；静态：T1<h1T_1 < h_1T1​<h1​。
      1. 若一个簇曾在历史帧中出现且未移动，其点到历史点云的最近距离应在传感器测量误差范围内（小 T1T_1T1​）
   3. Case 3（未知物体）（包括：之前被移动物体遮挡的静态物体，或新进入视野的物体）。未知：T1>h1T_1 > h_1T1​>h1​ 且 T2>h2T_2 > h_2T2​>h2​。
      1. 若是新出现或被遮挡的静态物体（刚从遮挡区显现或新进入视野），它的点到历史点的距离 dnd_ndn​ 会不均匀（有些点可能很近，有些点很远），因此 T2T_2T2​ 较大，归为 未知（Case 3）。
1. ε\varepsilonε（邻域半径）与 min_samples 决定聚类颗粒度[DBSCAN的]
2. h1​：可设为与传感器噪声/定位误差相关的倍数（例如噪声标准差的 2–3 倍），如果太小，会把移动物体误判为静态；太大，会把静态物体误判为移动
3. h2​：判断分布是否“均匀”的阈值，经验值需要在数据集上交叉验证。
4. 当 T1T_1T1​ 很小（接近 0）时 T2T_2T2​ 的定义会有数值问题，建议用 T12+ϵT_1^2 + \epsilonT12​+ϵ 做分母保护
4. 动态目标跟踪
   1. 用离散线性系统与卡尔曼滤波器（KF）估计，如果某簇是新出现的，则新建一个 KF，若某簇能与已有第 iii 个 tracker 关联，则把该簇的几何中心与帧差（中心的位移）作为测量向量 ZiZ_iZi​ 进入 KF 更新。卡尔曼滤波器会实时给出估计状态 X^i\hat X_iX^i​

对簇中的每个点 pnp_npn​（簇内共有 NNN 个点），计算其到历史点集 ξ\xiξ 的全局最近距离 dnd_ndn​。定义两个统计量：平均最近距离 T1T_1T1​（T1T_1T1​ 表征该簇点到历史点云的平均最小距离）归一化的方差 T2T_2T2​（ 表征 dnd_ndn​ 的归一化方差（用 T12T_1^2T12​ 归一化）:衡量相对离散程度。）GPT建议：若 T1T_1T1​ 很小，分母会很小，要注意数值稳定性（可在实现时加入一个小常数 ϵ\epsilonϵT1=1N∑n=1Ndn,(1)T_1 = \frac{1}{N}\sum_{n=1}^{N} d_n, \tag{1}T1​=N1​n=1∑N​dn​,(1)T2=1N∑n=1N(dn−T1)2T12.(2)T_2 = \frac{1}{N}\sum_{n=1}^{N}\frac{(d_n-T_1)^2}{T_1^2}. \tag{2}T2​=N1​n=1∑N​T12​(dn​−T1​)2​.(2)

Xi=[xi,yi,zi,x˙i,y˙i,z˙i]TX_i = [x_i, y_i, z_i, \dot{x}_i, \dot{y}_i, \dot{z}_i]^TXi​=[xi​,yi​,zi​,x˙i​,y˙​i​,z˙i​]T。系统模型：状态转移矩阵 AkA_kAk​ 与测量矩阵 HkH_kHk​ ，过程噪声与测量噪声假定为零均值高斯噪声，协方差分别为 QQQ 与 RRR。

Xik+1=AkXik+νi,(4)X^ {k+1}_i = A_k X^k_i + \nu_i, \tag{4}Xik+1​=Ak​Xik​+νi​,(4)

Zik=HkXik+ωi,(5)Z^k_i = H_k X^k_i + \omega_i, \tag{5}Zik​=Hk​Xik​+ωi​,(5)

Ak=[I3×3Δt⋅I3×303×3I3×3],Hk=[I3×303×303×3I3×3].(6)A_k = \begin{bmatrix} I_{3\times3} & \Delta t\cdot I_{3\times3} \\ 0_{3\times3} & I_{3\times3} \end{bmatrix},\quad H_k = \begin{bmatrix} I_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & I_{3\times3} \end{bmatrix}. \tag{6}Ak​=[I3×3​03×3​​Δt⋅I3×3​I3×3​​],Hk​=[I3×3​03×3​​03×3​I3×3​​].(6)

Process noise ν∼N(0,Q)\nu\sim\mathcal{N}(0,Q)ν∼N(0,Q), measurement noise ω∼N(0,R)\omega\sim\mathcal{N}(0,R)ω∼N(0,R). 若簇为新观测，创建新 KF；若能关联到第 iii 个 tracker，测量 Zi=[oix,oiy,oiz,Δoix,Δoiy,Δoiz]TZ_i=[o_{ix},o_{iy},o_{iz},\Delta o_{ix},\Delta o_{iy},\Delta o_{iz}]^TZi​=[oix​,oiy​,oiz​,Δoix​,Δoiy​,Δoiz​]T。

含义：oi=(oix,oiy,oiz)o_i = (o_{ix}, o_{iy}, o_{iz})oi​=(oix​,oiy​,oiz​)：当前帧的检测（点云聚类）中心；Δoi=oi(t)−oi(t−1)\Delta o_i = o_i^{(t)} - o_i^{(t-1)}Δoi​=oi(t)​−oi(t−1)​：相邻帧簇中心差分，近似“速度观测”。

其中 Δo\Delta oΔo 是簇中心在相邻帧间的差分（近似瞬时速度的观测）。这种做法把帧间差分作为“速度测量”输入 KF，有助于更快收敛。

AkA_kAk​：状态转移矩阵这对应连续运动方程的欧拉离散化：

{pk+1=pk+Δt vk+noisevk+1=vk+noise\begin{cases} \mathbf{p}_{k+1} = \mathbf{p}_k + \Delta t\, \mathbf{v}_k + \text{noise}\\ \mathbf{v}_{k+1} = \mathbf{v}_k + \text{noise} \end{cases}{pk+1​=pk​+Δtvk​+noisevk+1​=vk​+noise​

HkH_kHk​​： 显示观测是位置和速度两部分（例如通过中心与中心位移估计速度作为观测），所以是单位块对角矩阵

Q：过程噪声，反映模型不完美（如加速度变化），若场景中物体常以非恒速运动，需增大 QQQ，或采用自适应策略

R：测量噪声，与点云噪声、聚类中心误差相关。簇越小、遮挡越多，测量噪声越大。

初始 Q,RQ,RQ,R 可基于传感器噪声和目标运动幅度估计，再用在线协方差自适应

KF 更新：
计算残差 y=Z−HX^predy = Z - H \hat X_{pred}y=Z−HX^pred​，卡尔曼增益

K=PpredHT(HPpredHT+R)−1K = P_{pred} H^T (H P_{pred} H^T + R)^{-1}K=Ppred​HT(HPpred​HT+R)−1

然后更新

X^=X^pred+Ky,P=(I−KH)Ppred\hat X = \hat X_{pred} + K y,\quad P = (I-KH)P_{pred}X^=X^pred​+Ky,P=(I−KH)Ppred​

5. 数据关联

当你有多个检测（D 个障碍物）和多个 tracker（I 个跟踪目标），要决定：当你有多个检测（D 个障碍物）和多个 tracker（I 个跟踪目标），要决定： 

(1) 马氏距离（Mahalanobis distance）：

Ωd,i=(od−o^i)Tσi−1(od−o^i)\Omega_{d,i} = (o_d - \hat{o}_i)^T \sigma_i^{-1} (o_d - \hat{o}_i)Ωd,i​=(od​−o^i​)Tσi−1​(od​−o^i​)

含义：odo_dod​：检测的中心；o^i\hat{o}_io^i​：第 i 个跟踪器预测的中心；σi\sigma_iσi​：KF 的预测协方差（6×6）；Ωd,i\Omega_{d,i}Ωd,i​：越小越接近，越有可能是同一物体。

(2) 转换成置信指标：

Hd,i=1−2πarctan⁡(Ωd,i)H_{d,i} = 1 - \frac{2}{\pi} \arctan(\Omega_{d,i})Hd,i​=1−π2​arctan(Ωd,i​)

这个是一个经验函数，把距离映射到 (0,1) 区间：Ωd,i→0\Omega_{d,i} \to 0Ωd,i​→0 → Hd,i→1H_{d,i} \to 1Hd,i​→1（强关联）Ωd,i→∞\Omega_{d,i} \to \inftyΩd,i​→∞ → Hd,i→0H_{d,i} \to 0Hd,i​→0（弱关联）

构建 D×I 的代价矩阵，然后用 匈牙利算法（Hungarian Algorithm） 寻找最优匹配组合；
若匹配分数 Hd,iH_{d,i}Hd,i​ 小于阈值 thminth_{min}thmin​，则拒绝匹配

6. 静态局部地图输出

把当前帧的动态点集合 $W P_{dyn,k}$ 从当前点云 $W P_k$ 中移除，得到静态点云 $W P_{sta,k}$：$W P_{sta,k} = W P_k - W P_{dyn,k}$。静态点云包含了有关自由空间/占用空间的完整信息，随后用它来构建占用栅格地图，用于避障规划。 

每帧 k:获取局部点云 BPkB_{Pk}BPk​ 与 poseBTkWpose ^W_{B_Tk}poseBT​kW​ -> 变换为全局 WPkW_{Pk}WPk​更新 I-KD-Tree: 插入 WPkW_{Pk}WPk​、删除过期帧对 WPkW_{Pk}WPk​ 运行 DBSCAN -> 得到簇集合 {C1…Cm}欧式聚类（Euclidean Cluster Extraction） 将点云分为若干簇ξ={Ck}k=1M,Ck={pik}\xi = \{C_k\}_{k=1}^M, \quad C_k = \{p_i^k\}ξ={Ck​}k=1M​,Ck​={pik​}即每个簇 CkC_kCk​ 表示一个物体。计算每个簇的几何中心定义为：ok=1∣Ck∣∑pi∈Ckpio_k = \frac{1}{|C_k|} \sum_{p_i \in C_k} p_iok​=∣Ck​∣1​pi​∈Ck​∑​pi​这个点就是该簇点云的“平均坐标”簇匹配（确定“同一物体”）对于每个当前簇 oko_kok​，在上一帧所有簇的中心集合 {oiprev}\{o_i^{prev}\}{oiprev​} 中找最近邻：omatch=arg⁡min⁡oiprev∥ok−oiprev∥o_{match} = \arg\min_{o_i^{prev}} \| o_k - o_i^{prev} \|omatch​=argoiprev​min​∥ok​−oiprev​∥若最近距离小于某阈值（例如 1m），则认为它们是同一簇对每个簇 C:4.1 对簇中每个点 pnp_npn​ 查询 I-KD-Tree 最近距离 dnd_ndn​4.2 计算 T1=mean(dn),T2=mean((dn−T1)2)/(T12+eps)T1 = mean(d_n), T2 = mean((d_n-T1)^2) / (T1^2 + eps)T1=mean(dn​),T2=mean((dn​−T1)2)/(T12+eps)4.3 根据 (T1,h1) 与 (T2,h2) 分类为 Static / Moving / Unknown动态簇集合 WPdyn,kW_{Pdyn,k}WPdyn,k​ = union of Moving clusters静态点云 WPsta,k=WPk WPdyn,kW_{Psta,k} = W_{Pk} \ W_{Pdyn,k}WPsta,k​=WPk​ WPdyn,k​ (对于 Unknown，按策略决定是否加入)对每个 Moving cluster:7.1 如果可与某 tracker 匹配（计算 Ω, H），用匈牙利分配7.2 对匹配 tracker: 用测量 Z 更新该 tracker 的 KF7.3 对未匹配检测: 新建 tracker（初始协方差设大）用 WPsta,kW_{Psta,k}WPsta,k​ 更新占用栅格地图（或体素地图）输出: 静态局部地图 + 所有 tracker 的状态 Xhati{Xhat_i}Xhati​

自适应估计与预测

当目标突然加速或转弯时：模型 AAA（比如匀速模型）已经不再准确；卡尔曼滤波的预测会“滞后”；滤波器的**创新（innovation）**变大；最终预测偏差变大、甚至失效自适应过程噪声协方差 Qₖ 调整,不再固定 Qₖ，而是根据“创新项” γₖ 的统计特性，自动调整它

对一个线性卡尔曼滤波器，预测出的观测是 Z^k∣k−1=HkX^k∣k−1\hat Z_{k|k-1} = H_k \hat X_{k|k-1}Z^k∣k−1​=Hk​X^k∣k−1​。

1. 实际测量是 ZkZ_kZk​。
2. 理想情况下（模型与噪声假设成立），创新应该是零均值、协方差为 Sk=HkPk∣k−1HkT+RkS_k = H_k P_{k|k-1} H_k^T + R_kSk​=Hk​Pk∣k−1​HkT​+Rk​。
3. 若模型不匹配，我们可以检测到模型不匹配，并据此调整模型不确定性
4. 理论上：创新的理论协方差 SkS_kSk​ 与经验协方差 Cγ,kC_{\gamma,k}Cγ,k​ 应该一致（当滤波器稳定且样本充足时）。
5. 通过让理论协方差等于经验协方差，我们可以反解出模型中未知或需要调整的项（这里是 QQQ）。这就是“协方差匹配”
6. 推导（关键步骤）：Sk=Cov⁡(γk)=HkPk∣k−1HkT+RkPk∣k−1=Ak−1Pk−1∣k−1Ak−1T+Qk−1\begin{aligned} S_k &= \operatorname{Cov}(\gamma_k) = H_k P_{k|k-1} H_k^T + R_k \\ P_{k|k-1} &= A_{k-1} P_{k-1|k-1} A_{k-1}^T + Q_{k-1} \end{aligned}Sk​Pk∣k−1​​=Cov(γk​)=Hk​Pk∣k−1​HkT​+Rk​=Ak−1​Pk−1∣k−1​Ak−1T​+Qk−1​​代入：Sk=Hk(Ak−1Pk−1∣k−1Ak−1T+Qk−1)HkT+Rk.S_k = H_k (A_{k-1} P_{k-1|k-1} A_{k-1}^T + Q_{k-1}) H_k^T + R_k.Sk​=Hk​(Ak−1​Pk−1∣k−1​Ak−1T​+Qk−1​)HkT​+Rk​.我们用经验值 Cγ,kC_{\gamma,k}Cγ,k​ 近似 SkS_kSk​，得到：HkQk−1HkT≈Cγ,k−HkAk−1Pk−1∣k−1Ak−1THkT−Rk.H_k Q_{k-1} H_k^T \approx C_{\gamma,k} - H_k A_{k-1} P_{k-1|k-1} A_{k-1}^T H_k^T - R_k.Hk​Qk−1​HkT​≈Cγ,k​−Hk​Ak−1​Pk−1∣k−1​Ak−1T​HkT​−Rk​.如果 HkH_kHk​ 可逆或为单位矩阵块，则可以进一步求出 Qk−1Q_{k-1}Qk−1​ 或其对角近似

创新向量预测与观测之间的差距：γk=Zk−Z^k∣k−1\gamma_k = Z_k - \hat Z_{k|k-1}γk​=Zk​−Z^k∣k−1​。差距：γk=zk−Hkx^k∣k−1\gamma_k = z_k - H_k \hat{x}_{k|k-1}γk​=zk​−Hk​x^k∣k−1​

其中：x^k∣k−1\hat{x}_{k|k-1}x^k∣k−1​：基于上一次状态预测的当前状态（先验估计）zkz_kzk​：实际观测γk\gamma_kγk​：创新（代表预测误差）

参数：滑动窗口长度 WWW、上下界 Qmin⁡,Qmax⁡Q_{\min}, Q_{\max}Qmin​,Qmax​、小正数 ϵ\epsilonϵ。

每个时间步 kkk：卡尔曼预测（标准 KF）：X^k∣k−1=Ak−1X^k−1∣k−1\hat X_{k|k-1} = A_{k-1} \hat X_{k-1|k-1}X^k∣k−1​=Ak−1​X^k−1∣k−1​Pk∣k−1=Ak−1Pk−1∣k−1Ak−1T+Qk−1P_{k|k-1} = A_{k-1} P_{k-1|k-1} A_{k-1}^T + Q_{k-1}Pk∣k−1​=Ak−1​Pk−1∣k−1​Ak−1T​+Qk−1​计算创新：γk=Zk−HkX^k∣k−1\gamma_k = Z_k - H_k \hat X_{k|k-1}γk​=Zk​−Hk​X^k∣k−1​把 γk\gamma_kγk​ 存进滑动窗口（长度 W），并计算经验创新协方差：这个矩阵反映了最近几帧的预测误差的波动情况。Cγ,k=1W∑l=k−W+1kγlγlTC_{\gamma,k} = \frac{1}{W}\sum_{l=k-W+1}^{k} \gamma_l \gamma_l^TCγ,k​=W1​∑l=k−W+1k​γl​γlT​计算理论项：T=HkAk−1Pk−1∣k−1Ak−1THkT+RkT = H_k A_{k-1} P_{k-1|k-1} A_{k-1}^T H_k^T + R_kT=Hk​Ak−1​Pk−1∣k−1​Ak−1T​HkT​+Rk​ （注意含 RkR_kRk​）目标：令 HkQHkT≈Cγ,k−TH_k Q H_k^T \approx C_{\gamma,k} - THk​QHkT​≈Cγ,k​−T.在理想情况下，创新的理论协方差应该是：Sk=HkPk∣k−1HkT+RkS_k = H_k P_{k|k-1} H_k^T + R_kSk​=Hk​Pk∣k−1​HkT​+Rk​而预测协方差：Pk∣k−1=Ak−1Pk−1∣k−1Ak−1T+QkP_{k|k-1} = A_{k-1} P_{k-1|k-1} A_{k-1}^T + Q_kPk∣k−1​=Ak−1​Pk−1∣k−1​Ak−1T​+Qk​求解 QQQ 的估计：如果滤波正常工作，应有：Cγ,k≈SkC_{\gamma,k} \approx S_kCγ,k​≈Sk​因此我们可以反推出一个“自适应的 Qₖ”：HkQkHkT=Cγ,k−HkAk−1Pk−1∣k−1Ak−1THkT−RkH_k Q_k H_k^T = C_{\gamma,k} - H_k A_{k-1} P_{k-1|k-1} A_{k-1}^T H_k^T - R_kHk​Qk​HkT​=Cγ,k​−Hk​Ak−1​Pk−1∣k−1​Ak−1T​HkT​−Rk​化简得：Qk=Cγ,k−Ak−1Pk−1∣k−1Ak−1T−RkQ_k = C_{\gamma,k} - A_{k-1} P_{k-1|k-1} A_{k-1}^T - R_kQk​=Cγ,k​−Ak−1​Pk−1∣k−1​Ak−1T​−Rk​这就是论文的式 (13)保证正定与数值稳定：取对角： Qe=diag⁡(Q^k)Q_{e} = \operatorname{diag}(\hat Q_k)Qe​=diag(Q^​k​)逐元素截断： Qk(i,i)=max⁡(0,Qe(i,i))Q_k(i,i) = \max(0, Q_e(i,i))Qk​(i,i)=max(0,Qe​(i,i))可进一步限制范围： Qk(i,i)=clip⁡(Qk(i,i),Qmin⁡,Qmax⁡)Q_k(i,i) = \operatorname{clip}(Q_k(i,i), Q_{\min}, Q_{\max})Qk​(i,i)=clip(Qk​(i,i),Qmin​,Qmax​)用新 QkQ_kQk​ 继续 KF 更新（或下次预测）：按需立即进行一次 KF 更新：计算 Kk,Pk,X^k∣kK_k,P_k,\hat X_{k|k}Kk​,Pk​,X^k∣k​ 等Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}Kk​=Pk∣k−1​HkT​(Hk​Pk∣k−1​HkT​+Rk​)−1
x^k∣k=x^k∣k−1+Kkγk\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k \gamma_kx^k∣k​=x^k∣k−1​+Kk​γk​
Pk∣k=(I−KkHk)Pk∣k−1P_{k|k} = (I - K_k H_k) P_{k|k-1}Pk∣k​=(I−Kk​Hk​)Pk∣k−1​

动态碰撞代价（对 III 个被跟踪目标求和）：

Gd=∑i=1Imax⁡{(Did−∥pl(tτ)−pib(tτ)∥2), 0}3,(25)G_d = \sum_{i=1}^I \max\big\{(D^d_i - \| p_l(t_\tau) - p^b_i(t_\tau)\|^2),\,0\big\}^3, \tag{25}Gd​=i=1∑I​max{(Did​−∥pl​(tτ​)−pib​(tτ​)∥2),0}3,(25)

其中

Did=r0+ri+eiD^d_i = r_0 + r_i + e_iDid​=r0​+ri​+ei​

是机器人与第 iii 个目标的安全间隔，rir_iri​ 为目标半径，eie_iei​ 表示位置不确定性导致的额外缓冲，定义为

定义为：

ei=σiτ(1,1)2+σiτ(2,2)2+σiτ(3,3)2.(26)e_i = \sqrt{ \sigma_{i\tau}(1,1)^2 + \sigma_{i\tau}(2,2)^2 + \sigma_{i\tau}(3,3)^2 }. \tag{26}ei​=σiτ​(1,1)2+σiτ​(2,2)2+σiτ​(3,3)2​.(26)

这里 σiτ=AτσkiAτT\sigma_{i\tau} = A_\tau \sigma^i_k A_\tau^Tσiτ​=Aτ​σki​AτT​ 是第 iii 个目标的协方差从时间 tkt_ktk​ 推到 tτt_\tautτ​ 的传播（与 III 的卡尔曼滤波协方差传播相同），其中 AτA_\tauAτ​ 是状态转移矩阵，使用 (tτ−tk)(t_\tau - t_k)(tτ​−tk​) 作为时间间隔（而不是滤波器内 Δt\Delta tΔt 标准值）