目标跟踪算法中的卡尔曼滤波学习

在使用多目标跟踪算法时，接触到卡尔曼滤波，一直没时间总结下，现在来填坑。

1. 背景知识

在理解卡尔曼滤波前，有几个概念值得考虑下：时序序列模型，滤波，线性动态系统

1. 时间序列模型

时间序列模型都可以用如下示意图表示：

这个模型包含两个序列，一个是黄色部分的状态序列，用X表示，一个是绿色部分的观测序列（又叫测量序列、证据序列、观察序列，不同的书籍有不同的叫法，在这里统一叫观测序列。）用Y表示。状态序列反应了系统的真实状态，一般不能被直接观测，即使被直接观测也会引进噪声；观测序列是通过测量得到的数据，它与状态序列之间有规律性的联系。

上面序列中，假设初始时间为t1t1, 则X1,Y1X1,Y1是t1t1时刻的状态值和观测值，X2,Y2X2,Y2是t2t2时刻的状态值和观测值…，即随着时间的流逝，序列从左向右逐渐展开。

常见的时间序列模型主要包括三个：隐尔马尔科夫模型，卡尔曼滤波，粒子滤波。

2. 滤波

时间序列模型中包括预测和滤波两步

预测：指用当前和过去的数据来求取未来的数据。对应上述序列图中，则是利用t1t1时刻X1,Y1X1,Y1的值，估计t2t2时刻X2X2值。
滤波：是用当前和过去的数据来求取当前的数据。对应上述序列图中，则是先通过上一步的预测步骤得到X2X2的一个预测值，再利用t2t2时刻Y2Y2的值对这个预测值进行纠正，得到最终的X2X2估计值。（通俗讲，就是通过X1X1预测一个值, 通过传感器测量一个值Y2Y2, 将两者进行融合得到最终的X2X2值）

3.线性动态系统

卡尔曼滤波又称为基于高斯过程的线性动态系统(Linear Dynamic System, LDS) , 这里的高斯是指：状态变量XtXt和观测变量YtYt都符合高斯分布；这里的线性是指：XtXt可以通过Xt−1Xt−1线性表示，YtYt可以通过XtXt线性表示；如果用数学表达式来表达这两层含义如下：

Xt=FXt−1+wt−1,wt−1∼N(0,Q)Xt=FXt−1+wt−1,wt−1∼N(0,Q)

上面表达式中F是一个矩阵，常称作状态转移矩阵，保证了XtXt和Xt−1Xt−1的线性关系(线性代数中，矩阵就是线性变换)；wt−1wt−1常称作噪声，其服从均值为0，方差为Q的高斯分布，保证了XtXt服从高斯分布(因为高斯分布加上一个常数后依然是高斯分布)。

同样的关于XtXt和YtYt,也可以得到如下表示, 其中矩阵H称作状态空间到观测空间的映射矩阵， rtrt称作噪声，其服从高斯分布：

Yt=HXt+rt,(rt∼N(0,R))Yt=HXt+rt,(rt∼N(0,R))

参考：https://zhuanlan.zhihu.com/p/139215491

2. 卡尔曼滤波理论知识

关于卡尔曼滤波的理论知识有很多文章讲的很好，参考下面的两个链接

https://zhuanlan.zhihu.com/p/25598462

http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/#mjx-eqn-gaussequiv

这里简单总结下用得到数学表达式，方便实际编程时查阅和理解：

2.1 状态序列的预测

在时间序列模型图中，这一步表示的是, 在预测序列中，从Xt−1Xt−1到XtXt的预测步骤。

需要用到的变量含义如下：

^xkxk:状态变量
^PkPk: 状态变量的协方差矩阵
FkFk:状态转移矩阵
BkBk:控制矩阵
μkμk:控制向量
wkwk: 状态变量的噪声矩阵
QkQk:协方差矩阵的噪声矩阵

数学表达式如下：

^xk=Fkxk−1+Bk→μk+wk^Pk=FkPk−1FTk+Qkx^k=Fkxk−1+Bkμ→k+wkP^k=FkPk−1FkT+Qk

2.2 状态序列到观测序列的转换

在时间序列模型图中，这一步表示的是, 从XtXt到YtYt的转换。

需要用到的变量含义如下：

HkHk:状态空间到测量空间的映射矩阵
^xkxk: tktk时刻的状态变量
^PkPk: tktk时刻状态变量的协方差矩阵

映射后变量的均值和方差如下：

→μexpected=Hk^{xkΣexpected=Hk}PkHTkμ→expected=Hkx^{kΣexpected=HkP}kHkT

2.3 测量序列的滤波

在时间序列模型图中，这一步表示的是，测量值YtYt对预测值XtXt进行纠正，得到最终的状态向量XtXt

观测向量服从高斯分布, 假设其均值和协方差矩阵如下：

观测向量均值：→μmeasurement=→zk观测向量协方差矩阵:Σmeasurement=Rk观测向量均值：μ→measurement=z→k观测向量协方差矩阵:Σmeasurement=Rk

我们已经得到了状态序列的预测值，而且将其映射到了测量序列，也得到了测量序列的测量值，接来要做的就是将两者进行融合，由于两者都符合高斯分布，其融合过程就是两个高斯分布的混合

1. 高斯混合

单变量高斯分布方程：

N(x,μ,σ)=1σ√2πe−(x−μ)22σ2N(x,μ0,σ0)⋅N(x,μ1,σ1)?=N(x,μ′,σ′)(1)(1)N(x,μ,σ)=1σ2πe−(x−μ)22σ2N(x,μ0,σ0)⋅N(x,μ1,σ1)=?N(x,μ′,σ′)

两个单变量高斯分布混合：

μ′=μ0+k(μ1−μ0)σ′2=kσ12=σ02(1−k)其中，k=σ20σ20+σ21μ′=μ0+k(μ1−μ0)σ′2=kσ12=σ02(1−k)其中，k=σ02σ02+σ12

以上是单变量概率密度函数的计算结果，如果是多变量的，那么，就变成了协方差矩阵的形式：

K=Σ0(Σ0+Σ1)−1→μ′=→μ0+K(→μ1−→μ0)Σ′=KΣ1=Σ0(I−K)K=Σ0(Σ0+Σ1)−1μ→′=μ→0+K(μ→1−μ→0)Σ′=KΣ1=Σ0(I−K)

2. 预测值和测量值混合

预测值和测量值的均值和协方差如下：

预测值高斯分布：(μ0,Σ0)=(Hk^{xk,HkPkHTk)测量值高斯分布：(μ1,Σ1)=(zk,Rk)预测值高斯分布：(μ0,Σ0)=(Hkx}k,HkPkHkT)测量值高斯分布：(μ1,Σ1)=(zk,Rk)

预测值和测量值混合：

Hk^x′k=Hkxk+K(→zk−Hk^{xk)HkP′kHTk=HkPkHTk−KHkPkHTk其中，K称作卡尔曼增益：K=HkPkHTk(HkPkHTk+Rk)−1Hkx}k′=Hkx^{k+K(zk→−Hkx}k)HkPk′HkT=HkPkHkT−KHkPkHkT其中，K称作卡尔曼增益：K=HkPkHkT(HkPkHkT+Rk)−1

化简上述表达式，计算x′k,P′kxk′,Pk′，结果如下：(x′kxk′就是第k次卡尔曼预测结果, P′kPk′是该结果的协方差矩阵)

^x′k=xk+K′(→zk−Hk^{xk)P′k=Pk−K′HkPkK′=PkHTk(HkPkHTk+Rk)−1x}k′=x^{k+K′(zk→−Hkx}k)Pk′=Pk−K′HkPkK′=PkHkT(HkPkHkT+Rk)−1

最终，可以卡尔曼滤波流程图如下：

参考：https://zhuanlan.zhihu.com/p/142044672
https://zhuanlan.zhihu.com/p/158751412
https://zhuanlan.zhihu.com/p/58675854
https://leijiezhang001.github.io/卡尔曼滤波详解/
https://zhuanlan.zhihu.com/p/166342719

3. 卡尔曼滤波用于目标跟踪

对于一个小车，我们采用卡尔曼滤波来估计其位置，在进行估计的时候，有几个变量要进行定义和初始化。这里定义了六个变量的初始值：状态向量，状态向量协方差矩阵，状态向量协方差矩阵的噪声矩阵，动态转移矩阵，映射矩阵，测量值的协方差矩阵（假设小车是匀速运动，所以没有使用控制矩阵和控制变量）

1. 初始状态向量xx：（8x1的矩阵）

[xyahvxvyvavh]T[xyahvxvyvavh]T

其中对应含义和初始化值如下：

(x,y)表示目标的中心点，a=w/h表示目标的宽高比，h表示目标的高度，初始化值为目标第一帧的位置
v_x, v_y, v_a, v_h:表示对应变量随时间的变化率, 初始化值为0

2. 状态向量初始协方差矩阵PP：(8*8的矩阵，这里如何设置，需要经验？)

⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣151.2900000000151.29000000000.000100000000151.290000000059.090000000059.09000000000.000010000000059.09⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦[151.2900000000151.29000000000.000100000000151.290000000059.090000000059.09000000000.000010000000059.09]

3.状态向量协方差矩阵的噪声矩阵QQ：(8*8的矩阵，这里如何设置，需要经验？)

⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣37.820000000037.82000000000.00010000000037.82000000000.59000000000.59000000000.00001000000000.59⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦[37.820000000037.82000000000.00010000000037.82000000000.59000000000.59000000000.00001000000000.59]

4. 动态转移矩阵F：(8x8的矩阵)

⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣1000100001000100001000100001000100001000000001000000001000000001⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦[1000100001000100001000100001000100001000000001000000001000000001]

5. 映射矩阵H：（4x8的矩阵）

⎡⎢ ⎢ ⎢⎣10000000010000000010000000010000⎤⎥ ⎥ ⎥⎦[10000000010000000010000000010000]

6. 测量值的协方差矩阵RR：(4x4的矩阵, 这里如何设置，需要经验？)

⎡⎢ ⎢ ⎢⎣37.82000037.8200000.0001000037.82⎤⎥ ⎥ ⎥⎦[37.82000037.8200000.0001000037.82]

下面代码中，采用卡尔曼滤波，对小车的随机运动进行了估计，结果如图所示，绿色框表示小车的真实位置，红色框表示卡尔曼滤波的估计位置

# vim: expandtab:ts=4:sw=4
import numpy as np
import scipy.linalg
import cv2

"""
Table for the 0.95 quantile of the chi-square distribution with N degrees of
freedom (contains values for N=1, ..., 9). Taken from MATLAB/Octave's chi2inv
function and used as Mahalanobis gating threshold.
"""
chi2inv95 = {
    1: 3.8415,
    2: 5.9915,
    3: 7.8147,
    4: 9.4877,
    5: 11.070,
    6: 12.592,
    7: 14.067,
    8: 15.507,
    9: 16.919}


class KalmanFilter(object):
    """
    A simple Kalman filter for tracking bounding boxes in image space.

    The 8-dimensional state space

        x, y, a, h, vx, vy, va, vh

    contains the bounding box center position (x, y), aspect ratio a, height h,
    and their respective velocities.

    Object motion follows a constant velocity model. The bounding box location
    (x, y, a, h) is taken as direct observation of the state space (linear
    observation model).

    """

    def __init__(self):
        ndim, dt = 4, 1.

        # Create Kalman filter model matrices.
        self._motion_mat = np.eye(2 * ndim, 2 * ndim)  # 初始化动态转移矩阵, shape(8, 8)
        for i in range(ndim):
            self._motion_mat[i, ndim + i] = dt
        self._update_mat = np.eye(ndim, 2 * ndim)   # 初始化映射矩阵，shape(4, 8)

        # Motion and observation uncertainty are chosen relative to the current
        # state estimate. These weights control the amount of uncertainty in
        # the model. This is a bit hacky.
        self._std_weight_position = 1. / 20
        self._std_weight_velocity = 1. / 160

    def initiate(self, measurement):
        """Create track from unassociated measurement.

        Parameters
        ----------
        measurement : ndarray
            Bounding box coordinates (x, y, a, h) with center position (x, y),
            aspect ratio a, and height h.

        Returns
        -------
        (ndarray, ndarray)
            Returns the mean vector (8 dimensional) and covariance matrix (8x8
            dimensional) of the new track. Unobserved velocities are initialized
            to 0 mean.

        """
        mean_pos = measurement
        mean_vel = np.zeros_like(mean_pos)
        mean = np.r_[mean_pos, mean_vel]   # 状态向量，shape(1, 8)

        std = [
            2 * self._std_weight_position * measurement[3],
            2 * self._std_weight_position * measurement[3],
            1e-2,
            2 * self._std_weight_position * measurement[3],
            10 * self._std_weight_velocity * measurement[3],
            10 * self._std_weight_velocity * measurement[3],
            1e-5,
            10 * self._std_weight_velocity * measurement[3]]
        covariance = np.diag(np.square(std))    # 状态向量协方差矩阵， shape(8, 8)
        return mean, covariance

    def predict(self, mean, covariance):
        """Run Kalman filter prediction step.

        Parameters
        ----------
        mean : ndarray
            The 8 dimensional mean vector of the object state at the previous
            time step.
        covariance : ndarray
            The 8x8 dimensional covariance matrix of the object state at the
            previous time step.

        Returns
        -------
        (ndarray, ndarray)
            Returns the mean vector and covariance matrix of the predicted
            state. Unobserved velocities are initialized to 0 mean.

        """
        std_pos = [
            self._std_weight_position * mean[3],
            self._std_weight_position * mean[3],
            1e-2,
            self._std_weight_position * mean[3]]
        std_vel = [
            self._std_weight_velocity * mean[3],
            self._std_weight_velocity * mean[3],
            1e-5,
            self._std_weight_velocity * mean[3]]
        motion_cov = np.diag(np.square(np.r_[std_pos, std_vel]))

        mean = np.dot(self._motion_mat, mean)  # 动态转移矩阵*状态向量
        covariance = np.linalg.multi_dot((
            self._motion_mat, covariance, self._motion_mat.T)) + motion_cov   # 动态转移矩阵*状态向量协方差矩阵 + 噪声矩阵

        return mean, covariance

    def project(self, mean, covariance):
        """Project state distribution to measurement space.

        Parameters
        ----------
        mean : ndarray
            The state's mean vector (8 dimensional array).
        covariance : ndarray
            The state's covariance matrix (8x8 dimensional).

        Returns
        -------
        (ndarray, ndarray)
            Returns the projected mean and covariance matrix of the given state
            estimate.

        """
        std = [
            self._std_weight_position * mean[3],
            self._std_weight_position * mean[3],
            1e-1,
            self._std_weight_position * mean[3]]
        innovation_cov = np.diag(np.square(std))

        mean = np.dot(self._update_mat, mean)   # 映射矩阵*状态向量
        covariance = np.linalg.multi_dot((
            self._update_mat, covariance, self._update_mat.T))
        return mean, covariance + innovation_cov         # 映射矩阵*状态向量 + 噪声矩阵

    def update(self, mean, covariance, measurement):
        """Run Kalman filter correction step.

        Parameters
        ----------
        mean : ndarray
            The predicted state's mean vector (8 dimensional).
        covariance : ndarray
            The state's covariance matrix (8x8 dimensional).
        measurement : ndarray
            The 4 dimensional measurement vector (x, y, a, h), where (x, y)
            is the center position, a the aspect ratio, and h the height of the
            bounding box.

        Returns
        -------
        (ndarray, ndarray)
            Returns the measurement-corrected state distribution.

        """
        projected_mean, projected_cov = self.project(mean, covariance)

        chol_factor, lower = scipy.linalg.cho_factor(
            projected_cov, lower=True, check_finite=False)    # Cholesky分解
        kalman_gain = scipy.linalg.cho_solve(
            (chol_factor, lower), np.dot(covariance, self._update_mat.T).T,
            check_finite=False).T                             # 求解卡尔曼增益矩阵
        innovation = measurement - projected_mean

        new_mean = mean + np.dot(innovation, kalman_gain.T)    # 预测值和测量值融合后，新的状态向量
        new_covariance = covariance - np.linalg.multi_dot((
            kalman_gain, projected_cov, kalman_gain.T))        # 预测值和测量值融合后，新状态向量的协方差矩阵
        return new_mean, new_covariance

    def gating_distance(self, mean, covariance, measurements,
                        only_position=False):
        """Compute gating distance between state distribution and measurements.

        A suitable distance threshold can be obtained from `chi2inv95`. If
        `only_position` is False, the chi-square distribution has 4 degrees of
        freedom, otherwise 2.

        Parameters
        ----------
        mean : ndarray
            Mean vector over the state distribution (8 dimensional).
        covariance : ndarray
            Covariance of the state distribution (8x8 dimensional).
        measurements : ndarray
            An Nx4 dimensional matrix of N measurements, each in
            format (x, y, a, h) where (x, y) is the bounding box center
            position, a the aspect ratio, and h the height.
        only_position : Optional[bool]
            If True, distance computation is done with respect to the bounding
            box center position only.

        Returns
        -------
        ndarray
            Returns an array of length N, where the i-th element contains the
            squared Mahalanobis distance between (mean, covariance) and
            `measurements[i]`.

        """
        mean, covariance = self.project(mean, covariance)
        if only_position:
            mean, covariance = mean[:2], covariance[:2, :2]
            measurements = measurements[:, :2]

        cholesky_factor = np.linalg.cholesky(covariance)
        d = measurements - mean
        z = scipy.linalg.solve_triangular(
            cholesky_factor, d.T, lower=True, check_finite=False,
            overwrite_b=True)
        squared_maha = np.sum(z * z, axis=0)
        return squared_maha

if __name__ == "__main__":
    kf = KalmanFilter()
    img = cv2.imread("./car.png")
    m = np.array([108.5, 360.5, 1.618, 123])   # 小车的初始化测量值，分别表示小车的x_center, y_center, w/h, h
    mean_ini, covariance_ini = kf.initiate(m)  # 初始化卡尔曼的动态转移矩阵，映射矩阵



    for i in range(30):
        mean_pre, covariance_pre = kf.predict(mean_ini, covariance_ini)  # 预测状态变量
        dx = np.random.randint(0, 120)
        dy = np.random.randint(-30, 30)
        m = m + np.array([dx, dy, 0, 0])   # 随机向左，和上下移动
        cv2.rectangle(img, (int(m[0]-m[2]*m[3]/2), int(m[1]-m[3]/2)),
                      (int(m[0]+m[2]*m[3]/2), int(m[1]+m[3]/2)), (0, 255, 0), 2)  # 用绿色框，绘制小车移动后的真实位置

        mean_upd, covariance_upd = kf.update(mean_pre, covariance_pre, m+np.random.randn(4))   # 利用测量值，更新状态变量
        mean_ini = mean_upd
        covariance_ini = covariance_upd

        # 用红色框，绘制小车移动后的估计位置
        cv2.rectangle(img, (int(mean_ini[0] - mean_ini[2] * mean_ini[3] / 2), int(mean_ini[1] - mean_ini[3] / 2)),
                      (int(mean_ini[0] + mean_ini[2] * mean_ini[3] / 2), int(mean_ini[1] + mean_ini[3] / 2)), (0, 0, 255), 2)

        cv2.namedWindow("img", cv2.WINDOW_NORMAL)
        cv2.resizeWindow("img", 960, 540)
        cv2.imshow("img", img)
        cv2.waitKey(0)
        cv2.destroyAllWindows()