自动驾驶算法-滤波器系列(八)——IMM交互多模型介绍

2023-05-16

IMM交互多模型介绍

  • 1. 简介
    • (1)IMM(Interacting Multiple Model)
    • (2)马尔科夫概率转移矩阵
  • 2. 算法流程
    • (1)输入交互(模型j)
    • (2)卡尔曼滤波(模型j)
    • (3)模型概率更新
    • (4)输出交互


1. 简介

(1)IMM(Interacting Multiple Model)

IMM算法采用多个Kalman滤波器进行并行处理。每个滤波器对应不同的状态空间模型,不同的状态空间模型描述不同的目标运行模式,所以每个滤波器对目标状态的估计结果不同。IMM算法的基本思想是在每一时刻,假设某个模型在现在时刻有效的条件下,通过混合前一时刻所有滤波器的状态估计值来获得与这个特定模型匹配的滤波器的初始条件,然后对每个模型并行实现正规滤波(预测和修正)步骤;最后,以模型匹配似然函数为基础更新模型概率,并组合所有滤波器修正后的状态估计值(加权和)以得到状态估计。因此IMM算法的估计结果是对不同模型所得估计的混合,而不是仅仅在每一个时刻选择完全正确的模型来估计。

(2)马尔科夫概率转移矩阵

假定某大学有1万学生,每人每月用1支牙膏,并且只使用“中华”牙膏与“黑妹”牙膏两者之一。根据本月(12月)调查,有3000人使用黑妹牙膏,7000人使用中华牙膏。又据调查,使用黑妹牙膏的3000人中,有60%的人下月将继续使用黑妹牙膏,40%的人将改用中华牙膏; 使用中华牙膏的7000人中, 有70%的人下月将继续使用中华牙膏,30%的人将改用黑妹牙膏。据此,可以得到如表-1所示的统计表。
在这里插入图片描述
上表中的4个概率就称为状态的转移概率,而这四个转移概率组成的矩阵
P = [   60 % 40 %   30 % 70 % ] P = \left[ \begin{array}{ccc} \ 60 \% & 40 \% \\ \ 30 \% & 70 \% \end{array} \right] P=[ 60% 30%40%70%]

称为转移概率矩阵。可以看出, 转移概率矩阵的一个特点是其各行元素之和为1。 在本例中,其经济意义是:现在使用某种牙膏的人中,将来使用各种品牌牙膏的人数百分比之和为1。

用转移概率矩阵预测市场占有率的变化

有了转移概率矩阵,就可以预测,到下个月(1月份)使用黑妹牙膏和中华牙膏的人数,计算过程如下:
300060%+700030% = 3900个
300040%+700070% = 6100个

即:1月份使用黑妹牙膏的人数将为3900,而使用中华牙膏的人数将为6100个。

在IMM中,各模型之间的转移由马尔科夫概率转移矩阵确定,其中的元素 p i j p_{ij} pij表示目标由第 i i i个运动模型转移到第 j j j个运动模型的概率,概率转移矩阵如下:
P = [   p 11 . . . p 1 r   . . . . . . . . .   p r 1 . . . p r r ] P = \left[ \begin{array}{ccc} \ p_{11} & ...& p_{1r} \\ \ ... & ... &...\\ \ p_{r1}&...&p_{rr}\\ \end{array} \right] P= p11 ... pr1.........p1r...prr

2. 算法流程

IMM算法是以递推方式进行的,每次递推主要分为以下四个步骤:

(1)输入交互(模型j)

由目标的状态估计 X i ^ ( k − 1 ∣ k − 1 ) \hat{X_i}(k-1|k-1) Xi^(k1k1)与上一步中每个滤波器的模型概率 μ j ( k − 1 ) μ_j(k-1) μj(k1)得到混合估计
X 0 j ^ ( k − 1 ∣ k − 1 ) \hat{X_{0j}}(k-1|k-1) X0j^(k1k1)和协方差 P 0 j ( k − 1 ∣ k − 1 ) P_{0j}(k-1|k-1) P0j(k1k1),将混合估计作为当前循环的初始状态。具体的参数计算如下:

模型j的预测概率(归一化常数)为
c j ˉ = ∑ i = 1 r p i j μ i ( k − 1 ) \bar{c_j} = \sum_{{i=1}}^{r}p_{ij}μ_i(k-1) cjˉ=i=1rpijμi(k1)

模型 i i i到模型 j j j的混合概率
μ i j ( k − 1 ∣ k − 1 ) = ∑ i = 1 r p i j μ i ( k − 1 ) / c j ˉ μ_{ij}(k-1|k-1) = \sum_{i=1}^{r}p_{ij}μ_i(k-1)/\bar{c_j} μij(k1k1)=i=1rpijμi(k1)/cjˉ

模型j的混合状态估计
X 0 j ^ ( k − 1 ∣ k − 1 ) = ∑ i = 1 r X i ^ ( k − 1 ∣ k − 1 ) μ i j ( k − 1 ∣ k − 1 ) \hat{X_{0j}}(k-1|k-1) = \sum_{i=1}^{r}\hat{X_i}(k-1|k-1)μ_{ij}(k-1|k-1) X0j^(k1k1)=i=1rXi^(k1k1)μij(k1k1)

模型j的混合协方差估计
P 0 j ( k − 1 ∣ k − 1 ) = ∑ i = 1 r μ i j ( k − 1 ∣ k − 1 ) { P i ( k − 1 ∣ k − 1 ) + [ X i ^ ( k − 1 ∣ k − 1 ) − X i ^ ( k − 1 ∣ k − 1 ) ] ∗ [ X i ^ ( k − 1 ∣ k − 1 ) − X i ^ ( k − 1 ∣ k − 1 ) ] T } P_{0j}(k-1|k-1) = \sum_{i=1}^{r}μ_{ij}(k-1|k-1)\{P_i(k-1|k-1)\\+[\hat{X_i}(k-1|k-1) - \hat{X_i}(k-1|k-1)]*[\hat{X_i}(k-1|k-1) - \hat{X_i}(k-1|k-1)]^T\} P0j(k1k1)=i=1rμij(k1k1){Pi(k1k1)+[Xi^(k1k1)Xi^(k1k1)][Xi^(k1k1)Xi^(k1k1)]T}
式中, p i j p_{ij} pij为模型 i i i到模型 j j j的转移概率; μ j ( k − 1 ) μ_j(k-1) μj(k1)为模型j在 k − 1 k-1 k1时刻的概率。

(2)卡尔曼滤波(模型j)

X 0 j ^ ( k − 1 ∣ k − 1 ) \hat{X_{0j}}(k-1|k-1) X0j^(k1k1) P 0 j ^ ( k − 1 ∣ k − 1 ) \hat{P_{0j}}(k-1|k-1) P0j^(k1k1) Z ( k ) Z(k) Z(k)作为输入进行Kalman滤波,来更新预测状态 X j ^ ( k ∣ k ) \hat{X_j}(k|k) Xj^(kk)和滤波协方差 P j ( k ∣ k ) P_j(k|k) Pj(kk)

预测
X j ^ = Φ j ( k − 1 ) X 0 j ^ ( k − 1 ∣ k − 1 ) \hat{X_j} = Φ_j(k-1)\hat{X_{0j}}(k-1|k-1) Xj^=Φj(k1)X0j^(k1k1)

预测误差协方差
P j ( k ∣ k − 1 ) = Φ j P 0 j ( k − 1 ∣ k − 1 ) Φ j T + G j Q j G j T P_j(k|k-1)=Φ_jP_{0j}(k-1|k-1)Φ_j^T+G_jQ_jG_j^T Pj(kk1)=ΦjP0j(k1k1)ΦjT+GjQjGjT

Kalman增益
K j ( k ) = P j ( k ∣ k − 1 ) H T [ H P j ( k ∣ k − 1 ) H T + R ] − 1 K_j(k) = P_j(k|k-1)H^T[HP_j(k|k-1)H^T+R]^{-1} Kj(k)=Pj(kk1)HT[HPj(kk1)HT+R]1

滤波
X j ( k ∣ k ) ^ = X j ^ ( k ∣ k − 1 ) + K j ( k ) [ Z ( k ) − H ( k ) X j ( k ∣ k − 1 ) ] \hat{X_j(k|k)} = \hat{X_j}(k|k-1) + K_j(k)[Z(k)-H(k)X_j(k|k-1)] Xj(kk)^=Xj^(kk1)+Kj(k)[Z(k)H(k)Xj(kk1)]

滤波协方差
P j ( k ∣ k ) = [ I − K j ( k ) H ( k ) ] P j ( k ∣ k − 1 ) P_j(k|k) = [I - K_j(k)H(k)]P_j(k|k-1) Pj(kk)=[IKj(k)H(k)]Pj(kk1)

(3)模型概率更新

采用似然函数来更新模型概率 μ j ( k ) μ_j(k) μj(k),模型j的似然函数为
Λ j ( k ) = 1 ( 2 π ) n / 2 ∣ S j ( k ) ∣ 1 / 2 e x p { − 1 2 ν j T S j − 1 ( k ) ν j } Λ_j(k) = \frac{1}{(2π)^{n/2}|S_j(k)|^{1/2}}exp\{-\frac{1}{2}ν_j^{T}S_j^{-1}(k)ν_j\} Λj(k)=(2π)n/2Sj(k)1/21exp{21νjTSj1(k)νj}
式中,
ν j ( k ) = Z ( k ) − H ( k ) X j ^ ( k ∣ k − 1 ) ν_j(k) = Z(k) - H(k)\hat{X_j}(k|k-1) νj(k)=Z(k)H(k)Xj^(kk1)
S j ( k ) = H ( k ) P j ( k ∣ k − 1 ) H ( k ) T + R ( k ) S_j(k) = H(k)P_j(k|k-1)H(k)^T+R(k) Sj(k)=H(k)Pj(kk1)H(k)T+R(k)

则模型 j j j的概率为
μ j ( k ) = Λ j ( k ) c j ˉ / c μ_j(k) = Λ_j(k)\bar{c_j}/c μj(k)=Λj(k)cjˉ/c

式中, c c c为归一化常数,且 c = ∑ j = 1 r Λ j ( k ) c j ˉ c = \sum_{j=1}^{r}Λ_j(k)\bar{c_j} c=j=1rΛj(k)cjˉ

(4)输出交互

基于模型概率,对每个滤波器的估计结果加权合并,得到总的状态估计 X ^ ( k ∣ k ) \hat{X}(k|k) X^(kk)和总的协方差估计 P ( k ∣ k ) P(k|k) P(kk)

总的状态估计
X ^ ( k ∣ k ) = ∑ j = 1 r X j ^ ( k ∣ k ) μ j ( k ) \hat{X}(k|k) = \sum_{j=1}^{r}\hat{X_j}(k|k)μ_j(k) X^(kk)=j=1rXj^(kk)μj(k)

总的协方差估计
P ( k ∣ k ) = ∑ j = 1 r μ j ( k ) { P j ( k ∣ k ) + [ X j ^ ( k ∣ k ) − X ^ ( k ∣ k ) ] ∗ [ X j ( k ∣ k ) − X ^ ( k ∣ k ) ^ ] T } P(k|k) = \sum_{j=1}^{r}μ_j(k)\{P_j(k|k)+[\hat{X_j}(k|k) - \hat{X}(k|k)]*[\hat{X_j(k|k)- \hat{X}(k|k)}]^T\} P(kk)=j=1rμj(k){Pj(kk)+[Xj^(kk)X^(kk)][Xj(kk)X^(kk)^]T}

所以,滤波器的总输出是多个滤波器估计结果的加权平均值。权重即为该时刻模型正确描述目标运动的概率,简称为模型概率。

选取滤波器的目标运动模型,可以从下面3个方面考虑:

①选择一定个数的IMM滤波器,包括较为精确的模型和较为粗糙的模型。IMM滤波算法不仅描述了目标的连续运动状态,而且描述了目标的机动性。

②马尔科夫链状态转移概率的选取对IMM滤波器的性能有较大影响。马尔科夫链状态转移概率矩阵实际上相当于模型状态方程的状态转移矩阵,它将直接影响模型误差和模型概率估计的准确性。一般情况下,当马尔科夫链状态转移概率呈现一定程度的模型性时,IMM滤波器能够更稳健地描述目标运动。

③IMM滤波算法具有模块化的特性。当对目标的运动规律较为清楚时,滤波器可以选择能够比较精确地描述目标运动的模型。当无法预料目标的运动规律时,就应该选择更一般的模型,即该模型应具有较强的鲁棒性。

注意:关于“牙膏”的例子通俗介绍了什么是马尔科夫转移矩阵,例子中涉及到的“转移”是在时间序列上的转移,但是在IMM的输入交互步骤中,“转移”更多的是一种空间状态的转移,是我们为了混合模型而对模型进行“转移”,进而实现模型的混合。其中,模型j的预测概率(归一化常数)为IMM中每个滤波器模型向模型j转移之后得到的概率,它是一种“混合态”概率。如假设IMM使用了两个滤波器,马尔科夫转移矩阵如下:

P = [   0.98 0.02   0.02 0.98 ] P = \left[ \begin{array}{ccc} \ 0.98 & 0.02 \\ \ 0.02 & 0.98 \end{array} \right] P=[ 0.98 0.020.020.98]

滤波器1的概率为0.8,滤波器2的概率为0.2,则滤波器1的预测概率经过状态“转移”之后由两部分组成,一部分来自于转移前的滤波器1,值为 p 00 ∗ 0.8 = 0.784 p_{00}*0.8 = 0.784 p000.8=0.784;另一部分来自滤波器2,值为 p 10 ∗ 0.2 = 0.004 p_{10}*0.2 = 0.004 p100.2=0.004,最终,滤波器1的预测概率为0.788。进一步,模型i到模型j的混合概率指的是得到模型j的预测概率过程中,模型i贡献了多少百分比。对应到上例中,模型0到模型0的混合概率为 0.784 / 0.788 = 0.99492 0.784/0.788=0.99492 0.784/0.788=0.99492;模型1到模型0的混合概率为 0.004 / 0.788 = 0.005076 0.004/0.788=0.005076 0.004/0.788=0.005076,这也是模型j的预测概率被称为归一化常数的原因。以上均是模型混合过程中关于概率的操作,概率与状态相乘则得到“混合态”状态,如模型0的混合状态估计 X 0 j ^ ( k − 1 ∣ k − 1 ) \hat{X_{0j}}(k-1|k-1) X0j^(k1k1)等于模型0的状态估计 X 0 ^ ( k − 1 ∣ k − 1 ) \hat{X_0}(k-1|k-1) X0^(k1k1)乘以模型0到模型0的混合概率与模型1的状态估计 X 1 ^ ( k − 1 ∣ k − 1 ) \hat{X_1}(k-1|k-1) X1^(k1k1)乘以模型1到模型0的混合概率之和。

模型 j j j的混合协方差估计与混合状态估计类似,但需要注意的是公式中加上了概率转移过程中产生的协方差。
模型概率更新公式中, ν j ( k ) ν_j(k) νj(k)为观测值与先验估计之差,该值越大,意味着滤波器 j j j预测的值与实际值偏差比较大,即该滤波器的表现不好,进一步 Λ j ( k ) Λ_j(k) Λj(k)越小,模型 j j j的概率越小。

本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)

自动驾驶算法-滤波器系列(八)——IMM交互多模型介绍 的相关文章

随机推荐

  • 01_使用cMake编译目标文件与库文件

    1 将单个源文件构建为可执行文件 1 1 准备工作 假设有以下源代码 xff0c 希望使用cMake将其构建为一个可执行文件 示例源码如下 span class token macro property span class token d
  • ROS image_transport及cv_bridge

    ROS image transport 压缩 此外上面提到过FPS这个参数 xff0c 自然少不了介绍图像的压缩 一般使用的时候直接会使用没有压缩的图像 xff0c 但是在需要传输的时候 xff0c 这一些大图像非常占用带宽 xff0c 所
  • NEMA格式

    NMEA標準格式 大部份的GPS receiver都具被有美國國家海洋電子學會 National Marine Electronics Association xff0c NMEA 所制定的標準規格 xff0c 其制定了所有航海電子儀器間的
  • 在 JETSON 上配置 CAN 总线收发并基于Node.js编写应用DEMO

    概述 CAN 总线 是汽车电子行业常用的通信协议 Nvidia 推出的边缘 AI 推理设备 JETSON TX2 AGX Xavier 两款开发板支持 CAN 总线通信 这里以 AGX Xavier 开发者套件 为例 介绍如何配置实现基本的
  • C++开发UDP通信:使用socket创建UDP服务器端和客户端

    我的主页视频讲解 博客不经常在线 xff0c 私信请到西瓜视频搜索 智能之心 xff0c 以智能之心赴智能之梦 UDP通信步骤 引用UDP通信步骤 一 UDP套接字服务端架构步骤 1 xff0e 初始化套接字环境WSAStartup 2 x
  • D6 PRO充电器使用方法

    充电方式两种 xff1a 接220V供电 xff08 推荐 xff09 接XT60电池供电 接线 xff1a 大头XT60接口平衡头6S 4S 3S 2S接口 xff0c 123456 黑线从右侧减号一侧开始接 xff0c 红线在左侧数字一
  • uORB和MAVLink通讯例程

    uORB uORB 是一种异步 publish subscribe 的消息传递 API xff0c 用于进程或者线程间通信 IPC 添加新的Topic xff08 主题 xff09 在msg 目录下创建一个新的 msg文件 xff0c 并将
  • 往android studio原生java工程中添加jni的过程

    一 配置步骤 xff1a 第一步是在 src main 中建立一个文件夹cpp 第二步是在文件夹中建立两个文件一个是native lib cpp和CMakeLists txt 第三步在上述两文件中添加如下内容 xff1a span clas
  • C++迭代器

    一 什么是迭代器 xff1a C 43 43 中 xff0c 迭代器就是一个类似于指针的对象 xff0c 它能够用来遍历C 43 43 标准模板库容器中的部分或全部元素 xff0c 每个迭代器对象代表容器中的确定的地址 以下面的string
  • webpack vue-loader was used without the corresponding plugin. Make sure to include VueLoaderPlugin

    场景 webpack2 4 集成vue loader 64 15 2 4报错 vue loader was used without the corresponding plugin Make sure to include VueLoad
  • slamugv使用说明--5.电机编码测速

    电机编码器测速 编码器 xff08 encoder xff09 是将信号 xff08 如比特流 xff09 或数据进行编制 转换为可用以通讯 传输和存储的信号形式的设备 编码器把角位移或直线位移转换成电信号 编码器分类及原理 xff1a 按
  • STM32相关名词解释

    L N NO NC COM名词解释 L是line的意思 xff0c 相线 xff0c 俗称火线 xff1b N是neutral xff0c 中性线的意思 xff0c 在两相线中也称为零线 NO xff1a 也就是常开 xff0c 即在未通电
  • 三轴加速度计、三轴陀螺仪、三轴磁力计

    1 今天要做的事情 列出 xff1a 可能影响因素 xff0c 对影响因素做实验 如何设置对比实验 xff1f 如何设置对比实验 xff1f 三轴加速度计 1 加速度计的原理 三轴加速度 xff1a 输出的速度是去掉重力后的整体加速度 xf
  • zed相机使用

    zed立体相机 43 pyhon 43 opencv zed相机网上资料都是关于安装的教程 xff0c 后续的一些使用介绍的不是很多 xff0c 因为官方文档已经介绍的很详细 xff0c 这里介绍一下自己的学习过程 xff0c 以备查阅 x
  • 思岚A1激光雷达hector_mapping建图与定位

    ROS中最常见的定位包还是gmapping xff0c gmapping是需要里程计的数据也就是IMU或者视觉里程计的数据 xff0c 但是目前还是仅仅在电脑中测试 xff0c 并未实现树莓派与飞控通信 xff0c 因此使用不用里程计数据的
  • 科技概念/名词解释

    文章目录 自然科学算法类无人机互联网软件类硬件类计算机架构类人工智能 自然科学 量子 百度百科 量子 xff08 quantum xff09 是现代物理的重要概念 即一个物理量如果存在最小的不可分割的基本单位 xff0c 则这个物理量是量子
  • 自动驾驶基础知识(一)——英文缩写(持续更新...)

    这一篇就先整理一下接触到的英文缩写 xff0c 相当于给自己补补课 xff0c 以后遇到新的 xff0c 会持续更新上来 xff01 英文缩写中文全称英文全称ABS防抱死制动系统Anti lock Braking SystemACCS自适应
  • 自动驾驶基础知识(二)——术语中英文对照

    文章目录 深度学习自动驾驶深度增强学习计算机视觉其他 深度学习 中文名称英文名称人工智能系统Artificial Intelligence System认知负荷Cognitive Load深度学习Deep Learning3D卷积神经网络3
  • 使用Typora绘制流程图

    Typora可以直接在markdown中画流程图 xff0c 而且语法简洁易懂 xff0c 这是个让我异常惊喜的功能 Typora使用flowchart js来生成简单的SVG流程图 xff0c 此外它还支持mermaid engine 要
  • 自动驾驶算法-滤波器系列(八)——IMM交互多模型介绍

    IMM交互多模型介绍 1 简介 xff08 1 xff09 IMM Interacting Multiple Model xff08 2 xff09 马尔科夫概率转移矩阵 2 算法流程 xff08 1 xff09 输入交互 xff08 模型