卡尔曼滤波轨迹预测过程
源码:bytetrack/kalman_filter.py 的
predict(单条轨迹)和multi_predict(N 条轨迹批量)。
本文按代码顺序、用实际数字逐步推算两个函数,最后对比。
公式与符号总表
| 符号 | 代码里的真身 | 是什么 | 从哪来 |
|---|---|---|---|
| mean | 参数mean | 状态均值(8 维) | 轨迹上一帧的 mean(initiate 建档时初始化) |
| cov(P) | 参数covariance | 状态协方差(8×8),对角线是各分量的不确定度 | 轨迹上一帧的 covariance |
| F | self._motion_mat | 状态转移矩阵(8×8),匀速模型 | __init__构造:单位阵,右上 4×4 填 dt=1 |
| Q | 局部变量motion_cov | 过程噪声协方差(8×8 对角阵),“匀速假设本身的误差预算” | predict 内部用两个噪声权重 × mean[3] 现算 |
| h | mean[3] | 这条轨迹的框高(像素),不是固定值,每条轨迹各自取 | 状态向量第 4 个分量 |
_std_weight_position | 类属性 | 位置噪声权重 = 1/20 | __init__写死 |
_std_weight_velocity | 类属性 | 速度噪声权重 = 1/160 | __init__写死 |
| mean_new | 返回的mean | 外推后的状态均值(8 维) | mean · Fᵀ 算出 |
| cov_new(P’) | 返回的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] ← x=100, y=50, a=1.5, h=40, vx=8, vy=0, va=0, vh=0 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] = 40:
std_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 = 40:
std_vel = [40/160, 40/160, 0.00001, 40/160] = [0.25, 0.25, 0.00001, 0.25]第 3 步:拼成 Q 矩阵(motion_cov)
motion_cov=np.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 步:外推 mean
mean=np.dot(mean,self._motion_mat.T)# mean · FᵀF(=_motion_mat,__init__构造)右上角的 1 使"位置 + 速度×dt":
x' = 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 步:外推 cov
covariance=np.linalg.multi_dot((self._motion_mat,covariance,self._motion_mat.T))+motion_cov# = F · cov · Fᵀ + Qcov 是对角阵时,结果规律简单:新对角线 = 旧对角线(自己) + 旧对角线(对应速度分量) + Q(自己),位置分量与自己的速度分量之间产生交叉项 = 旧速度方差:
cov_new(x 行): 16 + 6 + 4 = 26, 交叉项 [x, vx] = 6 cov_new(y 行): 18 + 5 + 4 = 27, 交叉项 [y, vy] = 5 cov_new(a 行): 0.5 + 0.3 + 0.0001 ≈ 0.8,交叉项 [a, va] = 0.3 cov_new(h 行): 14 + 7 + 4 = 25, 交叉项 [h, vh] = 7 cov_new(速度行): vx 6+0.0625=6.06,vy 5.06,va 0.3,vh 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_predict(N 条轨迹批量)实际推算
输入(举例 N=2)
mean = [[100, 50, 1.5, 40, 8, 0, 0, 0], ← 轨迹1(就是上面的例子) [200, 300, 0.8, 60, -6, 10, 0, 0]] ← 轨迹2:h=60,向左下移动 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/20,N 个 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_vel
std_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(每条轨迹一份)
sqr=np.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) ← 轨迹1,h=40 Q₂ = diag(9, 9, 0.0001, 9, 0.14, 0.14, ~0, 0.14) ← 轨迹2,h=60(噪声更大)第 4 步:外推 mean(一次矩阵乘全部完成)
mean=np.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] ← 100+8 轨迹2: [200,300,0.8,60,-6,10,0,0] → [194, 310, 0.8, 60, -6, 10, 0, 0] ← 200-6, 300+10第 5 步:外推 cov(三维数组批量乘)
left=np.dot(self._motion_mat,covariance).transpose((1,0,2))covariance=np.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 对角线: [20+8+9=37, 22+9+9=40, 0.6+0.4+0.0001≈1, 18+10+9=37, 8.14, 9.14, 0.4, 10.14]输出(返回)
mean_new(2×8) = [[108, 50, 1.5, 40, 8, 0, 0, 0], [194, 310, 0.8, 60, -6, 10, 0, 0]] cov_new(2×8×8)= 轨迹1、轨迹2 各一份 8×8(对角线见上)三、两函数对比
| 方法 | 作用 | 输入 | 输出 |
|---|---|---|---|
predict | 预测步:按匀速模型把状态外推到下一帧 | mean: 上一帧状态均值(8 维 ndarray);covariance: 上一帧协方差(8×8 ndarray) | (mean, covariance):外推后的均值与协方差(不确定性变大) |
multi_predict | predict 的向量化批量版,一次外推所有轨迹 | mean: N×8 状态矩阵(ndarray);covariance: N×8×8 协方差数组(ndarray) | (mean, covariance):N×8 均值矩阵与 N×8×8 协方差数组 |
| 对比项 | predict | multi_predict |
|---|---|---|
| 处理数量 | 1 条轨迹 | N 条轨迹 |
| mean 形状 | 8 维向量 | N×8 矩阵 |
| covariance 形状 | 8×8 | N×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 维 ndarray);covariance: 预测协方差(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_mat |
| mean | 8 维预测均值 | predict 的输出 |
| cov | 8×8 预测协方差 | predict 的输出 |
| R | 观测噪声(4×4 对角阵),检测框自身的不确定性 | 函数内现算:diag((h/20)², (h/20)², 0.1², (h/20)²) |
| h | 目标框高(像素) | mean[3] |
计算过程(实际数字)
输入(predict 输出的轨迹1):
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]]算 R(h = 40):
R = diag((40/20)², (40/20)², 0.1², (40/20)²) = diag(4, 4, 0.01, 4)投影(H 取前 4 维 = 取 cov 左上 4×4):
new_mean = H · mean = [108, 50, 1.5, 40] new_cov = H · cov · Hᵀ + R = [[26+4, 0, 0, 0 ], [0, 27+4, 0, 0 ], [0, 0, 0.8+0.01, 0], [0, 0, 0, 25+4]] = [[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 维 ndarray);covariance: 预测协方差(8×8 ndarray);measurement: 检测框[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×4) | project 的输出 |
| innovation | 残差(4 维),检测框比预测框偏了多少 | 检测框 − 投影预测 |
| measurement | 检测框[x, y, a, h] | 配对成功的检测框坐标转换 |
| mean / cov | 8 维预测状态 | 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.5)project 的输出(上一步算好的):
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 格式 → 转 xyah):
measurement = [55, 90, 0.5, 100] ← x=55, y=90(框中心),a=0.5(宽高比),h=100(框高)第 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 步:构造初始 std(8 个分量的噪声标准差,h=100)
std = [ 2 × (1/20) × 100, ← x:2 × h/20 = 10 2 × (1/20) × 100, ← y:10 1e-2, ← a:固定 0.01 2 × (1/20) × 100, ← h:10 10 × (1/160) × 100, ← vx:10 × h/160 = 6.25 10 × (1/160) × 100, ← vy:6.25 1e-5, ← va:固定 0.00001 10 × (1/160) × 100] ← vh:6.25 = [10, 10, 0.01, 10, 6.25, 6.25, 0.00001, 6.25]位置噪声用 2 倍权重、速度噪声用 10 倍权重(比 predict 的 1 倍大)——刚建档时啥都不确定,尤其速度完全没观测过,方差给大,让 update 阶段能快速修正它。
第 3 步:拼初始 covariance(std 平方上对角线)
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 的多目标跟踪与改进 |
| 往期回顾 | 链接 |
| 数据集 | 数据集汇总 |
| 软件开发 | 深度学习软件开发汇总 |