交互式多模型IMM算法实现难点——模型维数不同
原创不易，路过的各位大佬请点个赞
针对机动目标跟踪的探讨、技术支持欢迎联系，也可以站内私信
WX: ZB823618313
基于IMM机动目标跟踪算法设计最重要的核心部分主要包括：
- IMM框架
- 滤波器选择：(这里基于UKF)
- 目标运动模型：（这里基于CV CT）
1. 难点分析
针对机动目标跟踪问题，如果交互式多模型IMM框架、如模型转移概率、模型集合以及模型概率初始等确定时，IMM算法的实现（设计）主要存在两大难点：
1- 非线性滤波器的选择和集成
2- 模型集合多样、不统一
在IMM算法设计中，模型多样最直接的一个问题就是戈尔戈模型的状态维数不同，而IMM 的总体（混合）估计却只存在一个统一的状态维数，因此这就导致了很多模型组合不能直接适用于IMM 算法中。
以二维目标为例，CV模型的状态维数4，而CA模型的状态维数6，CT模型的状态维数存在两种情形4和5，singer模型状态维数为6，Jerk模型的状态维数为8，等等。这直接导致IMM滤波器状态失配。
2. 设计思路/解决方案
以典型组合
模型1：匀速运动CV（4维）
模型2：匀速转弯运动CT(4维)
模型3：匀加速运动CA（6维）
为例，进行分析。
其它不同维数的模型组合可以基于该思想，很容易的推广。哈哈哈哈哈哈啊哈哈哈，一学就会，…
2.1 CV模型： 
其中为零均值白噪声，其方差为：
定义矩阵
,
2.2 CT模型：
其中为零均值白噪声，其方差为：
或者为(两种形式都可以用，下面一代码形式给出)
Qk= q2*[2*(w1*T-sin(w1*T))/w1^3 0 (1-cos(w1*T))/w1^2 (w1*T-sin(w1*T))/w1^2 ;
0 2*(w1*T-sin(w1*T))/w1^3 -(w1*T-sin(w1*T))/w1^2 (1-cos(w1*T))/w1^2 ;
(1-cos(w1*T))/w1^2 -(w1*T-sin(w1*T))/w1^2 T 0 ;
(w1*T-sin(w1*T))/w1^2 (1-cos(w1*T))/w1^2 0 T;];
定义矩阵
,
2.3 CA模型 ：
其中为零均值白噪声，其方差为：
Qk3=q3^2*[T^5/20 0 T^4/8 0 T^3/6 0;
0 T^5/20 0 T^4/8 0 T^3/6;
T^4/8 0 T^3/3 0 T^2/2 0;
0 T^4/8 0 T^3/3 0 T^2/2;
T^3/6 0 T^2/2 0 T 0
0 T^3/6 0 T^2/2 0 T];
定义矩阵
,
2.4 IMM滤波器状态维数设计
滤波状态设计为：
目标真实航迹生成方程：
第一阶段：
第二阶段：
第三阶段：
代码：
%% 产生真实轨迹
for k=1:t1
X=Fk_cv*X+Gk_cv*sqrtm(Qk1)*randn(4,1); %产生真实轨迹
X_true(:,k,index)=X;
end
for k=t1+1:t2
X=Fk_ct*X+Gk_ct*sqrtm(Qk2)*randn(4,1);
X_true(:,k,index)=X;
end
for k=t2+1:steps
X=Fk3*X+Gk_ca*sqrtm(Qk3)*randn(6,1);
X_true(:,k,index)=X;
end
这样目标状态统一为
和
最后两行均为0，因此CVCT模型对加速并没有产生任何作用，加速度的引入只是为了满足状态维数。
同样，
和
最后两行均为0，为6×4的矩阵，为了让CVCT的4路噪声满足6维状态。
存在的问题：这样用0对齐维数，直接导致CV和CT的过程噪声方差奇异、进而导致滤波估计协方差奇异，使得矩阵分解失败、没办法产生采样点。（目标大多数非线性滤波器都是基于矩近似的采样滤波，e.g.,UKF,CKF,DDF,QKF…）
2.5 IMM各模型滤波器设计
思路：采用变维思想。针对不同模型的局部滤波器，只对该模型真实包含的状态进行滤波更新，其余状态保持不变。
CV模型局部滤波：
注意：滤波中采用的参数矩阵为，
，上面右定义并给出。
CT模型局部滤波：
注意：滤波中采用的参数矩阵为，
，上面右定义并给出。
CA模型局部滤波：
注意：滤波中采用的参数矩阵为，
，上面右定义并给出。
代码实现：
%filer1
[xk_UKF1,Pk_UKF1,A_UKF1] = fun_2UKF_cvct(X_update_hat1,P_update_hat1,Fk1,Gk1,Z_true(:,k,index),Qk1,sigma_r,sigma_b,xp(:,1));
%filer2
[xk_UKF2,Pk_UKF2,A_UKF2] = fun_2UKF_cvct(X_update_hat2,P_update_hat2,Fk2,Gk2,Z_true(:,k,index),Qk2,sigma_r,sigma_b,xp(:,1));
%filer3
[xk_UKF3,Pk_UKF3,A_UKF3] = fun_2UKF(X_update_hat3,P_update_hat3,Fk3,Gk3,Z_true(:,k,index),Qk3,sigma_r,sigma_b,xp(:,1));
3. IMM-UKF仿真实现
3.1. 仿真参数
一、目标模型：CV CT CA
第一阶段：1:39s，匀速运动CV
第二阶段：40:91s，匀速圆周运动CT，角速度：
第三阶段：92:150s，匀加速运动CA
t1=39; t2=91; t3=steps;
%% 产生真实轨迹
for k=1:t1
X=Fk_cv*X+Gk_cv*sqrtm(Qk1)*randn(4,1); %产生真实轨迹
X_true(:,k,index)=X;
end
for k=t1+1:t2
X=Fk_ct*X+Gk_ct*sqrtm(Qk2)*randn(4,1);
X_true(:,k,index)=X;
end
for k=t2+1:steps
X=Fk3*X+Gk_ca*sqrtm(Qk3)*randn(6,1);
X_true(:,k,index)=X;
end
二、测量模型：2D主动雷达
在二维情况下，雷达量测为距离和角度
其中
为雷达坐标，一般情况为0。雷达量测为
。雷达量测方差为
且
，
。
三、性能评估
RMSE(Root mean-squared error):蒙塔卡罗次数，
为第
次仿真得到的估计。
ANEES（average normalized estimation error square）, 为状态维数，
为第
次仿真滤波器输出的估计协方差
3.2. 跟踪轨迹
3.3. 位置/速度RMSE
4.4. 模型概率
4.5. 部分代码
原创不易，路过的各位大佬请点个赞
文章出处登录后可见！
