交互多模型(IMM)Kalman滤波

         前段时间不慎感染新冠，本来元旦好了，结果跑了个五公里。感觉心脏不行了，出现胸闷，气短，心痛的情况。保险起见又歇了几天，今天感觉还可以。所以把《卡尔曼滤波原理及应用-matlab仿真》的最后一篇交互多模型Kalman滤波学习一下，争取在过年前整理出来。话不多说，咱们一起来学习。

　　参考内容：书籍《卡尔曼滤波原理及应用------matlab仿真》这本书对kalman算法的解析很清晰，MATLAB程序很全，适合初学者（如有侵权，请联系删除(qq:1491967912)

　　1、背景：

　　在Kalman滤波算法中用到了状态转移方程和观测方程，被估计量随时间变化，他是一种动态估计。在目标跟踪中，不必知道目标的运动模型就能够实时地修正状态参量（位置、速度等信息），有良好的适应性。但是当目标运动运动变得复杂时（比如加速、减速等），仅仅用kalman滤波得不到理想的效果。这时就需要用自适应算法。交互多模型（IMM）是一种软切换算法，现在在机动目标领域得到广泛应用。该算法主要通过两个或更多的模型来描述工作过程中可能的状态，最后通过有效的加权融合进行系统状态估计，能够很好的克服单模型估计误差大的问题。

　　2、交互多模型Kalman滤波原理

　　IMM算法采用多个Kalman滤波器进行并行处理。每一个滤波器对应着不同的状态空间模型，不同的状态空间模型描述不同的目标运动模式，因此每一个滤波器对目标的状态估计是不同的。

　　IMM算法的基本思想：

　　1、在每个时刻，假设某个模型在现在时刻有效的前提下，通过混合前一时刻所有滤波器的状态估计值来获得与这个特定模型匹配的滤波器的初始条件；

　　2、然后对每一个并行实现正规滤波（预测和修正）步骤；

　　3、最后，以模型匹配似然函数为基础更新模型概率，并组合所有滤波器修正后的状态估计值（加权和）来得到状态估计。

　　因此IMM算法的估计结果是对不同模型所得估计的混合，而不是仅仅在每个时刻选择完全正确的模型来估计。下面我门通过一些公式来具体学习一下。

　　3、IMM算法步骤：

　　假定运动目标有r种运动状态，对应有r个运动模型（相当于有r个状态转移方程），假设第j个模型的目标状态方程为：

　　量测方程为：

　　公式中，Wj(k)是均值为0，Gj(k)是噪声驱动矩阵,协方差矩阵为Qj的白噪声序列。各个模型之间的转移是通过马尔科夫概率转移矩阵确定的，其中元素pij表示目标由第i个运动模型转移到第j个运动模型的概率。概率转移矩阵如下（关于马尔科夫概率转移矩阵的知识大家可以参考一下马尔科夫链与转移矩阵这篇文章，里面的例子非常生动形象）：

　　步骤1：输入交互（模型j）

　　由目标的状态估计与上一步中每个滤波器的模型概率得到混合估计和协方差,将混合估计作为当前循环的初始状态。具体参数计算如下：

　　模型j的预测概率（归一化常数）为：

　　模型i到模型j的混合概率为：

　　模型j的混合状态估计为：

　　模型j的混合协方差估计：

　　公式中的Pij是模型i到模型j的转移概率，μj(k-1)为模型j在k-1时刻的概率。

　　步骤2：kalman滤波（模型j）

　　以、和作为输入进行Kalman滤波，来更新预测状态和滤波协方差。

　　预测：

　　预测误差协方差：

　　kalman增益：

　　滤波：

　　滤波协方差：

　　步骤3：模型概率更新

　　采用似然函数来更新模型概率来更新模型概率μj(k)，模型j的似然函数为：

　　公式中：

　　所以模型j的概率为：

　　公式14中，c为归一化常数，并且

　　步骤4：输出交互：

　　基于模型概率，对每个滤波器的估计结果加权合并，得到总的状态估计和总的协方差估计。

　　总的状态估计：

总的协方差估计：

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

　　选取滤波器的目标运动模型，可以从下面三个方面考虑：

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

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

　　（3）IMM滤波算法具有模块化的特性  。当对目标的运动规律有较为清晰的了解时，滤波器可以选择能够比较精确的描述目标运动的模型。当无法预料目标的运动规律时，应该选择更加一般的模型，也就是说模型应该具有较强的鲁棒性。

　　4、交互多模型kalman滤波在目标跟踪中的应用

　　1、问题描述

　　假定有一个雷达对平面上运动的目标进行观测。目标在t=0~40s沿着y轴作匀速直线运动，运动速度为-15m/s，目标起始点为（2000m,10000m）；在t=400~600s向x轴方向做90°的慢速转弯，加速度为μx=μy=0.075m/s2,当慢转弯结束后加速度将会降到0；从t=610s开始做90度快速转弯，加速度为0.3m/s2；在660s结束快转弯，加速度降到0。雷达扫描周期T=2s，x和y独立进行观测，观测噪声的标准差均为100m。试建立雷达对目标的跟踪算法，并进行仿真分析。

　　2、IMM滤波器设计

　　针对上述描述的情况，采用三个模型：第一个模型是非机动模型，假定它的系统扰动噪声方差为0，即不考虑W的影响；第二个、第三个模型为机动模型，假定第二个模型的系统扰动噪声方差为Q=0.001I2*2，第三个模型系统扰动噪声方差为0.0144I2*2。控制模型转换的马尔科夫链的转移概率矩阵为：

　　在跟踪的初始阶段，首先采用常规kalman滤波（非机动模型）进行跟踪，从20次采样开始，采用三个模型的IMM算法。设定各个模型在此时刻的概率分别为μ1=0.8,μ2=0.1,μ3=0.1;

各个模型参数定义如下：

　　采用两点起始法（航迹起始算法之一，后面会针对航迹起始算法进行学习，有兴趣的可以关注了解一下），求得初始状态为：

　　定义滤波误差的均值为：

　　定义滤波误差的标准差为：

　　上式中，M为蒙特卡洛模拟次数k=1,2,...,N，N为采样次数。

　　5、算法程序：