卡尔曼滤波轨迹预测过程源码bytetrack/kalman_filter.py 的predict单条轨迹和multi_predictN 条轨迹批量。本文按代码顺序、用实际数字逐步推算两个函数最后对比。公式与符号总表符号代码里的真身是什么从哪来mean参数mean状态均值8 维轨迹上一帧的 meaninitiate 建档时初始化covP参数covariance状态协方差8×8对角线是各分量的不确定度轨迹上一帧的 covarianceFself._motion_mat状态转移矩阵8×8匀速模型__init__构造单位阵右上 4×4 填 dt1Q局部变量motion_cov过程噪声协方差8×8 对角阵“匀速假设本身的误差预算”predict 内部用两个噪声权重 × mean[3] 现算hmean[3]这条轨迹的框高像素不是固定值每条轨迹各自取状态向量第 4 个分量_std_weight_position类属性位置噪声权重 1/20__init__写死_std_weight_velocity类属性速度噪声权重 1/160__init__写死mean_new返回的mean外推后的状态均值8 维mean · Fᵀ 算出cov_newP’返回的covariance外推后的协方差不确定性变大F · cov · Fᵀ Q 算出两个函数的数学公式完全相同mean_new mean · Fᵀ 位置 位置 速度×1 帧速度不变 cov_new F · cov · Fᵀ Q 不确定性传播变大一、predict单条轨迹实际推算输入举例mean [100, 50, 1.5, 40, 8, 0, 0, 0] ← x100, y50, a1.5, h40, vx8, vy0, va0, vh0 cov [[16, 0, 0, 0, 0, 0, 0, 0], [0, 18, 0, 0, 0, 0, 0, 0], [0, 0, 0.5, 0, 0, 0, 0, 0], [0, 0, 0, 14, 0, 0, 0, 0], [0, 0, 0, 0, 6, 0, 0, 0], [0, 0, 0, 0, 0, 5, 0, 0], [0, 0, 0, 0, 0, 0, 0.3, 0], [0, 0, 0, 0, 0, 0, 0, 7]] ← 8×8 对角阵演示值各分量方差不同、交叉项 0第 1 步构造 std_pos位置类 4 个分量的噪声标准差std_pos[self._std_weight_position*mean[3],# x 的噪声 h/20self._std_weight_position*mean[3],# y 的噪声 h/201e-2,# a 的噪声 固定 0.01比例无量纲不随 h 缩放self._std_weight_position*mean[3]]# h 的噪声 h/20代入 h mean[3] 40std_pos [40/20, 40/20, 0.01, 40/20] [2, 2, 0.01, 2]第 2 步构造 std_vel速度类 4 个分量的噪声标准差std_vel[self._std_weight_velocity*mean[3],# vx 的噪声 h/160self._std_weight_velocity*mean[3],# vy 的噪声 h/1601e-5,# va 的噪声 固定 0.00001self._std_weight_velocity*mean[3]]# vh 的噪声 h/160代入 h 40std_vel [40/160, 40/160, 0.00001, 40/160] [0.25, 0.25, 0.00001, 0.25]第 3 步拼成 Q 矩阵motion_covmotion_covnp.diag(np.square(np.r_[std_pos,std_vel]))# np.r_ 把两个 4 元素列表拼成 8 个顺序 状态分量顺序 x,y,a,h,vx,vy,va,vh# np.square 标准差 → 方差平方# np.diag 放到 8×8 对角线上Q diag(2², 2², 0.01², 2², 0.25², 0.25², 0.00001², 0.25²) diag(4, 4, 0.0001, 4, 0.0625, 0.0625, 0.0000000001, 0.0625)第 4 步外推 meanmeannp.dot(mean,self._motion_mat.T)# mean · FᵀF_motion_mat__init__构造右上角的 1 使位置 速度×dtx 100 8 × 1 108 dt 1__init__ 里 ndim, dt 4, 1. 写死 y 50 0 × 1 50 a 1.5 0 × 1 1.5 h 40 0 × 1 40 vx 8, vy 0, va 0, vh 0 速度不变 mean_new [108, 50, 1.5, 40, 8, 0, 0, 0]第 5 步外推 covcovariancenp.linalg.multi_dot((self._motion_mat,covariance,self._motion_mat.T))motion_cov# F · cov · Fᵀ Qcov 是对角阵时结果规律简单新对角线 旧对角线(自己) 旧对角线(对应速度分量) Q(自己)位置分量与自己的速度分量之间产生交叉项 旧速度方差cov_newx 行: 16 6 4 26 交叉项 [x, vx] 6 cov_newy 行: 18 5 4 27 交叉项 [y, vy] 5 cov_newa 行: 0.5 0.3 0.0001 ≈ 0.8交叉项 [a, va] 0.3 cov_newh 行: 14 7 4 25 交叉项 [h, vh] 7 cov_new速度行: vx 60.06256.06vy 5.06va 0.3vh 7.06输出返回mean_new [108, 50, 1.5, 40, 8, 0, 0, 0] cov_new [[26, 0, 0, 0, 6, 0, 0, 0 ], [0, 27, 0, 0, 0, 5, 0, 0 ], [0, 0, 0.8, 0, 0, 0, 0.3, 0 ], [0, 0, 0, 25, 0, 0, 0, 7 ], [6, 0, 0, 0, 6.06, 0, 0, 0 ], [0, 5, 0, 0, 0, 5.06,0, 0 ], [0, 0, 0.3, 0, 0, 0, 0.3, 0 ], [0, 0, 0, 7, 0, 0, 0, 7.06]]位置方差变大16→26、18→27、14→25外推后更不确定速度方差基本不变x-vx、y-vy、a-va、h-vh 之间出现交叉项位置的不确定性里掺进了速度的不确定性。二、multi_predictN 条轨迹批量实际推算输入举例 N2mean [[100, 50, 1.5, 40, 8, 0, 0, 0], ← 轨迹1就是上面的例子 [200, 300, 0.8, 60, -6, 10, 0, 0]] ← 轨迹2h60向左下移动 cov [轨迹1 的 8×8同上, 轨迹2 的 8×8 对角阵演示值[20, 22, 0.6, 18, 8, 9, 0.4, 10]]第 1 步std_pos注意下标变了std_pos[self._std_weight_position*mean[:,3],# x 的噪声 h/20N 个 h 一起算self._std_weight_position*mean[:,3],1e-2*np.ones_like(mean[:,3]),# a 的噪声 0.01 × 全1向量 每条都是 0.01self._std_weight_position*mean[:,3]]mean[:, 3]是列——N 条轨迹各自的 h[40, 60]。代入std_pos [[40/20, 60/20], [40/20, 60/20], [0.01, 0.01], [40/20, 60/20]] [[2, 3], [2, 3], [0.01, 0.01], [2, 3]]每行一个分量、每列一条轨迹ones_like只是广播手段a 的数值仍是 0.01第 2 步std_velstd_vel [[40/160, 60/160], ..., [1e-5, 1e-5], ...] [[0.25, 0.375], [0.25, 0.375], [0.00001, 0.00001], [0.25, 0.375]]第 3 步拼 Q每条轨迹一份sqrnp.square(np.r_[std_pos,std_vel]).T# 8×N每列一条轨迹的 8 个方差motion_cov[]foriinrange(len(mean)):motion_cov.append(np.diag(sqr[i]))# 逐条造 8×8Q₁ diag(4, 4, 0.0001, 4, 0.0625, 0.0625, ~0, 0.0625) ← 轨迹1h40 Q₂ diag(9, 9, 0.0001, 9, 0.14, 0.14, ~0, 0.14) ← 轨迹2h60噪声更大第 4 步外推 mean一次矩阵乘全部完成meannp.dot(mean,self._motion_mat.T)轨迹1: [100,50,1.5,40,8,0,0,0] → [108, 50, 1.5, 40, 8, 0, 0, 0] ← 1008 轨迹2: [200,300,0.8,60,-6,10,0,0] → [194, 310, 0.8, 60, -6, 10, 0, 0] ← 200-6, 30010第 5 步外推 cov三维数组批量乘leftnp.dot(self._motion_mat,covariance).transpose((1,0,2))covariancenp.dot(left,self._motion_mat.T)motion_cov数学同 predict 的 F·cov·FᵀQ只是对 N 个 8×8 同时做规律新对角线 自己 对应速度分量 Q轨迹1 cov_new 对角线: [26, 27, 0.8, 25, 6.06, 5.06, 0.3, 7.06]同 predict 的输出 轨迹2 cov_new 对角线: [208937, 229940, 0.60.40.0001≈1, 1810937, 8.14, 9.14, 0.4, 10.14]输出返回mean_new2×8 [[108, 50, 1.5, 40, 8, 0, 0, 0], [194, 310, 0.8, 60, -6, 10, 0, 0]] cov_new2×8×8 轨迹1、轨迹2 各一份 8×8对角线见上三、两函数对比方法作用输入输出predict预测步按匀速模型把状态外推到下一帧mean: 上一帧状态均值8 维 ndarraycovariance: 上一帧协方差8×8 ndarray(mean, covariance)外推后的均值与协方差不确定性变大multi_predictpredict 的向量化批量版一次外推所有轨迹mean: N×8 状态矩阵ndarraycovariance: N×8×8 协方差数组ndarray(mean, covariance)N×8 均值矩阵与 N×8×8 协方差数组对比项predictmulti_predict处理数量1 条轨迹N 条轨迹mean 形状8 维向量N×8 矩阵covariance 形状8×8N×8×8取 h 的写法mean[3]标量mean[:, 3]N 个a 的噪声写法1e-2固定值1e-2 * np.ones_like(...)广播成 N 份数值不变Q 构造直接np.diag(...)拼好再逐条np.diag循环数学公式完全相同完全相同BYTETracker 跟踪循环Step 2/3/4 前实际调用的是STrack.multi_predict(strack_pool)→ 内部调KalmanFilter.multi_predict每帧对匹配池全部轨迹一次算完predict是单条接口供STrack.predict()单轨迹场景使用。四、project投影到观测空间方法作用输入输出project把 8 维预测状态投影到 4 维观测空间检测器只能看到 x、y、a、h 这 4 维并叠加观测噪声mean: 预测均值8 维 ndarraycovariance: 预测协方差8×8 ndarray(new_mean, new_cov)投影后的均值4 维与协方差4×4公式new_mean H · mean new_cov H · cov · Hᵀ R字母是什么从哪来H观测矩阵4×8只取前 4 维__init__的_update_matmean8 维预测均值predict 的输出cov8×8 预测协方差predict 的输出R观测噪声4×4 对角阵检测框自身的不确定性函数内现算diag((h/20)², (h/20)², 0.1², (h/20)²)h目标框高像素mean[3]计算过程实际数字输入predict 输出的轨迹1mean [108, 50, 1.5, 40, 8, 0, 0, 0] cov [[26, 0, 0, 0, 6, 0, 0, 0 ], [0, 27, 0, 0, 0, 5, 0, 0 ], [0, 0, 0.8, 0, 0, 0, 0.3, 0 ], [0, 0, 0, 25, 0, 0, 0, 7 ], [6, 0, 0, 0, 6.06, 0, 0, 0 ], [0, 5, 0, 0, 0, 5.06,0, 0 ], [0, 0, 0.3, 0, 0, 0, 0.3, 0 ], [0, 0, 0, 7, 0, 0, 0, 7.06]]算 Rh 40R diag((40/20)², (40/20)², 0.1², (40/20)²) diag(4, 4, 0.01, 4)投影H 取前 4 维 取 cov 左上 4×4new_mean H · mean [108, 50, 1.5, 40] new_cov H · cov · Hᵀ R [[264, 0, 0, 0 ], [0, 274, 0, 0 ], [0, 0, 0.80.01, 0], [0, 0, 0, 254]] [[30, 0, 0, 0 ], [0, 31, 0, 0 ], [0, 0, 0.81, 0 ], [0, 0, 0, 29 ]]输出new_mean [108, 50, 1.5, 40] ← 4 维 new_cov [[30, 0, 0, 0 ], ← 4×4 [0, 31, 0, 0 ], [0, 0, 0.81, 0 ], [0, 0, 0, 29 ]]五、update修正步方法作用输入输出update修正步用配对成功的检测框修正预测状态——算卡尔曼增益 K加权融合预测和检测输出更准的状态mean: 预测均值8 维 ndarraycovariance: 预测协方差8×8 ndarraymeasurement: 检测框[x, y, a, h]4 维 ndarray(mean, covariance)修正后的均值与协方差不确定性变小公式K cov · Hᵀ · new_cov⁻¹ innovation measurement − new_mean mean_new mean K · innovation cov_new cov − K · new_cov · Kᵀ字母是什么从哪来K卡尔曼增益8×4 矩阵每个状态分量的修正权重公式算出new_mean投影后的预测均值4 维project 的输出new_cov投影后的协方差4×4project 的输出innovation残差4 维检测框比预测框偏了多少检测框 − 投影预测measurement检测框[x, y, a, h]配对成功的检测框坐标转换mean / cov8 维预测状态predict 的输出计算过程实际数字输入mean [108, 50, 1.5, 40, 8, 0, 0, 0] cov [[26, 0, 0, 0, 6, 0, 0, 0 ], [0, 27, 0, 0, 0, 5, 0, 0 ], [0, 0, 0.8, 0, 0, 0, 0.3, 0 ], [0, 0, 0, 25, 0, 0, 0, 7 ], [6, 0, 0, 0, 6.06, 0, 0, 0 ], [0, 5, 0, 0, 0, 5.06,0, 0 ], [0, 0, 0.3, 0, 0, 0, 0.3, 0 ], [0, 0, 0, 7, 0, 0, 0, 7.06]] measurement [108.5, 50.5, 1.5, 41] ← 检测框检测器说目标在 108.5project 的输出上一步算好的new_mean [108, 50, 1.5, 40] new_cov [[30, 0, 0, 0 ], [0, 31, 0, 0 ], [0, 0, 0.81, 0 ], [0, 0, 0, 29 ]]算 K公式带实际数据K cov · Hᵀ · new_cov⁻¹ [[26, 0, 0, 0 ] [1/30, 0, 0, 0 ] [0, 27, 0, 0 ] [0, 1/31, 0, 0 ] [0, 0, 0.8, 0 ] · [0, 0, 1/0.81,0 ] [0, 0, 0, 25 ] [0, 0, 0, 1/29] [6, 0, 0, 0 ] [0, 5, 0, 0 ] [0, 0, 0.3, 0 ] [0, 0, 0, 7 ]] [[26/30, 0, 0, 0 ] [0, 27/31, 0, 0 ] [0, 0, 0.8/0.81, 0 ] [0, 0, 0, 25/29 ] [6/30, 0, 0, 0 ] [0, 5/31, 0, 0 ] [0, 0, 0.3/0.81, 0 ] [0, 0, 0, 7/29 ]] [[0.87, 0, 0, 0 ] ← 8 行8 个状态分量× 4 列4 个观测分量 [0, 0.87, 0, 0 ] ← 每行的非零值 该分量的修正权重 [0, 0, 0.99, 0 ] [0, 0, 0, 0.86] [0.20, 0, 0, 0 ] ← 速度行的小权重位置残差通过它传导到速度 [0, 0.16, 0, 0 ] [0, 0, 0.37, 0 ] [0, 0, 0, 0.24]]算残差innovation measurement − new_mean [108.5−108, 50.5−50, 1.5−1.5, 41−40] [0.5, 0.5, 0, 1]修正 mean公式带实际数据mean_new mean K · innovation [108, 50, 1.5, 40, 8, 0, 0, 0] [0.87×0.5, 0.87×0.5, 0.99×0, 0.86×1, 0.20×0.5, 0.16×0.5, 0.37×0, 0.24×1] [108, 50, 1.5, 40, 8, 0, 0, 0 ] [0.44, 0.44, 0, 0.86, 0.10, 0.08, 0, 0.24 ] [108.4, 50.4, 1.5, 40.9, 8.1, 0.08, 0, 0.24 ]修正 cov公式带实际数据cov_new cov − K · new_cov · Kᵀ K · new_cov先算8×4 · 4×4每行 该行权重 × 所在列的方差 [[0.87×30, 0, 0, 0 ] [0, 0.87×31, 0, 0 ] [0, 0, 0.99×0.81,0 ] [0, 0, 0, 0.86×29] [0.20×30, 0, 0, 0 ] [0, 0.16×31, 0, 0 ] [0, 0, 0.37×0.81,0 ] [0, 0, 0, 0.24×29]] [[26.1, 0, 0, 0 ] [0, 27.0, 0, 0 ] [0, 0, 0.80, 0 ] [0, 0, 0, 24.9] [6.0, 0, 0, 0 ] [0, 5.0, 0, 0 ] [0, 0, 0.30, 0 ] [0, 0, 0, 7.0 ]] K · new_cov · Kᵀ再乘 Kᵀ对角线 每行 26.1²/30 之类即 权重×方差×权重 对角线 [0.87×26.1, 0.87×27.0, 0.99×0.80, 0.86×24.9, 0.20×6.0, 0.16×5.0, 0.37×0.30, 0.24×7.0] [22.7, 23.5, 0.79, 21.4, 1.2, 0.8, 0.11, 1.68] cov_new cov − 上面的矩阵 [[26−22.7, 0, 0, 0, 6−1.2, 0, 0, 0 ], [0, 27−23.5, 0, 0, 0, 5−0.8, 0, 0 ], [0, 0, 0.8−0.79, 0, 0, 0, 0.3−0.11, 0 ], [0, 0, 0, 25−21.4, 0, 0, 0, 7−1.68 ], [6−1.2, 0, 0, 0, 6.06−0.4,0, 0, 0 ], [0, 5−0.8, 0, 0, 0, 5.06−0.13,0, 0 ], [0, 0, 0.3−0.11, 0, 0, 0, 0.3−0.04, 0 ], [0, 0, 0, 7−1.68, 0, 0, 0, 7.06−0.5]] [[3.3, 0, 0, 0, 4.8, 0, 0, 0 ], [0, 3.5, 0, 0, 0, 4.2, 0, 0 ], [0, 0, 0.008, 0, 0, 0, 0.19, 0 ], [0, 0, 0, 3.6, 0, 0, 0, 5.3 ], [4.8, 0, 0, 0, 5.7, 0, 0, 0 ], [0, 4.2, 0, 0, 0, 4.9, 0, 0 ], [0, 0, 0.19, 0, 0, 0, 0.26, 0 ], [0, 0, 0, 5.3, 0, 0, 0, 6.6 ]]输出mean_new [108.4, 50.4, 1.5, 40.9, 8.1, 0.08, 0, 0.24] cov_new [[3.3, 0, 0, 0, 4.8, 0, 0, 0 ], [0, 3.5, 0, 0, 0, 4.2, 0, 0 ], [0, 0, 0.008, 0, 0, 0, 0.19, 0 ], [0, 0, 0, 3.6, 0, 0, 0, 5.3 ], [4.8, 0, 0, 0, 5.7, 0, 0, 0 ], [0, 4.2, 0, 0, 0, 4.9, 0, 0 ], [0, 0, 0.19, 0, 0, 0, 0.26, 0 ], [0, 0, 0, 5.3, 0, 0, 0, 6.6 ]]修正后的框中心 (108.4, 50.4) 落在预测 108 和检测 108.5 之间这两个值存回轨迹下一帧 predict 从它们出发——predict 变大 / update 变小循环闭环。六、initiate建档初始化方法作用输入输出initiate建档初始化用建档检测框生成轨迹的初始状态——位置来自检测框速度置 0没观测过速度并给一个初始不确定性速度方差给得大位置方差给得小measurement: 建档检测框[x, y, a, h]4 维 ndarray检测框 tlwh 转换来的中心坐标、宽高比、高(mean, covariance)初始均值8 维 ndarray速度 4 维全 0与初始协方差8×8 对角阵计算过程实际数字输入检测D 在[30, 40, 50, 100]tlwh 格式 → 转 xyahmeasurement [55, 90, 0.5, 100] ← x55, y90框中心a0.5宽高比h100框高第 1 步拼初始 mean位置 速度mean_pos [55, 90, 0.5, 100] ← 位置 4 维直接用检测框 mean_vel [0, 0, 0, 0] ← 速度 4 维没观测过全置 0 mean np.r_[mean_pos, mean_vel] ← 拼成 8 维 [55, 90, 0.5, 100, 0, 0, 0, 0]第 2 步构造初始 std8 个分量的噪声标准差h100std [ 2 × (1/20) × 100, ← x2 × h/20 10 2 × (1/20) × 100, ← y10 1e-2, ← a固定 0.01 2 × (1/20) × 100, ← h10 10 × (1/160) × 100, ← vx10 × h/160 6.25 10 × (1/160) × 100, ← vy6.25 1e-5, ← va固定 0.00001 10 × (1/160) × 100] ← vh6.25 [10, 10, 0.01, 10, 6.25, 6.25, 0.00001, 6.25]位置噪声用 2 倍权重、速度噪声用 10 倍权重比 predict 的 1 倍大——刚建档时啥都不确定尤其速度完全没观测过方差给大让 update 阶段能快速修正它。第 3 步拼初始 covariancestd 平方上对角线covariance diag(10², 10², 0.01², 10², 6.25², 6.25², 0.00001², 6.25²) diag(100, 100, 0.0001, 100, 39.06, 39.06, ~0, 39.06)输出mean [55, 90, 0.5, 100, 0, 0, 0, 0] covariance [[100, 0, 0, 0, 0, 0, 0, 0 ], [0, 100, 0, 0, 0, 0, 0, 0 ], [0, 0, 0.0001, 0, 0, 0, 0, 0 ], [0, 0, 0, 100, 0, 0, 0, 0 ], [0, 0, 0, 0, 39.06, 0, 0, 0 ], [0, 0, 0, 0, 0, 39.06, 0, 0 ], [0, 0, 0, 0, 0, 0, ~0, 0 ], [0, 0, 0, 0, 0, 0, 0, 39.06]]这个 mean/covariance 就是轨迹的初始状态activate()存进 STrack下一帧 predict 从它们出发。描述链接项目基于 Ultralytics ByteTrack 的多目标跟踪与改进往期回顾链接数据集数据集汇总软件开发深度学习软件开发汇总