Traditional Multiple Objects Tracking (updating)

落羽归尘

已于 2023-01-29 13:59:44 修改

阅读量522

点赞数 4

文章标签：目标跟踪

于 2021-11-03 16:28:11 首次发布

本文链接：https://blog.csdn.net/qq_21933647/article/details/107433035

版权

2. 常用数据关联方法(二分图（或者叫二部图）最大匹配算法之匈牙利算法、概率密度PDA)

1. 卡尔曼滤波的详细推导

1. 1 概率论基础

随机变量：随机变量是指变量的值无法预先确定仅以一定的可能性(概率)取值的量；

概率表示：

$p(X=x)$

随机变量所有取值的概率之和为1：

$\sum_{x}p(X=x)=1$ , $\int p(x)dx =1$

一维高斯分布：

$p(x)=\frac{1}{\sqrt{2\pi \sigma ^{^{2}}}}exp(-\frac{1}{2}\frac{(x-\mu )^{2}}{\sigma ^{2}})$

多维高斯分布：

$p(x)=det(2\pi \Sigma )^{-\frac{1}{2}}exp(-\frac{1}{2}(x-\mu )^{T}\Sigma ^{-1}(x-\mu ))$

联合分布：

$p(x,y)=p(X=x,Y=y)$ , 如果X,Y 相互独立，则有 $p(x,y)=p(x)p(y)$

条件概率：

$p(x|y)=p(X=x|Y=y)$

$p(x|y)=\frac{p(x,y)}{p(y)}$

全概率公式：

$p(x)=\sum_{y}p(x|y)p(y)$

$p(x)=\int p(x|y)p(y)dy$

贝叶斯公式：

举个栗子：我的朋友小鹿说，他的女神每次看到他的时候都冲他笑，他现在想知道女神是不是喜欢他呢？首先，我分析了给定的已知信息和未知信息：

1）要求解的问题：女神喜欢你，记为A事件

2）已知条件：女神经常冲你笑，记为B事件

所以，P(A|B)表示女神经常冲你笑这个事件(B)发生后，女神喜欢你（A）的概率。

先验概率： 也就是在不知道B事件的前提下，我们对A事件概率的一个主观判断;

可能性函数： $p(B|A)/p(B)$ 这是一个调整因子，也就是新信息B带来的调整，作用是将先验概率（之前的主观判断）调整到更接近真实概率(这部分暂不解释，下文解释)

后验概率： $p(A|B)$ , 即在B事件发生之后，我们对A事件概率的重新评估.

通过上面的实例，在这引出经典的贝叶斯公式：

$p(x|y)=\frac{p(y|x)p(x)}{p(y)}=\frac{p(y|x)p(y)}{\sum_{x'}p(y|x')p(x')}$ 公式 1

$p(x|y)=\frac{p(y|x)p(x)}{p(y)}=\frac{p(y|x)p(x)}{\int p(y|x')p(x')dx'}$ 公式2

"Sensor Generative Model or Emit Probability" ： $p(y|x)$

由于 $p(y)$ 不依赖于 $x$ ,通常其倒数用归一化参数 $\eta$ 表示;

$p(x|y)=\frac{p(y|x)p(x)}{p(y)}= \eta p(y|x)p(x)$

继续上面那个例子：先给定一些参数，假女神喜欢小鹿初始先验概率 $p(A)=0.5$ , 并且女神喜欢在喜欢一个人的情况下对那人笑的概率为 $p(B|A)=0.75$ ，不喜欢一个人对那个人笑的概率为 $p(B|\overline{A})=0.3$

第一周：小鹿和女神刚认识，且在这周内女神对小鹿笑了，求女神喜欢小鹿的概率, p(A|B)

$p(A|B)=\frac{0.75*0.5}{0.75*0.5 +0.3*0.5}=0.714$ ，

这一步中：

$\eta =\frac{1}{0.75*0.5+0.3*0.5}=1.91$

第二周：小鹿和女神已经认识一周了，且确定女神对小鹿有些好感，此时的先验概率为 $p(A)=0.714$ ，在第二周里，女神又对小鹿笑了，求第二周结束后，求女神喜欢小鹿的概率, p(A|B)

$p(A|B)=\frac{0.75*0.714}{0.75*0.714 +0.3*0.286}=0.862$

这一步中： $\eta =1.61$

第三周：小鹿和女神已经认识两周了，且确定女神对小鹿好感有明显提升，此时的先验概率为 $p(A)=0.862$ ，在第三周里，女神又对小鹿笑了，求第三周结束后，求女神喜欢小鹿的概率, p(A|B)

$p(A|B)=\frac{0.862*0.75}{0.862*0.75+0.138*0.3}=0.94$

这一步中： $\eta =1.45$

第四周：都94%的概率可以判定女神喜欢小鹿了，男生应该主动点，赶紧表白吧：

贝叶斯公式进阶 (留给你们自己推导吧)：

$p(x|y,z)=\frac{p(y|x,z)p(x|z)}{p(y|z)}$ 公式3

期望：

$E[x]=\sum_{x}xp(x)$ , $E[x]=\int xp(x)dx$ 

$E[ax+b]=aE[x]+b$ 

方差：

$Cov[X]=E[x-E[x]]^{^{2}}=E[x^{2}]-E[x]^{2}$ 

1. 2 状态、观测、控制、马尔卡夫链

状态： 表征智能体与环境相对关系或者本身一些属性的量，机器人和自动驾驶车辆中常见的状态

有以下几种： pose 、 velocity 等;

传感器观测： 智能体通过传感器直接获取的和环境交互的状态;

瞬时观测数据通常用 $z_{t}$ 表示，一系列的观测用 $z_{t^{_1} :t^{_2} }=z_{t^{_1}},z_{t^{_1}+1},z_{t^{_1}+2},...,z_{t^{_2}}$

控制： 智能体作出的某种动作或者给予智能体的某种激励;

瞬时控制量通常用 $u_{t}$ 表示,控制序列用 $u_{t^{_1} :t^{_2} }=u_{t^{_1}},u_{t^{_1}+1},u_{t^{_1}+2},...,u_{t^{_2}}$

状态估计的基本问题：

马尔科夫链

马尔科夫假设：

1. 当一个随机过程在给定现在状态及所有过去状态情况下，其未来状态的条件概率分布仅依赖于当前状态， $p(x_{t}|x_{0:t-1},z_{1:t-1},u_{1:t})=p(x_{t}|x_{t-1},u_{t})$

2. 当前时刻观测值只和当前时刻状态有关， $p(z_{t}|x_{0:t},z_{0:t-1},u_{1:t})=p(z_{t}|x_{t})$

基本问题1 ： 根据之前的观测、控制序列预测当前时刻的状态：

$p(x_{t}|z_{1:t-1},u_{1:t})=\overline{bel}(x_{t})$

基本问题2： 获得当前时刻的传感器观测量后，重新评定当前状态,估计得到最优状态;

$p(x_{t}|z_{1:t},u_{1:t})=bel(x_{t})$

1.3 贝叶斯滤波：

贝叶斯滤波的核心问题：确定上一时刻最优状态估计、当前时刻观测值和当前时刻所需要估计的最优状态的联系，即确定 $bel(x_{t-1})$ 、 $bel(x_{t})$ 、观测 $z_{t}$ 三者的联系。

$bel(x_{t})=p(x_{t}|z_{1:t},u_{1:t})$ 根据公式3 可以转换成：

$p(x_{t}|z_{1:t},u_{1:t})=\frac{p(z_{t}|x_{t},z_{1:t-1},u_{1:t})p(x_{t}|z_{1:t-1},u_{1:t})}{p(z_{t}|z_{1:t-1},u_{1:t})}$

$=\frac{p(z_{t}|x_{t},z_{1:t-1},u_{1:t})p(x_{t}|z_{1:t-1},u_{1:t})}{p(z_{t})}$

$=\eta p(z_{t}|x_{t},z_{1:t-1},u_{1:t})p(x_{t}|z_{1:t-1},u_{1:t})$

根据马尔科夫第二个假设：

$p(x_{t}|z_{1:t},u_{1:t})=\eta p(z_{t}|x_{t},z_{1:t-1},u_{1:t})p(x_{t}|z_{1:t-1},u_{1:t})$

$=\eta p(z_{t}|x_{t})p(x_{t}|z_{1:t-1},u_{1:t})$

因此有了： $bel(x_{t})=\eta p(z_{t}|x_{t})\overline{bel}(x_{t})$ ，到这里就有些疑问了， $\overline{bel}(x_{t})$ 和 $bel(x_{t-1})$ 有什么关

系呢？根据全概率公式和公式3 ，可以得到如下形式：

$\overline{bel}(x_{t})=p(x_{t}|z_{1:t-1},u_{1:t})$

$=\int p(x_{t}|x_{t-1},z_{1:t-1},u_{1:t})p(x_{t-1}|z_{1:t-1},u_{1:t})dx_{t-1}$

$=\int p(x_{t}|x_{t-1},u_{t})p(x_{t-1}|z_{1:t-1},u_{1:t-1})dx_{t-1}$

$=\int p(x_{t}|x_{t-1},u_{t})bel(x_{t-1})dx_{t-1}$

贝叶斯滤波核心理念如下：

loop :

$\overline{bel}(x_{t})=\int p(x_{t}|x_{t-1},u_{t})bel(x_{t-1})dx_{t-1}$ 公式4 (预测）

$bel(x_{t})=\eta p(z_{t}|x_{t})\overline{bel}(x_{t})$ 公式5 (测量更新）

到这里是不是得再举个栗子？

example : 让一个小机器人通过摄像头来判断一扇门是不是开着的

初始状态： $bel(X_{0}=open)=0.5$ , $bel(X_{0}=closed)=0.5$

观测模型假设是带噪声的：

$p(Z_{t}=senseopen|X_{t}=open)=0.6$

$p(Z_{t}=senseclosed|X_{t}=open)=0.4$

$p(Z_{t}=senseopen|X_{t}=closed)=0.2$

$p(Z_{t}=senseclosed|X_{t}=closed)=0.8$

控制量的取值空间为 { ${push , donothing}$ }

带控制量的状态转移方式如下：

$p(X_{t}=open|U_{t}=push,X_{t-1}=open)=1$

$p(X_{t}=closed|U_{t}=push,X_{t-1}=open)=0$

$p(X_{t}=open|U_{t}=push,X_{t-1}=closed)=0.8$

$p(X_{t}=closed|U_{t}=push,X_{t-1}=closed)=0.2$

$p(X_{t}=open|U_{t}=donothing,X_{t-1}=open)=1$

$p(X_{t}=closed|U_{t}=donothing,X_{t-1}=open)=0$

$p(X_{t}=open|U_{t}=donothing,X_{t-1}=closed)=0$

$p(X_{t}=closed|U_{t}=donothing,X_{t-1}=closed)=1$

loop index1: 假设在 $t=0$ 时刻，机器人对门没有做任何动作，即 $U_{1}=donothing$

预测：

$\overline{bel}(x_{1})=\int p(x_{1}|x_{t0},u_{t})bel(x_{0})dx_{0}$

$=\sum_{x_{0}}p(x_{1}|u_{1},x_{0})bel(x_{0})$ $=p(x_{1}|U_{1}=donothing,X_{0}=open)bel(X_{0}=open)+p(x_{1}|U_{1}=donothing,X_{0}=closed)bel(X_{0}=closed)$

$\overline{bel}(X_{1}=open)=p(open|U_{1}=donothing,X_{0}=open)bel(X_{0}=open)+p(open|U_{1}=donothing,X_{0}=closed)bel(X_{0}=closed) = 0.5$

$\overline{bel}(X_{1}=closed)=p(closed|U_{1}=donothing,X_{0}=open)bel(X_{0}=open)+p(closed|U_{1}=donothing,X_{0}=closed)bel(X_{0}=closed) = 0.5$

更新：观测结果为门是开着的

$bel(x_{t})=\eta p(z_{t}|x_{t})\overline{bel}(x_{t})$

$=\eta p(Z_{1}=senseopen|x_{1})\overline{bel}(x_{1})$

$bel(X_{1}=open)=0.3\eta$

$bel(X_{1}=closed)=0.1\eta$

根据概率总和为1,得出 $bel(X_{1}=open)=0.75$ ， $p(x=closed)=0.25$

loop index2: 假设下一时刻，机器人对门做push动作，即 $U_{2}=push$ , $bel(X_{1}=open)=0.75$ ,

$bel(X_{1}=closed)=0.25$

预测 :

$\overline{bel}(X_{2}=open)=0.95$

$\overline{bel}(X_{2}=closed)=0.05$

更新：观测结果为门是开着的

$bel(X_{2}=open)=0.983$

$bel(X_{2}=closed)=0.017$

1.4 线性高斯系统的贝叶斯滤波

复盘一下，多维高斯分布的表示形式为：

$p(x)=det(2\pi \Sigma )^{-\frac{1}{2}}exp(-\frac{1}{2}(x-\mu )^{T}\Sigma ^{-1}(x-\mu ))$ ，

简易表示为： $N(x;\mu ,\Sigma )$

线性高斯系统：需满足如下两个条件

条件1： 状态转移矩阵可表示成如下形式的系统，即当前时刻的状态量和上一时刻的状态量满足如下关系：

$x_{t}=A_{t}x_{t-1}+B_{t}u_{t}+\varepsilon _{t}$ 公式6 (状态转移方程)

$A_{t},B_{t}$ 为系统矩阵， $\varepsilon _{t}$ 为均值为0,方差为 $R_{t}$ 的高斯随机变量，通常情况下， $x_{t},u_{t}$ 的维度大于1，将公式6写成概率的形式为 $N(x_{t};A_{t}x_{t-1}+B_{t}u_{t} ,R_{t})$ ：

$p(x_{t}|x_{t-1},u_{t})= det(2\pi R_{t})^{-\frac{1}{2}}exp(-\frac{1}{2}(x_{t}-A_{t}x_{t-1}-B_{t}u_{t})^{T}R_{t}^{-1}(x_{t}-A_{t}x_{t-1}-B_{t}u_{t}))$

条件2：当前时刻观测值和状态量需满足如下形式

$z_{t}=C_{t}x_{t}+\delta _{t}$ 公式7 (观测方程)

$C_{t}$ 为观测矩阵， $\delta_{t}$ 为均值为0,方差为 $Q_{t}$ 的高斯随机变量,将公式7写成概率的形式为 $N(z_{t},C_{t}x_{t},Q_{t})$ :

$p(z_{t}|x_{t})=det(2\pi Q_{t})^{-\frac{1}{2}}exp(-\frac{1}{2}(z_{t}-C_{t})^{T}Q_{t}^{-1}(z_{t}-C_{t}))$

线性高斯系统的贝叶斯滤波：卡尔曼滤波

结合贝叶斯滤波的核心部分：预测（公式4）、更新（公式5），

线性高斯系统的预测为：

$\overline{bel}(x_{t})=\int p(x_{t}|x_{t-1},u_{t})bel(x_{t-1})dx_{t-1}$

$=\eta \int exp(-\frac{1}{2}(x_{t}-A_{t}x_{t-1}-B_{t}u_{t})^{T}R_{t}^{-1}(x_{t}-A_{t}x_{t-1}-B_{t}u_{t})) exp(-\frac{1}{2}(x_{t-1}-\mu _{t-1})^{T}Q_{t}^{-1}(x_{t-1}-\mu _{t-1}))dx_{t-1}$

........ (推导步骤太长，偷个懒，此处省略800字)

最后求得的结果为:

$\overline{bel}(x_{t})=N({x_{t};\overline{\mu }}_{t},\overline{\Sigma }_{t})$

$\overline{\mu }_{t}=A_{t}\mu _{t-1}+B_{t}u_{t}$

$\overline{\Sigma }_{t}=A_{t}\Sigma _{t-1}A_{t}^{T}+R^{T}$

线性高斯系统的测量更新为：

$bel(x_{t})=\eta p(z_{t}|x_{t})\overline{bel}(x_{t})$

$=\eta N(z_{t};C_{t}x_{t},Q_{t})N(x_{t},\overline{\mu }_{t},\overline{\Sigma }_{t})$

最后求得的结果为:

$bel(x_{t})=N(x_{t},\mu _{t},\Sigma _{t})$

$\mu _{t}=\overline{\mu }_{t}+K_{t}(z_{t}-C_{t}\overline{\mu }_{t})$

$\Sigma _{t}=(I-K_{t}C_{t})\overline{\Sigma }_{t}$

$K_{t}=\overline{\Sigma }_{t}C_{t}^{T}(C_{t}\overline{\Sigma }_{t}C_{t}^{T}+Q_{t})^{-1}$ (卡尔曼增益)

综上卡尔曼滤波流程为：

step1: $\overline{\mu }_{t}=A_{t}\mu _{t-1}+B_{t}u_{t}$

step2: $\overline{\Sigma }_{t}=A_{t}\Sigma _{t-1}A_{t}^{T}+R^{T}$

step3: $K_{t}=\overline{\Sigma }_{t}C_{t}^{T}(C_{t}\overline{\Sigma }_{t}C_{t}^{T}+Q_{t})^{-1}$

step4: $\mu _{t}=\overline{\mu }_{t}+K_{t}(z_{t}-C_{t}\overline{\mu }_{t})$

step5: $\Sigma _{t}=(I-K_{t}C_{t})\overline{\Sigma }_{t}$

代码示例：

 * @class KalmanFilter
 *
 * @brief Implements a discrete-time Kalman filter.
 *
 * @param XN dimension of state
 * @param ZN dimension of observations
 * @param UN dimension of controls
 */
template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
class KalmanFilter {
 public:
  /**
   * @brief Constructor which defers initialization until the initial state
   * distribution parameters are set (with SetStateEstimate),
   * typically on the first observation
   */
  KalmanFilter() {
    F_.setIdentity();
    Q_.setZero();
    H_.setIdentity();
    R_.setZero();
    B_.setZero();

    x_.setZero();
    P_.setZero();
    y_.setZero();
    S_.setZero();
    K_.setZero();
  }

  /**
   * @brief Sets the initial state belief distribution.
   *
   * @param x Mean of the state belief distribution
   * @param P Covariance of the state belief distribution
   */
  void SetStateEstimate(const Eigen::Matrix<T, XN, 1> &x,
                        const Eigen::Matrix<T, XN, XN> &P) {
    x_ = x;
    P_ = P;
    is_initialized_ = true;
  }

  /**
   * @brief Constructor which fully initializes the Kalman filter
   * @param x Mean of the state belief distribution
   * @param P Covariance of the state belief distribution
   */
  KalmanFilter(const Eigen::Matrix<T, XN, 1> &x,
               const Eigen::Matrix<T, XN, XN> &P)
      : KalmanFilter() {
    SetStateEstimate(x, P);
  }

  /**
   * @brief Destructor
   */
  virtual ~KalmanFilter() {}

  /**
   * @brief Changes the system transition function under zero control.
   *
   * @param F New transition matrix
   */
  void SetTransitionMatrix(const Eigen::Matrix<T, XN, XN> &F) { F_ = F; }

  /**
   * @brief Changes the covariance matrix of the transition noise.
   *
   * @param Q New covariance matrix
   */
  void SetTransitionNoise(const Eigen::Matrix<T, XN, XN> &Q) { Q_ = Q; }

  /**
   * @brief Changes the observation matrix, which maps states to observations.
   *
   * @param H New observation matrix
   */
  void SetObservationMatrix(const Eigen::Matrix<T, ZN, XN> &H) { H_ = H; }

  /**
   * @brief Changes the covariance matrix of the observation noise.
   *
   * @param R New covariance matrix
   */
  void SetObservationNoise(const Eigen::Matrix<T, ZN, ZN> &R) { R_ = R; }

  /**
   * @brief Changes the covariance matrix of current state belief distribution.
   *
   * @param P New state covariance matrix
   */
  void SetStateCovariance(const Eigen::Matrix<T, XN, XN> &P) { P_ = P; }

  /**
   * @brief Changes the control matrix in the state transition rule.
   *
   * @param B New control matrix
   */
  void SetControlMatrix(const Eigen::Matrix<T, XN, UN> &B) { B_ = B; }

  /**
   * @brief Get the system transition function under zero control.
   *
   * @return Transition matrix.
   */
  const Eigen::Matrix<T, XN, XN> &GetTransitionMatrix() const { return F_; }

  /**
   * @brief Get the covariance matrix of the transition noise.
   *
   * @return Covariance matrix
   */
  const Eigen::Matrix<T, XN, XN> &GetTransitionNoise() const { return Q_; }

  /**
   * @brief Get the observation matrix, which maps states to observations.
   *
   * @return Observation matrix
   */
  const Eigen::Matrix<T, ZN, XN> &GetObservationMatrix() const { return H_; }

  /**
   * @brief Get the covariance matrix of the observation noise.
   *
   * @return Covariance matrix
   */
  const Eigen::Matrix<T, ZN, ZN> &GetObservationNoise() const { return R_; }

  /**
   * @brief Get the control matrix in the state transition rule.
   *
   * @return Control matrix
   */
  const Eigen::Matrix<T, XN, UN> &GetControlMatrix() const { return B_; }

  /**
   * @brief Updates the state belief distribution given the control input u.
   *
   * @param u Control input (by default, zero)
   */
  void Predict(
      const Eigen::Matrix<T, UN, 1> &u = Eigen::Matrix<T, UN, 1>::Zero());

  /**
   * @brief Updates the state belief distribution given an observation z.
   *
   * @param z Observation
   */
  void Correct(const Eigen::Matrix<T, ZN, 1> &z);

  /**
   * @brief Gets mean of our current state belief distribution
   *
   * @return State vector
   */
  Eigen::Matrix<T, XN, 1> GetStateEstimate() const { return x_; }

  /**
   * @brief Gets covariance of our current state belief distribution
   *
   * @return Covariance matrix
   */
  Eigen::Matrix<T, XN, XN> GetStateCovariance() const { return P_; }

  /**
   * @brief Gets debug string containing detailed information about the filter.
   *
   * @return Debug string
   */
  std::string DebugString() const;

  /**
   * @brief Get initialization state of the filter
   * @return True if the filter is initialized
   */
  bool IsInitialized() const { return is_initialized_; }

 private:
  // Mean of current state belief distribution
  Eigen::Matrix<T, XN, 1> x_;

  // Covariance of current state belief dist
  Eigen::Matrix<T, XN, XN> P_;

  // State transition matrix under zero control
  Eigen::Matrix<T, XN, XN> F_;

  // Covariance of the state transition noise
  Eigen::Matrix<T, XN, XN> Q_;

  // Observation matrix
  Eigen::Matrix<T, ZN, XN> H_;

  // Covariance of observation noise
  Eigen::Matrix<T, ZN, ZN> R_;

  // Control matrix in state transition rule
  Eigen::Matrix<T, XN, UN> B_;

  // Innovation; marked as member to prevent memory re-allocation.
  Eigen::Matrix<T, ZN, 1> y_;

  // Innovation covariance; marked as member to prevent memory re-allocation.
  Eigen::Matrix<T, ZN, ZN> S_;

  // Kalman gain; marked as member to prevent memory re-allocation.
  Eigen::Matrix<T, XN, ZN> K_;

  // true iff SetStateEstimate has been called.
  bool is_initialized_ = false;
};

template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
inline void KalmanFilter<T, XN, ZN, UN>::Predict(
    const Eigen::Matrix<T, UN, 1> &u) {
  CHECK(is_initialized_);

  x_ = F_ * x_ + B_ * u;

  P_ = F_ * P_ * F_.transpose() + Q_;
}

template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
inline void KalmanFilter<T, XN, ZN, UN>::Correct(
    const Eigen::Matrix<T, ZN, 1> &z) {
  CHECK(is_initialized_);
  y_ = z - H_ * x_;

  S_ = static_cast<Eigen::Matrix<T, ZN, ZN>>(H_ * P_ * H_.transpose() + R_);

  K_ = static_cast<Eigen::Matrix<T, XN, ZN>>(P_ * H_.transpose() *
                                             PseudoInverse<T, ZN>(S_));

  x_ = x_ + K_ * y_;

  P_ = static_cast<Eigen::Matrix<T, XN, XN>>(
      (Eigen::Matrix<T, XN, XN>::Identity() - K_ * H_) * P_);
}

}

2. 常用数据关联方法(匈牙利算法、概率数据关联算法PDA)

数据关联，在自动驾驶感知中可以理解为，确定你所获取的每个数据的来源，即判断检测的当前帧每个结果分别来源于（跟踪上障碍物的序列tracked list）哪个物体。下面介绍两种典型的方法：

2.1 匈牙利算法(概念有点多，但比较简单)

2.1.1 图论基本概念

图(graph): 由顶点集合和顶点间的二元关系集合(即边的集合或弧的集合)组成的数据结构,通常可以用 G(V, E)来表示。

顶点集合(vertext set)和边的集合(edge set)分别用 V(G)和 E(G)表示。

V(G)中的元素称为顶点(vertex),用 u、 v 等符号表示;顶点个数称为图的阶(order),通常用 n 表示。

E(G)中的元素称为边(edge),用 e 等符号表示;边的个数称为图的边数(size),通常用 m 表示。

图2.1

图2.1 (a)所示的图:

顶点集合 V(G) = {1, 2, 3, 4, 5, 6}

边的集合为 E(G) = {(1, 2), (1, 3), (2, 3), (2, 4), (2, 5), (2, 6), (3, 4), (3, 5), (4, 5)}

每个元素(u, v)为一对顶点构成的无序对，表示与顶点u 和 v 相关联的一条无向边,这条边没有特定的方向,因此(u, v)与(v, u)是同一条的边。如果图中所有的边都没有方向性,这种图称为无向图(undirected graph)

图2.1(b) 所示的图：

顶点集合: V(G) = {1, 2, 3, 4, 5, 6, 7}

边的集合为: E(G) = {<1, 2>, <2, 3>, <2, 5>, <2, 6>, <3, 5>, <4, 3>, <5, 2>, <5, 4>, <6, 7>},

每个元素<u, v>为一对顶点构成的有序对(用尖括号括起来),表示从顶点 u 到顶点 v 的有向边,其中 u 是这条有向边的起始顶点(start vertex,简称起点),v 是这条有向边的终止顶点(end vertex,简称终点),如果图中所有的边都是有方向性的,这种图称为有向图(directed graph).

图2.1(c) 有向图的基图：

忽略有向图所有边的方向,得到的无向图称为该有向图的基图。

2.1.2 二分图

二部图(bipartite graph) :

设无向图为 G(V, E) , 它的顶点集合 V 包含两个没有公共元素的子集: $X=\left \{ x_{1},x_{2},..., x_{s} \right \}$ , $Y=\left \{ y_{1},y_{2},..., y_{t} \right \}$ , 元素个数分别为 s 和 t; 并且 $x_{i}$ 与 $x_{j}$ 之间 $(1\leqslant i, j\leqslant s)$ ， $y_{l}$ 与 $y_{r}$ 之间 $(1\leq l, r\leq t)$ 没有边连接,则称G为二部图,有的文献也称为二分图。

图2.2

完全二部图(complete bipartite graph):

在二部图 G 中,如果顶点集合 $X$ 中每个顶点 $x_{i}$ 与顶点集合 $Y$ 中每个顶点 $y_{l}$ 都有边相连,则称G为完全二部图,记为 $K_{s,t}$ , $s$ 和 $t$ 分别为集合 $X$ 和集合 $Y$ 中的顶点个数。在完全二部图 $K_{s,t}$ 中一共有 $s\times t$ 条边. 图2.2中(b)、(c)均为完全二部图.

2.1.3 匹佩问题

边覆盖点、边覆盖集、边覆盖数:

覆盖与边覆盖集: 设无向图为 G(V, E),边的集合 E*⊆E,若对于∀v∈V , ∃e∈E*,使得v与 e 相关联,则称 e 覆盖 v,并称 E*为边覆盖集(edge covering set),或简称边覆盖。

图2.3

在图2.3(a)中,取 E* = $\left \{ e_{1},e_{4},e_{7} \right \}$ ,则 E*就是图 G 的一个边覆盖集,因为图 G 中每个顶点都被 E*中某条边“覆盖”住了。通俗地讲,所谓边覆盖集 E*,就是 G 中所有的顶点都是 E*中某条边的邻接顶点(边覆盖顶点)。

极小边覆盖:若边覆盖 E*的任何真子集都不是边覆盖, 则称 E*是极小边覆盖。

最小边覆盖: 边数最少的边覆盖集称为最小边覆盖。

边覆盖数(edge covering number): 最小的边覆盖所含的边数称为边覆盖数, 记作 $\alpha _{1}$

在图2.3(a)中, $\left \{ e_{1},e_{4},e_{7} \right \}$ 和 $\left \{ e_{2},e_{5},e_{6},e_{7} \right \}$ 都是极小边覆盖, $\left \{ e_{2},e_{5},e_{6},e_{7} \right \}$ 是最小边覆盖, $\alpha _{1}$ = 3,

在图2.3(b)中, $\left \{ e_{1},e_{3},e_{6} \right \}$ 和 $\left \{ e_{2},e_{4},e_{8} \right \}$ 都是极小边覆盖,也都是最小边覆盖,因此 $\alpha _{1}$ = 3。

边独立集、边独立数:

边独立集(匹配):设无向图为 G(V, E),边的集合 E*⊆E,若 E*中任何两条边均不相邻,则称 E*为 G 的边独立集(edge independent set),也称 E*为 G 的匹配(matching)。所谓任何两条边均不相邻,通俗地讲,就是任何两条边都没有公共顶点.

图2.4

在2.4(a)图中，取 E* = $\left \{ e_{1},e_{4},e_{7} \right \}$ ,则 E*就是图 G 的一个边独立集,因为 E*中每两条边都没有公共顶点。

极大匹配: 若在 E*中加入任意一条边所得到的集合都不是匹配,则称 E*为极大匹配。

最大匹配: 边数最多的匹配称为最大匹配。

边独立数(edge independent number,也称匹配数):最大匹配的边数称为边独立数或匹配数 $\beta _{1}$ 。

在2.4(a)图中, $\left \{ e_{2},e_{6} \right \}$ 、 $\left \{ e_{3}, e_{5} \right \}$ 和 $\left \{ e_{1},e_{4},e_{7} \right \}$ 都是极大匹配, $\left \{ e_{1},e_{4},e_{7} \right \}$ 是最大匹配,因此 $\beta _{1}=3$

在2.4(b)图中, $\left \{ e_{1},e_{3} \right \}$ 、 $\left \{ e_{2},e_{4} \right \}$ 和 $\left \{ e_{4},e_{7} \right \}$ 都是极大匹配,也都是最大匹配,因此 $\beta _{1}=2$ 。

盖点与未盖点:设 v 是图 G 的一个顶点,如果 v 与 M 中的某条边关联,则称 v 为 M 的盖点(有的文献上也称为 M 饱和点)。如果 v 不与任意一条属于匹配 M 的边相关联,则称 v 是匹配 M的未盖点(相应地,有的文献上也称为非 M 饱和点)。所谓盖点,就是被匹配中的边盖住了,而未盖点就是没有被匹配M中的边“盖住”的顶点。

图2.5

在2.5图 (a)所示的无向图中,取定 M = $\left \{ e_{1},e_{4} \right \}$ ,M 中的边用粗线标明,则顶点 $v_{1}$ 与 $v_{2}$ 被 M 所匹配; $v_{1}$ 、 $v_{2}$ 、 $v_{3}$ 和 $v_{4}$ 是 M 的盖点, $v_{5}$ 和 $v_{6}$ 是 M 的未盖点。而在图(b)中,取定 M = $\left \{ e_{1},e_{4},e_{7} \right \}$ 则M中不存在未盖点。

完美匹配: 对于一个图 G 与给定的一个匹配 M,如果图 G 中不存在 M 的未盖点,则称匹配M为图 G 的完美匹配。

二部图的完备匹配与完美匹配:

二部图的完备匹配:设无向图 G(V, E)为二部图,它的两个顶点集合为 X 和 Y,且|X|≤|Y|,M为 G 中的一个最大匹配,且|M| = |X|,则称 M 为 X 到 Y 的二部图 G 的完备匹配。若|X| = |Y|,则该完备匹配覆盖住G 的所有顶点,所以该完备匹配也是完美匹配。

图2.6

图2.6所示的3个二部图,其中图(a)和图(b)中取定的匹配 M 都是完备匹配,而图(c)中不存在完备匹配。

example：某公司有工作人员 $\left \{ x_{1} , x_{2} , ..., x_{m} \right \}$ ,他们去做工作 $\left \{ y_{1} , y_{2} , ..., y_{n} \right \}$ ，问能否恰当地安排使得每个人都分配到一项合适的工作。

二部图的最佳匹配：(在此引出概念，用于多目标跟踪)

设 G(V, E)为加权二部图,它的两个顶点集合分别为 $X = \left \{ x_{1} , x_{2} , ..., x_{m} \right \} , Y = \left \{ y_{1} , y_{2} , ..., y_{n} \right \}$ $W(x_{i},y_{k})\geq 0$ 。表示工作人员 $x_{i}$ 做工作 $y_{k}$ 时的效益,权值总和最大的完备匹配称为二部图的最佳匹配。

2.1.4 二分图的最大匹配之匈牙利算法

交错轨:设 P 是图 G 的一条轨(即路径),M 是图 G 中一个给定的匹配,如果 P 的任意两条相邻的边一定是一条属于匹配 M 而另一条不属于 M,则称 P 是关于 M 的一条交错轨。

图2.7

在图2.7 中，取定M= $\left \{ e_{4},e_{6},e_{10} \right \}$ ,则图(b)、(c)所示的路径都是交错轨

可增广轨:对于一个给定的图 G 和匹配 M,两个端点都是未盖点的交错轨称为关于 M 的可增广轨。图2.7(b)所示的交错轨的两个端点 $v_{2},v_{11}$ 都是匹配 M 的未盖点,所以这条轨是可增广轨,而图(c)所示的交错轨不是可增广轨。

可增广轨的含义:对于图 G 的一个匹配 M 来说,如果能找到一条可增广轨 P,那么这个匹配M 一定可以通过下述方法改进成一个多包含一条边的匹配 Ms (即匹配 M 扩充了)，即把 P 中原来属于匹配 M 的边从匹配 M 中去掉(粗边改成细边),而把 P 中原来不属于 M 的边加到匹配 Ms 中去(细边改成粗边),变化后的匹配 Ms 恰好比原匹配 M 多一条边。

example: 对图 2.7(a)中 G 的一个匹配 M,找到图2.8(a)所示的一条可增广轨,那么按照前面所述的方法可以将原匹配进行扩充,得到图2.8(b)所示的新匹配 Ms,Ms 比 M 多一条边。

图2.8

理解上面的内容之后，匈牙利算法基本上就很好理解了：

匈牙利算法的原理:从当前匹配 M(如果没有匹配,则取初始匹配为 M=Ø)出发,检查每一个未盖点,然后从它出发寻找可增广路,找到可增广路,则沿着这条可增广路进行扩充,直到不存在可增广路为止。根据从未盖点出发寻找可增广路搜索的方法,可以分为:
1) DFS 增广（深度优先）
2) BFS 增广（广度优先）
3) 多增广路(Hopcroft-Karp 算法)

DFS 增广算法解读：

#define MAXN 10
//MAXN 为表示 X 集合和 Y 集合顶点个数最大值的符号常量
int nx, ny; //X 和 Y 集合中顶点的个数
int g[MAXN][MAXN]; //邻接矩阵,g[i][j]为 1 表示 Xi 和 Yj 有边相连
int cx[MAXN] , cy[MAXN]; //cx[i]表示最终求得的最大匹配中与 Xi 匹配的 Y 顶点, cy[i]同理

//DFS 算法中记录顶点访问状态的数组, mk[i] = 0 表示未访问过,为 1 表示访问过
int mk[MAXN] ;
// 从 X 集合中的顶点 u 出发,用深度优先的策略寻找增广路
//( 这种增广路只能使当前的匹配数增加 1)
int path( int u )
{
  for( int v = 0 ; v < ny ; v++ )
  // 考虑所有 Yi 顶点 v
  {
    if( g[u][v] && !mk[v] ) //v 与 u 邻接,且没有访问过
    {
       mk[v] = 1; // 访问 v
       // 如果 v 没有匹配,或者 v 已经匹配了,但从 cy[v] 出发可以找到一条增广路
       if( cy[v] == -1 || path( cy[v] ) )
       // 注意如果前一个条件成立,则不会递归调用
       {
          cx[u] = v; // 把 v 匹配给 u
          cy[v] = u; // 把 u 匹配给 v
          return 1; // 找到可增广路
       }
    }
  }
  return 0 ; // 如果不存在从 u 出发的增广路
}
int MaxMatch( ) // 求二部图最大匹配的匈牙利算法
{
  int res = 0; // 所求得的最大匹配
  memset( cx , 0xff , sizeof(cx) ) ; // 从 0 匹配开始增广 , 将 cx 和 cy 各元素初始化为 -1
  memset( cy , 0xff , sizeof(cy) ) ;
  for( int i = 0 ; i <= nx ; i++ )
  {
    if( cx[i] == -1 ) // 从每个未盖点出发进行寻找增广路
    {
       memset( mk , 0 , sizeof(mk) ) ;
       res += path(i) ; // 每找到一条增广路,可使得匹配数加 1
    }
  }
  return res;
}

接下来2.9(a)所示的二部图为例解释匈牙利算法求解过程(DFS 增广)。

在图(b)中,从顶点 $x_{1}$ 出发进行 DFS 搜索后,发现 $y_{2}$ 跟 $x_{1}$ 邻接且没有匹配,所以将 $x_{1}$ 匹配给 $y_{2}$ ,从 $x_{1}$ 出发的搜索过程结束。

图(c)~(e)为从顶点 $x_{2}$ 出发进行 DFS 搜索的过程,在图(c)中,发现 $y_{2}$ 跟 $x_{2}$ 邻接但已经匹配给 $x_{1}$ 了,所以递归地从 $x_{1}$ 出发进行 DFS 搜索,从而找到下一个跟 $x_{1}$ 邻接且没有匹配的顶点 $y_{3}$ ,从而将 $x_{1}$ 改为匹配给 $y_{3}$ 并返回;返回到 $x_{2}$ 的搜索过程后,将空出来的 $y_{2}$ 匹配 $x_{2}$ ,如图(d)所示,至此从 $x_{2}$ 出发的搜索过程结束。

在图(f)中,将 $x_{3}$ 匹配给 $y_{1}$ 。至此,算法求解结束,求得最大匹配为图(f),匹配数为3

图2.9 匈牙利算法求解过程(DFS 增广)

BFS 增广算法解读：

#define MAXN 10
//MAXN 为表示 X 集合和 Y 集合顶点个数最大值的符号常量
int nx, ny; //X 和 Y 集合中顶点的个数
int g[MAXN][MAXN]; //邻接矩阵,g[i][j]为 1 表示 Xi 和 Yj 有边相连
int cx[MAXN] , cy[MAXN]; //cx[i]表示最终求得的最大匹配中与 Xi 匹配的 Y 顶点, cy[i]同理


int pred[MAXN];
int queue[MAXN];
int MaxMatch( )
{
  int i, j, y;
  // 是用来记录交错轨的,同时也用来记录 Y 集合中的顶点是否遍历过
  // 实现 BFS 搜索用到的队列 ( 用数组模拟 )
  int cur, tail; // 表示队列头和尾位置的下标
  int res = 0; // 所求得的最大匹配数
  memset( cx , 0xff , sizeof(cx) ); // 初始化所有点为未被匹配的状态
  memset( cy , 0xff , sizeof(cy) );
  for( i = 0; i < nx; i++ )
  {
    if( cx[i] != -1 ) continue;
    // 对 X 集合中的每个未盖点 i 进行一次 BFS 找交错轨
    for( j = 0; j < ny; j++ ) pred[j] = -2;
    //-2 表示初始值
    cur = tail = 0; // 初始化 BFS 的队列
    for( j = 0; j < ny; j++ ) // 把 i 的邻接点顶都入队列
    {
      if( g[i][j] )
      {
         pred[j] = -1; queue[tail++] = j; //-1 表示遍历到,是邻接顶点
      }
    }
    while( cur < tail )//BFS
    {
       y = queue[cur];
       if( cy[y] == -1 ) break;
       // 找到一个未被匹配的点,则找到了一条交错轨
       cur++;
       //y 已经被匹配给 cy[y] 了,从 cy[y] 出发,将它的邻接顶点入队列
       for( j = 0; j < ny; j++ )
       {
          if( pred[j] == -2 && g[cy[y]][j] )
          {
             pred[j] = y;
             queue[tail++] = j;
          }
       }
    }
    if( cur == tail ) continue; // 没有找到交错轨
    while( pred[y] > -1 ) // 更改交错轨上匹配状态
    {
       cx[ cy[ pred[y] ] ] = y;
       cy[y] = cy[ pred[y] ];
       y = pred[y];
    }
    cy[y] = i; cx[i] = y;
    res++; // 匹配数加 1
 }
 return res;
}

接下来以图2.10(a)所示的二部图为例解释匈牙利算法求解过程(BFS 增广)。在图(c)中, $x_{2}$ 唯一的邻接顶点 $y_{2}$ 已经匹配给 $x_{1}$ 了,这是从 $x_{1}$ 出发进行 BFS 搜索,将 $x_{1}$ 的所有邻接顶点入队列,从而找到一条增广路 $x_{2}\rightarrow y_{2}\rightarrow x_{1}\rightarrow y_{3}$ 。在图(d)中更改交错轨上的匹配状态,使得匹配数增加 1。最终求得的最大匹配为图(f),匹配数为 3。

图2.10 匈牙利算法求解过程(BFS 增广)

2.1.5 匈牙利算法在多目标跟踪数据关联中的应用

2.2 概率数据关联算法

2.2.1 关联门

2.2.2 最小均方差估计

2.2.3 完备事件组

2.2.4 概率数据关联算法pda

2.2.5 pda在多目标跟踪数据关联中的应用

3. 交互多模型(Interacting Multiple Model ，IMM)

考虑每个物体可能存在不同的运动状态（匀速、匀速圆周、匀加速等），采用单一的模型估计可能会存在较大的偏差，遂引入交互多模型来解决改问题。

交互多模型介绍：

4. 多目标跟踪(MOT)

落羽归尘

关注

4
点赞
踩
5

收藏

觉得还不错? 一键收藏
1
评论
Traditional Multiple Objects Tracking (updating)

1. 卡尔曼滤波的详细推导(KF、EKF、UKF)2. 交互多模型(IMM)3. 数据关联(匈牙利、概率密度)4.多目标跟踪(MOT)
复制链接

扫一扫