卡尔曼滤波器用于图像上运动目标跟踪预测
简介
-
Kalman滤波器是通过前一状态预测当前状态，并使用当前观测状态进行校正
-
直接观测量为left, top, right, bottom, 分别代表目标的左上和右下坐标
-
一般地认为运动速度是匀速
-
但是监控场景中相机俯角太大，存在景深，导致运动是非匀速的
- 来向，从远到近，加速运动
- 去向，从近到远，减速运动
-
针对非匀速情况下
- 目标状态量引入加速度
- 目标状态转到世界坐标系
- 双目或多目相机：直接得到目标在相机坐标系下的三维坐标
- 单目可以采用单应变换矩阵转到到世界坐标系
-
选取来向的车辆，获取53帧运动坐标，选取前43帧预测更新，后10帧预测实验
匀速运动模型
- 状态量： l , t , r , b , v l , v t , v r , v b l,t,r,b, v_l, v_t, v_r, v_b l,t,r,b,vl?,vt?,vr?,vb? 分别是左上和右下角点位置和对应速度
- 观测量： l , t , r , b l,t,r,b l,t,r,b
- 物体匀速运动情况
- 跟踪非连续帧
- 观测噪声方差和过程噪声误差和bbox高度有关,h越大，误差越大，实时更新
class KalmanFilter:
def __init__(self):
self._std_weight_pos = 1. / 20 # 位置
self._std_weight_vel = 1. / 160 # 速度
self.H = np.matrix(np.zeros((4, 8))) # 观测矩阵 Z_t = Hx_t + v
for i in range(4):
self.H[i, i] = 1
self.F = np.matrix(np.eye(8)) # 状态转移矩阵
def init(self, val):
l,t,r,b = val
mean_pos = np.matrix([[l], [t], [r], [b]])
mean_vel = np.zeros_like(mean_pos) # 速度初始化为0
self.x_hat = np.r_[mean_pos, mean_vel] # x_k = [p, v]
h = b - t
std_pos = [
2 * self._std_weight_pos * h,
2 * self._std_weight_pos * h,
2 * self._std_weight_pos * h,
2 * self._std_weight_pos * h]
std_vel = [
10 * self._std_weight_vel * h,
10 * self._std_weight_vel * h,
10 * self._std_weight_vel * h,
10 * self._std_weight_vel * h]
self.P = np.diag(np.square(np.r_[std_pos, std_vel])) # 状态协方差矩阵
def predict(self, delt_t, val=None):
if val is not None:
h = val[3] - val[1]
std_pos = [
self._std_weight_pos * h,
self._std_weight_pos * h,
self._std_weight_pos * h,
self._std_weight_pos * h]
std_vel = [
self._std_weight_vel * h,
self._std_weight_vel * h,
self._std_weight_vel * h,
self._std_weight_vel * h]
self.Q = np.diag(np.square(np.r_[std_pos, std_vel])) # 过程噪声实时变化
for i in range(4):
self.F[i, i+4] = delt_t
self.x_hat_minus = self.F * self.x_hat
self.P_minus = self.F * self.P * self.F.T + self.Q
def update(self, val):
l,t,r,b = val
h = b - t
std_pos = [
self._std_weight_pos * h,
self._std_weight_pos * h,
self._std_weight_pos * h,
self._std_weight_pos * h]
self.R = np.diag(np.square(std_pos)) # 观测噪声方差
measure = np.matrix([[l], [t], [r], [b]])
self.K = self.P_minus * self.H.T * np.linalg.inv(self.H * self.P_minus * self.H.T + self.R)
self.x_hat = self.x_hat_minus + self.K * (measure - self.H * self.x_hat_minus)
self.P = self.P_minus - self.K * self.H * self.P_minus
引入加速度
- 状态量： l , t , r , b , v l , v t , v r , v b , a l , a t , a r , a b l,t,r,b,v_l, v_t,v_r,v_b,a_l,a_t,a_r,a_b l,t,r,b,vl?,vt?,vr?,vb?,al?,at?,ar?,ab?
- 观测量： l , t , r , b l,t,r,b l,t,r,b
- 物体加速运动
- 跟踪非连续帧
- 观测噪声方差和过程噪声误差和bbox高度有关,h越大，误差越大，实时更新
class KalmanFilter2:
def __init__(self):
self._std_weight_pos = 1. / 20 # 位置
self._std_weight_vel = 1. / 160 # 速度
self._std_weight_acc = 1. / 300 # 速度
self.H = np.matrix(np.zeros((4, 12))) # 观测矩阵 Z_t = Hx_t + v
for i in range(4):
self.H[i, i] = 1
self.F = np.matrix(np.eye(12)) # 状态转移矩阵
def init(self, val):
l,t,r,b = val
mean_pos = np.matrix([[l], [t], [r], [b]])
mean_vel = np.zeros_like(mean_pos) # 速度初始化为0
mean_acc = np.zeros_like(mean_pos) # 速度初始化为0
self.x_hat = np.r_[mean_pos, mean_vel, mean_acc] # x_k = [p, v]
h = b - t
std_pos = [
2 * self._std_weight_pos * h,
2 * self._std_weight_pos * h,
2 * self._std_weight_pos * h,
2 * self._std_weight_pos * h]
std_vel = [
10 * self._std_weight_vel * h,
10 * self._std_weight_vel * h,
10 * self._std_weight_vel * h,
10 * self._std_weight_vel * h]
std_acc = [
50 * self._std_weight_acc * h,
50 * self._std_weight_acc * h,
50 * self._std_weight_acc * h,
50 * self._std_weight_acc * h
]
self.P = np.diag(np.square(np.r_[std_pos, std_vel, std_acc])) # 状态协方差矩阵
def predict(self, delt_t, val=None):
if val is not None:
l,t,r,b = val
h = b - t
std_pos = [
self._std_weight_pos * h,
self._std_weight_pos * h,
self._std_weight_pos * h,
self._std_weight_pos * h]
std_vel = [
self._std_weight_vel * h,
self._std_weight_vel * h,
self._std_weight_vel * h,
self._std_weight_vel * h]
std_acc = [
self._std_weight_acc * h,
self._std_weight_acc * h,
self._std_weight_acc * h,
self._std_weight_acc * h]
self.Q = np.diag(np.square(np.r_[std_pos, std_vel, std_acc])) # 过程噪声实时变化
for i in range(4):
self.F[i,i + 4] = delt_t
self.F[i,i + 8] = delt_t ** 2 / 2
for i in range(4, 8):
self.F[i,i+4] = delt_t
self.x_hat_minus = self.F * self.x_hat
self.P_minus = self.F * self.P * self.F.T + self.Q
def update(self, val):
l,t,r,b = val
h = b - t
std_pos = [
self._std_weight_pos * h,
self._std_weight_pos * h,
self._std_weight_pos * h,
self._std_weight_pos * h]
self.R = np.diag(np.square(std_pos)) # 观测噪声方差
measure = np.matrix([[l], [t], [r], [b]])
self.K = self.P_minus * self.H.T * np.linalg.inv(self.H * self.P_minus * self.H.T + self.R)
self.x_hat = self.x_hat_minus + self.K * (measure - self.H * self.x_hat_minus)
self.P = self.P_minus - self.K * self.H * self.P_minus
引入单应变换
- 状态量： l,t,r,b, vl,vt,vr,vb
- 观测量： l,t,r,b
- 观测噪声方差和过程噪声误差和bbox高度有关,h越大，误差越大，实时更新
- 标定图像和世界坐标系之间的单应变换矩阵*
- 选标志点构建世界坐标系
- 选取图像上对应点
- 标定（bbox并不都在世界坐标系设定的平面，近似采用，可能会有问题）
- 目标在世界坐标系内匀速运动
class KalmanFilter:
def __init__(self, homography):
self._std_weight_pos = 1. / 10 # 位置
self._std_weight_vel = 1. / 100 # 速度
self.H = np.matrix(np.zeros((4, 8))) # 观测矩阵 Z_t = Hx_t + v
for i in range(4):
self.H[i, i] = 1
self.F = np.matrix(np.eye(8)) # 状态转移矩阵
self.homography = np.reshape(homography, (3,3))
def _img2world(self, val):
l,t,r,b = val
tmp = np.dot(np.linalg.inv(self.homography), np.array([[l], [t], [1]]))
l_ = tmp[0] / tmp[2]
t_ = tmp[1] / tmp[2]
tmp = np.dot(np.linalg.inv(self.homography), np.array([[r], [b], [1]]))
r_ = tmp[0] / tmp[2]
b_ = tmp[1] / tmp[2]
return [l_, t_, r_, b_]
def _world2img(self, val):
l,t,r,b = val
tmp = np.dot(self.homography, np.array([[l], [t], [1]]))
l_ = tmp[0] / tmp[2]
t_ = tmp[1] / tmp[2]
tmp = np.dot(self.homography, np.array([[r], [b], [1]]))
r_ = tmp[0] / tmp[2]
b_ = tmp[1] / tmp[2]
return [l_, t_, r_, b_]
def init(self, val):
ltrb = self._img2world(val)
mean_pos = np.matrix(ltrb)
mean_vel = np.zeros_like(mean_pos) # 速度初始化为0
self.x_hat = np.r_[mean_pos, mean_vel] # x_k = [p, v]
h = val[3]-val[1]
std_pos = [
self._std_weight_pos * h,
self._std_weight_pos * h,
self._std_weight_pos * h,
self._std_weight_pos * h]
std_vel = [
self._std_weight_vel * h,
self._std_weight_vel * h,
self._std_weight_vel * h,
self._std_weight_vel * h]
self.P = np.diag(np.square(np.r_[std_pos, std_vel])) # 状态协方差矩阵
def predict(self, delt_t, val=None):
if val is not None:
h = val[3] - val[1]
std_pos = [
self._std_weight_pos * h,
self._std_weight_pos * h,
self._std_weight_pos * h,
self._std_weight_pos * h]
std_vel = [
self._std_weight_vel * h,
self._std_weight_vel * h,
self._std_weight_vel * h,
self._std_weight_vel * h]
self.Q = np.diag(np.square(np.r_[std_pos, std_vel])) # 过程噪声实时变化
for i in range(4):
self.F[i, i+4] = delt_t
self.x_hat_minus = self.F * self.x_hat
self.P_minus = self.F * self.P * self.F.T + self.Q
def update(self, val):
l,t,r,b = val
h = b - t
std_pos = [
self._std_weight_pos * h,
self._std_weight_pos * h,
self._std_weight_pos * h,
self._std_weight_pos * h]
ltrb = self._img2world(val)
self.R = np.diag(np.square(std_pos)) # 观测噪声方差
measure = np.matrix(ltrb)
self.K = self.P_minus * self.H.T * np.linalg.inv(self.H * self.P_minus * self.H.T + self.R)
self.x_hat = self.x_hat_minus + self.K * (measure - self.H * self.x_hat_minus)
self.P = self.P_minus - self.K * self.H * self.P_minus
实验结果分析
-
显然匀速运动模型不适合
-
目标车辆在图像上并非是匀加(减)速，引入加速度模型，有了非线性预测效果，但是还不是很好
-
通过单应变换的转换，非线性明显，趋势不是很对，这种四个点都采用一个单应变换矩阵，误差有点大
-
详细工程加数据：卡尔曼滤波
