1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140
| import numpy as np
class KalmanTracker: """ 卡尔曼滤波跟踪器 用于跟踪车内多个目标(儿童、宠物) """ def __init__(self, n_targets=4): self.n_targets = n_targets self.state_dim = 4 self.obs_dim = 2 self.F = np.array([ [1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1] ]) self.H = np.array([ [1, 0, 0, 0], [0, 1, 0, 0] ]) self.Q = np.eye(self.state_dim) * 0.1 self.R = np.eye(self.obs_dim) * 0.5 self.x = np.zeros((self.n_targets, self.state_dim)) self.P = np.tile(np.eye(self.state_dim) * 100, (self.n_targets, 1, 1)) def predict(self): """ 预测步骤 """ for i in range(self.n_targets): self.x[i] = self.F @ self.x[i] self.P[i] = self.F @ self.P[i] @ self.F.T + self.Q return self.x def update(self, z): """ 更新步骤 Args: z: 观测值 (n_obs, 2) """ n_obs = len(z) for i, obs in enumerate(z): if i >= self.n_targets: break K = self.P[i] @ self.H.T @ np.linalg.inv( self.H @ self.P[i] @ self.H.T + self.R ) self.x[i] = self.x[i] + K @ (obs - self.H @ self.x[i]) self.P[i] = (np.eye(self.state_dim) - K @ self.H) @ self.P[i] return self.x def track(self, detections): """ 跟踪流程 Args: detections: 检测结果列表 [(x, y, life_sign), ...] Returns: tracks: 跟踪结果 [(id, x, y, vx, vy), ...] """ self.predict() z = np.array([[d[0], d[1]] for d in detections if d[2] > 0.5]) if len(z) > 0: self.update(z) tracks = [] for i in range(self.n_targets): tracks.append({ 'id': i, 'x': self.x[i, 0], 'y': self.x[i, 1], 'vx': self.x[i, 2], 'vy': self.x[i, 3], 'life_sign': self._estimate_lifeSign(i, detections) }) return tracks def _estimateLifeSign(self, target_id, detections): """ 估计生命体征强度 """ if target_id < len(detections): return detections[target_id][2] return 0.0
if __name__ == "__main__": tracker = KalmanTracker(n_targets=4) detections = [ (1.2, 0.5, 0.8), (1.5, -0.6, 0.1), ] tracks = tracker.track(detections) for t in tracks[:2]: print(f"目标{t['id']}: 位置({t['x']:.2f}, {t['y']:.2f}), " f"生命体征{t['life_sign']:.2f}")
|