《卡尔曼滤波器用于图像上运动目标跟踪预测》

卡尔曼滤波器用于图像上运动目标跟踪预测

---

简介

• Kalman滤波器是通过前一状态预测当前状态，并使用当前观测状态进行校正

• 直接观测量为left, top, right, bottom, 分别代表目标的左上和右下坐标

• 一般地认为运动速度是匀速

• 但是监控场景中相机俯角太大，存在景深，导致运动是非匀速的

  • 来向，从远到近，加速运动
  • 去向，从近到远，减速运动

• 针对非匀速情况下

  • 目标状态量引入加速度
  • 目标状态转到世界坐标系
    • 双目或多目相机：直接得到目标在相机坐标系下的三维坐标
    • 单目可以采用单应变换矩阵转到到世界坐标系

• 选取来向的车辆，获取53帧运动坐标，选取前43帧预测更新，后10帧预测实验

匀速运动模型

• 状态量：$$l,t,r,b,v_l,v_t,v_r,v_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$$
• 物体加速运动
• 跟踪非连续帧
• 观测噪声方差和过程噪声误差和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

实验结果分析

• 显然匀速运动模型不适合

• 目标车辆在图像上并非是匀加(减)速，引入加速度模型，有了非线性预测效果，但是还不是很好

• 通过单应变换的转换，非线性明显，趋势不是很对，这种四个点都采用一个单应变换矩阵，误差有点大

• 详细工程加数据：卡尔曼滤波

参考

1 [运动目标跟踪中kalman滤波器的使用]