【数学与算法】跟踪、预测、单目标、多目标、匈牙利匹配之间的关系

多目标跟踪的步骤先后(简单记法)&＃xff1a;

先预测&＃xff0c;再匈牙利匹配&＃xff0c;最后再卡尔曼预测矫正&＃xff0c;得到最优估计。

【卡尔曼预测&＃43;匈牙利匹配】 进行多目标跟踪的详细步骤&＃xff1a;

• (1) 根据预测方程&＃xff0c;计算【状态预测值(先验值)】、【状态误差协方差先验估计】&＃xff1b;
• (2) 计算每个检测对象与每个状态跟踪器之间的【代价矩阵】&＃xff1b;
• (3) 匈牙利匹配(KM算法)求出【检测对象和跟踪器的匹配对】、【未匹配上的检测对象】、【未匹配上的卡尔曼跟踪器】&＃xff1b;
• (4) 给没匹配上的检测对象创建卡尔曼跟踪器&＃xff1b;
• (5) 根据状态预测方程&＃xff0c;给未匹配上的跟踪器进行简单的状态更新&＃xff0c;即只使用状态预测方程得到先验值&＃xff0c;没有卡尔曼增益矫正&＃xff1b;
• (6) 对匹配上的跟踪器求出【卡尔曼增益】、根据检测对象的观测值求出【状态最优状态估计】、 【状态误差协方差后验估计】。
• (7) 跳转到第(1)步&＃xff0c;把第(6)步求出来的【状态最优状态估计】、 【状态误差协方差后验估计】作为上一帧的值&＃xff0c;来预测当前帧的【状态预测值(先验值)】、【状态误差协方差先验估计】

过程噪声和测量噪声一定存在&＃xff0c;否则&＃xff0c;上面两个方程就是确定的方程&＃xff0c;就不存在卡尔曼预测了。
过程噪声和测量噪声&＃xff0c;这两个噪声的协方差矩阵并不是状态方程和测量方程中的那两个





w


k




w_k


wk​和





v


k




v_k


vk​,注意看&＃xff0c;





w


k




w_k


wk​和





v


k




v_k


vk​是向量&＃xff0c;向量中各元素对应每个特征的随机扰动&＃xff0c;他们每个特征都假设服从高斯分布。

注意&＃xff1a;均值u&＃61;0,标准差$\sigma$&＃61;1的高斯分布叫标准正态分布。

卡尔曼滤波器中只假设特征服从均值为0的高斯分布&＃xff0c;并不是服从标准正态分布。你既可以把你的特征标准差 $\sigma$设为1,也可以设为其他值。所以&＃xff0c;简单的卡尔曼滤波器都是把标准差$\sigma$设为1。

单个特征服从高斯分布N(0,$\sigma$)。

如果把多个零均值的特征组成特征向量&＃xff0c;那么特征向量就服从N(0,C)。C就是协方差矩阵。
如果两个特征之间的协方差为0&＃xff0c;就表示这两个特征之间相互独立。

那么特征向量的协方差矩阵就是对角阵&＃xff0c;即只有对角线上的元素非0&＃xff0c;其他元素都为0。对角线元素就表示自己特征的方差。

卡尔曼滤波的三个假设&＃xff1a;

可以参考卡尔曼滤波的三个假设.

• 1.服从高斯分布&＃xff08;测量噪声、系统噪声以及初始值都要求是&＃xff0c;且互不相关&＃xff09;
• 2.系统是线性的&＃xff08;状态变化和测量都要求是&＃xff09;
• 3.贝叶斯滤波的马尔可夫假设。又叫完整状态假设&＃xff0c;假设过去以及未来的数据都是独立的。也就是t时刻的状态可以只由t-1时刻的状态推出&＃xff0c;与&＃xff08;t-2&＃xff09;时刻的状态无关。这个其实在上面的线性公式中已经体现出来了。

预测与观测均是高斯分布&＃xff0c;两个高斯分布的乘积就是加权后的结果。新的高斯分布方差最小&＃xff0c;确定性也最高。

障碍物跟踪主要功能为:防止障碍物的位置、速度信息的帧间跳变&＃xff0c;保证障碍物信息的连续性。

所有的状态估计问题都会有这两个方程&＃xff1a;
运动方程&＃xff1a;上一帧到当前帧的一个变化关系。通过前一帧的状态量和方差&＃xff0c;得到当前帧的状态量和方差。
观测方程&＃xff1a;将当前的状态&＃xff0c;观测矩阵乘上这个状态&＃xff0c;再把观测的噪声加进去&＃xff0c;就得到你当前的观测结果。
注意&＃xff0c;状态量和观测量的维度不一定相等&＃xff0c;比如位置和速度是状态量&＃xff0c;观测量只有速度&＃xff0c;那么可以通过设定观测矩阵&＃xff0c;来去掉观测量的位置。

如果观测量的维度和状态量的维度一致&＃xff0c;可以写为&＃xff1a;





Z


&＃61;



[







P



t


_


m


e


a


s


u


r


e











V



t


_


m


e


a


s


u


r


e








]



&＃61;



[






1






0








0






1






]




[







P


t










V


t







]



&＃43;


测量噪声向量


(


二维


)



Z&＃61; \begin{bmatrix} P_{t\_measure}\\V_{t\_measure}\end{bmatrix}&＃61;\begin{bmatrix} 1&0\\0&1 \end{bmatrix} \begin{bmatrix} P_t\\V_t\end{bmatrix}&＃43;测量噪声向量(二维)


Z&＃61;[Pt_measure​Vt_measure​​]&＃61;[10​01​][Pt​Vt​​]&＃43;测量噪声向量(二维)
观测矩阵就是




[






1






0








0






1






]



\begin{bmatrix} 1&0\\0&1 \end{bmatrix}


[10​01​]。

如果观测量的维度和状态量的维度不一致&＃xff0c;只有速度可以观测到&＃xff0c;那么可以写为&＃xff1a;





Z


&＃61;



V



t


_


m


e


a


s


u


r


e




&＃61;



[






0






1






]




[







P


t










V


t







]



&＃43;


测量噪声向量


(


一维


)



Z&＃61;V_{t\_measure}&＃61;\begin{bmatrix} 0&1 \end{bmatrix} \begin{bmatrix} P_t\\V_t\end{bmatrix}&＃43;测量噪声向量(一维)


Z&＃61;Vt_measure​&＃61;[0​1​][Pt​Vt​​]&＃43;测量噪声向量(一维)
观测矩阵就是




[






0






1






]



\begin{bmatrix} 0&1 \end{bmatrix}


[0​1​]。

预测值是根据系统的状态用运动方程来预测的&＃xff1b;
观测值是通过某种传感器来获取的。
zhz:卡尔曼滤波的核心就是加权平均法&＃xff0c;只不过我们平时设置权重可能是人为经验指定的权重&＃xff0c;而卡尔曼滤波则是根据测量噪声、过程噪声、状态向量到测量向量的转移矩阵等进行建模得到的权重。

注意2个预测公式和5个更新公式&＃xff0c;我们想要得到的是状态量的最优值&＃xff0c;他是在更新公式中&＃xff0c;不是在预测公式中&＃xff0c;因为最优估计是通过加权平均估计出来的&＃xff0c;不是预测出来的&＃xff0c;因为预测出来的值也是有过程噪声的&＃xff0c;根据上一时刻状态量预测出下一时刻的状态量&＃xff0c;有偏差&＃xff0c;需要加权法来矫正。

预测的两个公式是根据上一时刻的状态量 得到当前时刻的状态量&＃xff08;先验值&＃xff09; &＃xff0c; 或者根据上一时刻$k-1$的






x


^




k


−


1





\hat{x}_{k-1}


x^k−1​的后验估计协方差&＃xff0c;得到当前$k$时刻的






x


^



k


−




\hat{x}_{k}^-


x^k−​的先验估计协方差。一个是状态量的预测&＃xff0c;一个是状态估计的协方差的预测。

更新的三个公式是分别是求卡尔曼增益、状态量的最优值、当前时刻的后验估计协方差。
预测和更新是交替进行。预测后的值需要在更新中用到&＃xff1b;而更新后的值&＃xff0c;作为下一时刻的预测的输入(时刻下标k变为了k-1代入预测公式)。

更新公式中的状态最优估计公式需要用到卡尔曼增益公式&＃xff0c;而卡尔曼增益公式需要用到预测公式中的协方差预测&＃xff0c;协方差公式的更新也需要用到预测公式中的协方差预测&＃xff0c;以及卡尔曼增益。

预测&＃xff1a;

预测就是针对每一个单一目标来特定分析。如果有多个目标&＃xff0c;那么就是分别对每个目标进行预测。

• (1) 根据该目标k-1时刻的状态来预测k时刻的状态(得到k时刻的先验估计值)&＃xff1b;
• (2) 然后用k时刻的观测值再来校正k的估计值&＃xff0c;(进行加权)得到k时刻的后验估计(最优估计)。

跟踪&＃xff1a;

对于单目标跟踪来说&＃xff0c;不需要匈牙利匹配&＃xff0c;只需要卡尔曼滤波。
对于多目标跟踪来说&＃xff0c;需要卡尔曼滤波器和匈牙利匹配一起才能做到。

• (1) 卡尔曼滤波器先根据所有目标k-1时刻的状态分别预测对应目标k时刻的状态(是先验估计值&＃xff0c;并非最优估计&＃xff0c;因为还不知道k时刻的观测值&＃xff0c;要匹配上之后才知道观测值)&＃xff1b;
• (2) 然后匈牙利匹配把所有目标的k时刻状态预测值(先验估计)和所有目标的k时刻观测值的状态进行匹配&＃xff0c;匹配上就是跟踪上了。
• 注意&＃xff0c;是k时刻预测值(先验估计值)和k时刻观测值匹配&＃xff0c;进行下一次跟踪时&＃xff0c;再利用k时刻观测值矫正k时刻先验估计值&＃xff0c;得到k时刻最优估计值&＃xff0c;作为新的状态值来预测k&＃43;1时刻的先验估计值。

平滑&＃xff1a;

平滑是对历史数据做修正。