基本原理

状态估计问题依赖于对象运动模型来预测和更新对象状态。然而，在城市情况下，被跟踪对象不一定以明确定义的模型移动，若只使用一种系统动态模型的卡尔曼滤波器去对一个系统动态模型处于变化之中的目标进行状态跟踪估测的话，会产生不准确性。那么一个很直观的想法就是，使用几种它可能处于的系统动态模型的卡尔曼滤波器模型去分别对它进行状态估测，然后将这些结果以一定的比例进行融合。基于这种思路开发出来的算法就是Interacting Multiple Model(IMM)模型。

通常，动态对象以可预测的模式移动，而可以看到噪声以更随机的模式移动，非线性运动模型使用非线性 KF 作为状态估计器，例如 UKF。 IMM常用于车辆对象跟踪，旨在为具有模糊动态行为的对象生成更好的估计。

IMM 使用的这种方法如下：

使用代表不同的多个模型来运行状态估计过滤器，然后对最可能的估计结果进行排序。通常，IMM 的输出要么是单个滤波器的概率加权组合，要么是具有最高概率的滤波器的输出。 IMM 由 j 个并行运行的滤波器组成，每个滤波器使用不同的运动模型来表示状态预测和测量，对于单个跟踪，需要维护 j 个状态过滤器。
预测的状态向量和协方差矩阵是由每个单独的滤波器估计相对于预测的模型概率生成的。接下来，将每个估计值与当前测量值进行比较，以更新模型匹配概率。在更新步骤中，动态目标模型的计算概率被添加到相应滤波器产生的更新状态向量和误差协方差矩阵信息中。
在所谓的合并和混合步骤中使用模型概率。
- 合并：每个运动模型的状态向量和协方差矩阵被合并成一个单一的状态向量和协方差矩阵。
- 混合：使用加权估计为每个模型计算新的状态估计、协方差矩阵和相应的模型概率。

流程图

r个运动模型的第j个滤波器的IMM流程如下：

公式推导

推导如下，我们假设具有运动模型不确定性的系统演变为跳跃马尔可夫线性系统（JLMS），然后将每个模型j的随机状态方程和测量方程定义为：
xk+1=fj(xk,uk)+wj,kzk=hj(xk,uk)+vj,kx_{k+1}=f_j(x_k,u_k)+w_{j,k} \\ z_k=h_j(x_k,u_k)+v_{j,k}xk+1=fj(xk,uk)+wj,kzk=hj(xk,uk)+vj,k

其中：j = 1, 2…r 是模型集 M={Mj}j=1rM = \{M_j\} ^r_{j=1}M={Mj}j=1r 的一部分，w(j,k)w(j,k)w(j,k)是均值为0，协方差矩阵为vj,kv_{j,k}vj,k的高斯白噪声。 IMM 是 JMLS 的一种变体，它使用 r 个并行运行的运动模型进行状态估计。

基于贝叶斯框架中，推导出IMM的后验概率pdf为：
p(xk,Mk∣zk)p(x_k,M_k|z_k)p(xk,Mk∣zk)

测量值 zkz_kzk，离散状态变量 xkx_kxk 和运动模式的 MkM_kMk，可以使用条件概率引理进一步分解为：
p(xk,Mk∣zk)=p(xk,Mk∣zk)p(Mk∣zk)p(xk,Mk∣zk)=∑j=1rp(xk,Mj,k∣zk)p(Mj,k∣zk)⏟μj,kp(\mathbf x_k,M_{k}| \mathbf z_k)=p(\mathbf x_k,M_{k}| \mathbf z_k)p(M_k| \mathbf z_k) \\ p(\mathbf x_k,M_k| \mathbf z_k) =\sum ^{r}_{j=1}p(\mathbf x_k,M_{j,k}| \mathbf z_k)\underbrace{p(M_{j,k}|\mathbf z_k)}_{\mu_{j,k}}p(xk,Mk∣zk)=p(xk,Mk∣zk)p(Mk∣zk)p(xk,Mk∣zk)=j=1∑rp(xk,Mj,k∣zk)μj,kp(Mj,k∣zk)

其中：μj,k\mu_{j,k}μj,k是k时刻j个运动模型的后验概率，进一步推出：
p(xk−1,Mk∣zk−1)=∑j=1rp(xk−1,Mj,k∣zk−1)μi∣j,k−1p(\mathbf x_{k-1},M_k|\mathbf z_{k-1})=\sum^{r}_{j=1}p(\mathbf x _{k-1},M_{j,k}| \mathbf z_{k-1})\mu_{i|j,k-1}p(xk−1,Mk∣zk−1)=j=1∑rp(xk−1,Mj,k∣zk−1)μi∣j,k−1
其中：μi∣j,k−1\mu_{i|j,k-1}μi∣j,k−1为模型iii到jjj的相关性系数
μi∣j,k−1=pijμi,k−1∑i=1rpijμi,k−1⏟μj,kˉ\mu_{i|j,k-1}=\frac{p_{ij}\mu_{i,k-1}}{\underbrace{ \sum^r_{i=1} p_{ij}\mu_{i,k-1}} _{\bar {\mu_{j,k}}}}μi∣j,k−1=μj,kˉi=1∑rpijμi,k−1pijμi,k−1

μj,kˉ\bar {\mu_{j,k}}μj,kˉ是滤波器 j 在时刻 k 的模型匹配概率，模型转移矩阵可以表示为：
[p11⋯p1r⋮⋱⋮pr1⋯prr](3)\begin{bmatrix} p_{11}&\cdots & p_{1r} \\ \vdots & \ddots & \vdots\\ p_{r1}& \cdots & p_{rr}\\ \end{bmatrix} \tag{3}⎣⎢⎡p11⋮pr1⋯⋱⋯p1r⋮prr⎦⎥⎤(3)

其中：pijp_{ij}pij表示模型iii到模型jjj的转移矩阵

Mixing

混合：IMM 结合每个 j 模型滤波器状态 x^j,k−1\hat x_{j,k-1}x^j,k−1 的加权平均值来确定单个组合估计状态 x^j,k−1∗\hat x^∗_{j,k-1}x^j,k−1∗ 及其对应的方差 P^j,k−1∗\hat P^∗_{j,k-1}P^j,k−1∗。这是通过总结每个模型的联合后验密度来完成的，这个过程称为。因此，x^j,k−1\hat x_{j,k-1}x^j,k−1 和 P^j,k−1∗\hat P^∗_{j,k-1}P^j,k−1∗给出为：
xj,k−1∗=∑i=1rμ(i∣j),k−1x^i,k−1.i=1,2,3,...,r(4)x_{j,k-1}^{*} = \sum^{r}_{i=1} \mu_{(i|j),k-1} \hat x_{i,k-1}. \qquad i=1,2,3,...,r \tag{4}xj,k−1∗=i=1∑rμ(i∣j),k−1x^i,k−1.i=1,2,3,...,r(4)
Pj,k−1∗=∑i=1rμ(i∣j),k−1x^i,k−1[Pi,k−1+(x^j,k−1−x^j,k−1∗(x^j,k−1+x^j,k−1∗)T]P_{j,k-1}^{*} =\sum_{i=1}^r\mu_{(i|j),k-1} \hat x_{i,k-1} [P_{i,k-1}+( \hat x_{j,k-1} - \hat x_{j,k-1}^*( \hat x_{j,k-1}+ \hat x_{j,k-1}^*)^T] Pj,k−1∗=i=1∑rμ(i∣j),k−1x^i,k−1[Pi,k−1+(x^j,k−1−x^j,k−1∗(x^j,k−1+x^j,k−1∗)T]

Prediction

预测是基于混合初始状态xj,k−1∗\mathbf x_{j,k-1}^*xj,k−1∗ 和 Pj,k−1∗\mathbf P^∗_{j,k-1}Pj,k−1∗，由于卡尔曼滤波的效果较好，通常将该算法用于多模型交互跟踪中对目标进行滤波和预测，跟踪过程与单模型卡尔曼滤波算法类似，基于卡尔曼滤波算法，可以求出IMM的预测状态xj,kˉ\bar {\mathbf x_{j,k} }xj,kˉ 和 Pj,kˉ\bar {\mathbf P_{j,k} }Pj,kˉ

xj,kˉ=Fj,kxj,k−1∗Pk/k−1jˉ=Fj,kPj,k−1∗Fj,kT+Qj,k\bar {\mathbf x_{j,k} }= F_{{j,k}} \mathbf x_{j,k-1}^* \\ \bar {\mathbf P_{k/k-1}^j}= F_{j,k} \mathbf P^∗_{j,k-1}F_{j,k}^T+\mathbf Q_{{j,k}} xj,kˉ=Fj,kxj,k−1∗Pk/k−1jˉ=Fj,kPj,k−1∗Fj,kT+Qj,k
其中：Qj,kQ_{{j,k}}Qj,k为外部干扰噪声的协方差，其为非负定阵；

Updates

在IMM算法中，模型更新直接影响算法的有效程度，该算法采用极大似然函数法实现模型的更新：通过计算当前运动目标状态的相似度来给出当前最适合跟踪模型所占的权重。

产生的后验状态和更新协方差 xj,k^\hat {\mathbf x_{j,k}}xj,k^ 和Pj,k{\mathbf P_{j,k} }Pj,k表示

μj,k\mu_{j,k}μj,k为模型 j 的更新概率，反映了当前测量对模型 j 的适应程度

测量的高斯似然λj,k\lambda _{j,k}λj,k为：

其中：
Sj,k=HjPj,kˉHjT+Rj,kz^j,k=Hj,kxj,kˉ\mathbf S_{j,k}=H_j \mathbf {\bar {\mathbf P_{j,k} }} H^T_j+\mathbf R_{j,k} \\ \hat \mathbf z_{j,k}=H_{j,k}\bar {\mathbf x_{j,k} }Sj,k=HjPj,kˉHjT+Rj,kz^j,k=Hj,kxj,kˉ
Rj,kR_{{j,k}}Rj,k为测量噪声的协方差

Combination

每个 j 滤波器的滤波器输出（重新）组合为单个状态 x^k\hat x_kx^k 和 P^k\hat P_kP^k

手撕自动驾驶算法——多目标追踪：imm交互式多模型相关推荐

手撕自动驾驶算法——非线性优化
目录 1.状态估计问题 1.2 最小二乘的引出 2.非线性最小二乘 2.1 一阶和二阶梯度法 2.2 高斯牛顿法 2.3 列文伯格-马夸尔特方法(阻尼牛顿法) 3.实践:Ceres 3.1 Ceres ...
手撕自动驾驶算法——无迹卡尔曼滤波（UKF）
文章目录 1. 简介 2. CTRV运动模型 2.1 CTRV的目标状态量: 2.2 CTRV的状态转移函数: 2.3 CTRV Process Noise 3. Prediction 3.1 Gen ...
手撕自动驾驶算法——KLT 光流算法
1. Optical Flow 2. KLT KLT 算法本质上也基于光流的三个假设,不同于前述直接比较像素点灰度值的作法,KLT 比较像素点周围的窗口像素,来寻找最相似的像素点. 参考:总结:光流– ...
手撕自动驾驶算法——IMU测量模型、运动模型、误差模型
目录 IMU测量模型 IMU运动模型旋转量求导科氏加速度 IMU 误差模型确定性误差确定性误差误差标定六面法标定加速度六面法标定陀螺仪温度相关的参数标定随机误差高斯白噪声与随机游走 ...
解析自动驾驶算法四大模块的问题与后续发展
自动驾驶涉及技术方方面面.一个成熟的自动驾驶系统主要分为软硬两部分,硬件包括但不限于计算单元.传输网口.传感器本身.网络设备,软件包括但不限于进行任务调度的基础软件."灵魂所在"的 ...
扫地机器人“离家出走”的真实原因找到了：差个自动驾驶算法
萧箫发自凹非寺量子位报道 | 公众号 QbitAI 这家公司,竟然将自动驾驶算法用在了他们的扫地机器人上. 大材小用? 还真没有,虽然道路交通很复杂,但家里也一样. 不仅要在靠近柜脚床脚时减速 ...
自动驾驶深度多模态目标检测和语义分割:数据集、方法和挑战
自动驾驶深度多模态目标检测和语义分割:数据集.方法和挑战原文地址:https://arxiv.org/pdf/1902.07830.pdf Deep Multi-Modal Object Detec ...
自动驾驶算法详解(8)：特斯拉Tesla决策规划算法Planning解析下
前言: 本文将介绍特斯拉Tesla 在AI Day 上提到的决策规划模块Planner. 同人类驾驶员一样,Planner在接收到经过视觉神经网络处理过的3D Vector Space后,将会在该Sp ...
自动驾驶技术综述1：自动驾驶算法软件架构介绍
前言: 自动驾驶技术是一个庞大的工程体系,软件架构.功能算法.控制规划.感知识别.建图定位.电气架构.车载控制器.验证体系等等,有太多的角度可以去切入.对于自动驾驶功能与算法开发,自动驾驶功能的分级是 ...

手撕自动驾驶算法——多目标追踪：imm交互式多模型

文章目录