卡尔曼滤波器在车辆跟踪预测的应用（下）

上篇文章以小车在路上行驶为例，描述卡尔曼滤波器的５个公式和每个参数的含义。接在来，我将给出自己做的车辆位置预测的实战例子。

场景需求

一个实车视频中，多辆小车在高速公路上行驶，如果获取每帧多辆小车的位移，在算法实现上会浪费大量计算时间，而上下帧小车的速度和位移的计算分别是线性的关系，因此可以用卡尔曼滤波器（线性滤波器）来预测下一帧小车的位移，实现小车位移实时性这个目标。

卡尔曼滤波器的参数初始化

状态预测

上集描述到在状态的预测中，需要对系统状态转移矩阵 $F$ , 控制矩阵 $B$ , 和控制量 $U$ 初始化。对于本例子来说，速度和位移是系统状态量，小车位移表现为标识小车的矩形框四个坐标 $(x_0,y_0,x_1,y_1)$ ,速度表现在四个坐标点上的速度分量 $(vx_0,vy_0,vx_1,vy_1)$ 。由于上下帧的每一个方向坐标位移和速度都遵循上集描述的位移和速度的公式，因此，卡尔曼的第一个公式 $\hat x_{t}^-= F_{t}\hat x_{t}^-+B_{t}u_{t}$ 中的参数 $F$ , $B$ , $U$ ,分别是：

  self.F =np.array([[1, 0, 0, 0, self.dt, 0, 0, 0],
                        [0, 1, 0, 0, 0,self.dt,  0, 0],
                        [0, 0, 1, 0, 0, 0, self.dt, 0],
                        [0, 0, 0, 1, 0, 0, 0, self.dt],
                        [0, 0, 0, 0, 1, 0, 0, 0],
                         [0, 0, 0, 0, 0, 1, 0, 0],
                         [0, 0, 0, 0, 0, 0, 1, 0],
                         [0, 0, 0, 0, 0, 0, 0, 1]])
    self.B = np.array([[self.dt **2/2, 0, 0, 0, 0, 0, 0, 0],
                           [0, self.dt **2/2, 0, 0, 0, 0, 0, 0],
                           [0, 0, self.dt **2/2, 0, 0, 0, 0, 0],
                           [0, 0, 0, self.dt **2/2, 0, 0, 0, 0],
                           [0, 0, 0, 0, self.dt, 0, 0, 0],
                           [0, 0, 0, 0, 0, self.dt, 0, 0],
                           [0, 0, 0, 0, 0, 0, self.dt, 0],
                           [0, 0, 0, 0, 0, 0, 0, self.dt]])
      self.U = np.ones(8)

其中, $self.dt$ 表示时间，是上下帧的间隔时间。　
预测状态还有两个参数没有初始化，协方差矩阵 $P$ ,和噪声矩阵 $Q$ ，这两个初始化是根据实际运行结果进行调试的，但是必须保证矩阵的维度与 $\hat x_{t}^-$ 相互匹配。

self.P = np.diag(self.dt * np.ones(8))
self.Q = 4* np.eye(8)

观测矩阵

在本例中，观测值 $z_t$ 是小车的在四个方向的位移，而不关注这四个方向的速度。所以这里也证明了一个事实，系统的状态变量和测量的变量的数量可以不一样，要根据实际情况出发。接下来就要构造卡尔曼第三个公式的参数了．在构造观测矩阵 $H$ 的时候，需要明确矩阵的维度． $\hat x_{t}^-$ 是一个８１的矩阵， $z_t$ 是一个４1的列向量，所以 $H$ 的维度是４*8

self.H =   np.array([[1, 0, 0, 0, 0, 0, 0, 0],
                           [0, 0.98, 0, 0, 0, 0, 0, 0],
                            [0, 0, 0.9, 0, 0, 0, 0, 0],
                           [0, 0, 0, 0.95, 0, 0, 0, 0]])
 #  measure noise matrix
  self.R_scalar = 10.0
  self.R =  self.R_scalar *np.diag(np.ones(4))

跑起来

所有的参数完成初始化之后，那么就可以愉快的运行卡尔曼滤波的代码啦

    def kalman_filter(self,z):
        '''
        Implement the Kalman Filter ,including the predict and the update stages.
        with the measurement z
        '''
        x = self.x_state
        # Predict
        x = dot(self.F, x) + dot(self.B, self.U)
        self.P = dot(self.F,self.P).dot(self.F.T) +self.Q
        #update
        S = dot(self.H,self.P).dot(self.H.T)+ self.R
        K  = dot(self.P,self.H.T).dot(inv(S))   # kalman gain
        y = z - dot(self.H,x)  #residual
        x +=  dot(K,y)
        self.P = self.P - dot(K , self.H).dot(self.P)
        self.x_state = x  #conver to integer coordinates (pixel values)

视觉效果

ce378cd6a417c8f09ac7f69a3eb7cec7.gif

最后编辑于：2018.11.20 22:54:44