卡尔曼滤波目标跟踪的开源代码卡尔曼滤波运动跟踪

转载

lazihuman 2024-03-25 09:47:22

文章标签 卡尔曼滤波目标跟踪的开源代码自动驾驶机器学习算法世界坐标系 文章分类 计算机视觉人工智能

文章目录

简介
匀速运动模型
引入加速度
引入单应变换
实验结果分析
参考

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

简介

Kalman滤波器是通过前一状态预测当前状态，并使用当前观测状态进行校正
直接观测量为left, top, right, bottom, 分别代表目标的左上和右下坐标
一般地认为运动速度是匀速
但是监控场景中相机俯角太大，存在景深，导致运动是非匀速的

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

针对非匀速情况下

目标状态量引入加速度
目标状态转到世界坐标系

双目或多目相机：直接得到目标在相机坐标系下的三维坐标
单目可以采用单应变换矩阵转到到世界坐标系

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

卡尔曼滤波目标跟踪的开源代码卡尔曼滤波运动跟踪_自动驾驶

匀速运动模型

状态量： $卡尔曼滤波目标跟踪的开源代码卡尔曼滤波运动跟踪_算法_02$
观测量： $卡尔曼滤波目标跟踪的开源代码卡尔曼滤波运动跟踪_算法_03$
物体匀速运动情况
跟踪非连续帧
观测噪声方差和过程噪声误差和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

卡尔曼滤波目标跟踪的开源代码卡尔曼滤波运动跟踪_世界坐标系_04

引入加速度

状态量： $卡尔曼滤波目标跟踪的开源代码卡尔曼滤波运动跟踪_机器学习_05$
观测量： $卡尔曼滤波目标跟踪的开源代码卡尔曼滤波运动跟踪_算法_03$
物体加速运动
跟踪非连续帧
观测噪声方差和过程噪声误差和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

卡尔曼滤波目标跟踪的开源代码卡尔曼滤波运动跟踪_世界坐标系_07

引入单应变换

状态量： 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

卡尔曼滤波目标跟踪的开源代码卡尔曼滤波运动跟踪_卡尔曼滤波目标跟踪的开源代码_08