交互式多模型IMM|交互式多模型IMM算法实现难点——模型维数不同（基于CV\CT\CA模型的IMM算法）非线性滤波-多传感器融合|IM

交互式多模型IMM算法实现难点——模型维数不同原创不易，路过的各位大佬请点个赞
针对机动目标跟踪的探讨、技术支持欢迎联系，也可以站内私信
WX: ZB823618313

交互式多模型IMM算法设计难点——模型维数不同

交互式多模型IMM算法实现难点——模型维数不同
- 1. 难点分析
- 2. 设计思路/解决方案
- - 2.1 CV模型：X = [ x , y , x ˙ , y ˙ ] T {X}=[x, y, \dot{x}, \dot{y}]^T X=[x,y,x˙,y˙?]T
  - 2.2 CT模型：X = [ x , y , x ˙ , y ˙ ] T {X}=[x, y, \dot{x}, \dot{y}]^T X=[x,y,x˙,y˙?]T
  - 2.3 CA模型： X = [ x , x , x ˙ , y ˙ , x ¨ , y ¨ ] T {X}=[x, x, \dot{x},\dot{y},\ddot{x}, \ddot{y}]^T X=[x,x,x˙,y˙?,x¨,y¨?]T
  - 2.4 IMM滤波器状态维数设计
  - 2.5 IMM各模型滤波器设计
- 3. IMM-UKF仿真实现
- - 3.1. 仿真参数
  - 3.2. 跟踪轨迹
  - 3.3. 位置/速度RMSE
  - 4.4. 模型概率
  - 4.5. 部分代码

基于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模型：X = [ x , y , x ˙ , y ˙ ] T {X}=[x, y, \dot{x}, \dot{y}]^T X=[x,y,x˙,y˙?]T
X k + 1 = [ 1 0 T 0 0 1 0 T 0 0 1 0 0 0 0 1 ] X k + W k X_{k+1}=\begin{bmatrix}1&0&T&0\\0&1&0&T\\0&0&1&0\\0&0&0&1 \end{bmatrix}X_{k} + W_k Xk+1?=?????1000?0100?T010?0T01??????Xk?+Wk?
其中 W k W_k Wk?为零均值白噪声，其方差为：
Q k = q k 2 [ T 3 / 3 T 2 / 2 0 0 T 2 / 2 T 0 0 0 0 T 3 / 3 T 2 / 2 0 0 T 2 / 2 T ] Q_k=q_k^2\begin{bmatrix}T^3/3&T^2/2&0&0 \\T^2/2&T&0&0 \\0&0&T^3/3&T^2/2 \\0&0& T^2/2&T\end{bmatrix} Qk?=qk2??????T3/3T2/200?T2/2T00?00T3/3T2/2?00T2/2T??????
定义矩阵
F k 1 = [ 1 0 T 0 0 1 0 T 0 0 1 0 0 0 0 1 ] Fk_1=\begin{bmatrix}1&0&T&0\\0&1&0&T\\0&0&1&0\\0&0&0&1 \end{bmatrix} Fk1?=?????1000?0100?T010?0T01??????,F k c v = [ F k 1 0 2 ] Fk_{cv}=\begin{bmatrix}Fk_1& \\& 0_2 \end{bmatrix} Fkcv?=[Fk1??02??]
2.2 CT模型：X = [ x , y , x ˙ , y ˙ ] T {X}=[x, y, \dot{x}, \dot{y}]^T X=[x,y,x˙,y˙?]T
X k + 1 = [ 1 sin ? ( ω T ) ω 0 ? 1 ? cos ? ( ω T ) ω 0 cos ? ( ω T ) 0 ? sin ? ( ω T ) 0 1 ? cos ? ( ω T ) ω 1 sin ? ( ω T ) ω 0 sin ? ( ω T ) 0 cos ? ( ω T ) ] X k + W k X_{k+1}=\begin{bmatrix}1&\frac{\sin(\omega T)}{\omega}&0&-\frac{1-\cos(\omega T)}{\omega}\\0&\cos(\omega T)&0&-\sin(\omega T)\\0&\frac{1-\cos(\omega T)}{\omega}&1&\frac{\sin(\omega T)}{\omega}\\0&\sin(\omega T)&0&\cos(\omega T)\end{bmatrix}X_{k} + W_k Xk+1?=?????1000?ωsin(ωT)?cos(ωT)ω1?cos(ωT)?sin(ωT)?0010??ω1?cos(ωT)??sin(ωT)ωsin(ωT)?cos(ωT)??????Xk?+Wk?
其中 W k W_k Wk?为零均值白噪声，其方差为：
Q k = q k 2 [ T 3 / 3 T 2 / 2 0 0 T 2 / 2 T 0 0 0 0 T 3 / 3 T 2 / 2 0 0 T 2 / 2 T ] Q_k=q_k^2\begin{bmatrix}T^3/3&T^2/2&0&0 \\T^2/2&T&0&0 \\0&0&T^3/3&T^2/2 \\0&0& T^2/2&T\end{bmatrix} Qk?=qk2??????T3/3T2/200?T2/2T00?00T3/3T2/2?00T2/2T??????
或者为(两种形式都可以用，下面一代码形式给出)

Qk= q2*[2*(w1*T-sin(w1*T))/w1^30(1-cos(w1*T))/w1^2(w1*T-sin(w1*T))/w1^2; 02*(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^2T0; (w1*T-sin(w1*T))/w1^2(1-cos(w1*T))/w1^20T; ];

定义矩阵
F k 2 = [ 1 sin ? ( ω T ) ω 0 ? 1 ? cos ? ( ω T ) ω 0 cos ? ( ω T ) 0 ? sin ? ( ω T ) 0 1 ? cos ? ( ω T ) ω 1 sin ? ( ω T ) ω 0 sin ? ( ω T ) 0 cos ? ( ω T ) ] Fk_2=\begin{bmatrix}1&\frac{\sin(\omega T)}{\omega}&0&-\frac{1-\cos(\omega T)}{\omega}\\0&\cos(\omega T)&0&-\sin(\omega T)\\0&\frac{1-\cos(\omega T)}{\omega}&1&\frac{\sin(\omega T)}{\omega}\\0&\sin(\omega T)&0&\cos(\omega T)\end{bmatrix} Fk2?=?????1000?ωsin(ωT)?cos(ωT)ω1?cos(ωT)?sin(ωT)?0010??ω1?cos(ωT)??sin(ωT)ωsin(ωT)?cos(ωT)??????,F k c t = [ F k 2 0 2 ] Fk_{ct}=\begin{bmatrix}Fk_2& \\& 0_2 \end{bmatrix} Fkct?=[Fk2??02??]
2.3 CA模型： X = [ x , x , x ˙ , y ˙ , x ¨ , y ¨ ] T {X}=[x, x, \dot{x},\dot{y},\ddot{x}, \ddot{y}]^T X=[x,x,x˙,y˙?,x¨,y¨?]T
X k + 1 = [ 1 0 T 0 T 2 / 2 0 0 1 0 T 0 T 2 / 2 0 0 1 0 T 0 0 0 0 1 0 T 0 0 0 0 1 0 0 0 0 0 0 1 ] X k + W k X_{k+1}=\begin{bmatrix}1&0&T&0&T^2/2&0\\0&1&0&T&0&T^2/2\\0&0&1&0&T&0 \\0&0&0&1&0&T \\0&0&0&0&1&0 \\0&0&0&0&0&1 \end{bmatrix}X_{k} + W_k Xk+1?=?????????100000?010000?T01000?0T0100?T2/20T010?0T2/20T01??????????Xk?+Wk?
其中 W k W_k Wk?为零均值白噪声，其方差为：

Qk3=q3^2*[T^5/200T^4/80T^3/60; 0T^5/200T^4/80T^3/6; T^4/80T^3/30T^2/20; 0T^4/80T^3/30T^2/2; T^3/60T^2/20T0 0T^3/60T^2/20T];

定义矩阵
F k 3 = [ 1 0 T 0 T 2 / 2 0 0 1 0 T 0 T 2 / 2 0 0 1 0 T 0 0 0 0 1 0 T 0 0 0 0 1 0 0 0 0 0 0 1 ] Fk_3=\begin{bmatrix}1&0&T&0&T^2/2&0\\0&1&0&T&0&T^2/2\\0&0&1&0&T&0 \\0&0&0&1&0&T \\0&0&0&0&1&0 \\0&0&0&0&0&1 \end{bmatrix} Fk3?=?????????100000?010000?T01000?0T0100?T2/20T010?0T2/20T01??????????,F k c a = F k 3 Fk_{ca}=Fk_3 Fkca?=Fk3?
2.4 IMM滤波器状态维数设计
滤波状态设计为：
X = [ x , x , x ˙ , y ˙ , x ¨ , y ¨ ] T {X}=[x, x, \dot{x},\dot{y},\ddot{x}, \ddot{y}]^T X=[x,x,x˙,y˙?,x¨,y¨?]T
目标真实航迹生成方程：
第一阶段：
X k + 1 = F k c v X k + W k X_{k+1}=Fk_{cv}X_{k} + W_k Xk+1?=Fkcv?Xk?+Wk?
第二阶段：
X k + 1 = F k c t X k + W k X_{k+1}=Fk_{ct}X_{k} + W_k Xk+1?=Fkct?Xk?+Wk?
第三阶段：
X k + 1 = F k c a X k + W k X_{k+1}=Fk_{ca}X_{k} + W_k Xk+1?=Fkca?Xk?+Wk?
代码：

%% 产生真实轨迹 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 fork=t2+1:steps X=Fk3*X+Gk_ca*sqrtm(Qk3)*randn(6,1); X_true(:,k,index)=X; end

这样目标状态统一为 X = [ x , x , x ˙ , y ˙ , x ¨ , y ¨ ] T {X}=[x, x, \dot{x},\dot{y},\ddot{x}, \ddot{y}]^T X=[x,x,x˙,y˙?,x¨,y¨?]T 6维，实际上在CV和CT运动模型中， F k c t Fk_{ct} Fkct?和 F k c v Fk_{cv} Fkcv?最后两行均为0，因此CVCT模型对加速并没有产生任何作用，加速度的引入只是为了满足状态维数。
同样， G k c t Gk_{ct} Gkct?和 G k c v Gk_{cv} Gkcv?最后两行均为0，为6x4的矩阵，为了让CVCT的4路噪声满足6维状态。

存在的问题：这样用0对齐维数，直接导致CV和CT的过程噪声方差奇异、进而导致滤波估计协方差奇异，使得矩阵分解失败、没办法产生采样点。（目标大多数非线性滤波器都是基于矩近似的采样滤波，e.g.,UKF,CKF,DDF,QKF…）

2.5 IMM各模型滤波器设计
思路：采用变维思想。针对不同模型的局部滤波器，只对该模型真实包含的状态进行滤波更新，其余状态保持不变。
CV模型局部滤波： x ^ k ∣ k c v = [ x ^ k ∣ k ( 1 ) , x ^ k ∣ k ( 2 ) , x ^ k ∣ k ( 3 ) , x ^ k ∣ k ( 4 ) ] T \hat{x}_{k|k}^{cv}=[\hat{x}_{k|k}(1), \hat{x}_{k|k}(2), \hat{x}_{k|k}(3),\hat{x}_{k|k}(4)]^T x^k∣kcv?=[x^k∣k?(1),x^k∣k?(2),x^k∣k?(3),x^k∣k?(4)]T
x ^ k ∣ k c v = x ^ k ∣ k ? 1 c v + K z ( z k ? z ^ k ∣ k ? 1 ) P k ∣ k c v = P k ∣ k ? 1 ? K k S k K k ′ (5) \textcolor{#FF0000}{ \begin{aligned} \hat{x}_{k|k}^{cv}&=\hat{x}_{k|k-1}^{cv}+K_z\left(z_k-\hat{z}_{k|k-1}\right)\\ P_{k\mid k}^{cv}&=P_{k\mid k-1}-K_kS_kK_k' \end{aligned}} \tag{5} x^k∣kcv?Pk∣kcv??=x^k∣k?1cv?+Kz?(zk??z^k∣k?1?)=Pk∣k?1??Kk?Sk?Kk′??(5)
注意：滤波中采用的参数矩阵为 F k 1 ∈ R 4 Fk1\in\mathbb{R}^4 Fk1∈R4， Q k 1 ∈ R 4 Qk1\in\mathbb{R}^4 Qk1∈R4，上面右定义并给出。
CT模型局部滤波： x ^ k ∣ k c t = [ x ^ k ∣ k ( 1 ) , x ^ k ∣ k ( 2 ) , x ^ k ∣ k ( 3 ) , x ^ k ∣ k ( 4 ) ] T \hat{x}_{k|k}^{ct}=[\hat{x}_{k|k}(1), \hat{x}_{k|k}(2), \hat{x}_{k|k}(3),\hat{x}_{k|k}(4)]^T x^k∣kct?=[x^k∣k?(1),x^k∣k?(2),x^k∣k?(3),x^k∣k?(4)]T
x ^ k ∣ k c t = x ^ k ∣ k ? 1 c t + K z ( z k ? z ^ k ∣ k ? 1 ) P k ∣ k c t = P k ∣ k ? 1 ? K k S k K k ′ (5) \textcolor{#FF0000}{ \begin{aligned} \hat{x}_{k|k}^{ct}&=\hat{x}_{k|k-1}^{ct}+K_z\left(z_k-\hat{z}_{k|k-1}\right)\\ P_{k\mid k}^{ct}&=P_{k\mid k-1}-K_kS_kK_k' \end{aligned}} \tag{5} x^k∣kct?Pk∣kct??=x^k∣k?1ct?+Kz?(zk??z^k∣k?1?)=Pk∣k?1??Kk?Sk?Kk′??(5)
注意：滤波中采用的参数矩阵为 F k 2 ∈ R 4 Fk2\in\mathbb{R}^4 Fk2∈R4， Q k 2 ∈ R 4 Qk2\in\mathbb{R}^4 Qk2∈R4，上面右定义并给出。
CA模型局部滤波： x ^ k ∣ k c a = x ^ k ∣ k \hat{x}_{k|k}^{ca}=\hat{x}_{k|k} x^k∣kca?=x^k∣k?
x ^ k ∣ k c a = x ^ k ∣ k ? 1 c a + K z ( z k ? z ^ k ∣ k ? 1 ) P k ∣ k c a = P k ∣ k ? 1 ? K k S k K k ′ (5) \textcolor{#FF0000}{ \begin{aligned} \hat{x}_{k|k}^{ca}&=\hat{x}_{k|k-1}^{ca}+K_z\left(z_k-\hat{z}_{k|k-1}\right)\\ P_{k\mid k}^{ca}&=P_{k\mid k-1}-K_kS_kK_k' \end{aligned}} \tag{5} x^k∣kca?Pk∣kca??=x^k∣k?1ca?+Kz?(zk??z^k∣k?1?)=Pk∣k?1??Kk?Sk?Kk′??(5)
注意：滤波中采用的参数矩阵为 F k 3 ∈ R 4 Fk3\in\mathbb{R}^4 Fk3∈R4， Q k 3 ∈ R 4 Qk3\in\mathbb{R}^4 Qk3∈R4，上面右定义并给出。
代码实现：

%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，角速度： 5 ? π / 180 ; 5*\pi/180; 5?π/180;
第三阶段：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 fork=t2+1:steps X=Fk3*X+Gk_ca*sqrtm(Qk3)*randn(6,1); X_true(:,k,index)=X; end

二、测量模型：2D主动雷达
在二维情况下，雷达量测为距离和角度
r k m = r k + r ~ k b k m = b k + b ~ k {r}_k^m=r_k+\tilde{r}_k\\ b^m_k=b_k+\tilde{b}_k rkm?=rk?+r~k?bkm?=bk?+b~k?
其中
r k = ( x k ? x 0 ) + ( y k ? y 0 ) 2 ) b k = tan ? ? 1 y k ? y 0 x k ? x 0 r_k=\sqrt{(x_k-x_0)^+(y_k-y_0)^2)}\\ b_k=\tan^{-1}{\frac{y_k-y_0}{x_k-x_0}}\\ rk?=(xk??x0?)+(yk??y0?)2) ?bk?=tan?1xk??x0?yk??y0??
[ x 0 , y 0 ] [x_0,y_0] [x0?,y0?]为雷达坐标，一般情况为0。雷达量测为 z k = [ r k , b k ] ′ z_k=[r_k,b_k]' zk?=[rk?,bk?]′。雷达量测方差为
R k = cov ( v k ) = [ σ r 2 0 0 σ b 2 ] R_k=\text{cov}(v_k)=\begin{bmatrix}\sigma_r^2 & 0 \\0 & \sigma_b^2 \end{bmatrix} Rk?=cov(vk?)=[σr2?0?0σb2??]且 σ r = 70 m \sigma_r=70m σr?=70m，σ b = 0. 3 o \sigma_b=0.3^o σb?=0.3o。
三、性能评估
RMSE(Root mean-squared error):蒙塔卡罗次数 M = 500 M=500 M=500， x ^ k ∣ k i \hat{x}_{k|k}^i x^k∣ki?为第 i i i次仿真得到的估计。
RMSE ( x ^ ) = 1 M ∑ i = 1 M ( x k ? x ^ k ∣ k i ) ( x k ? x ^ k ∣ k i ) ′ \text{RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum_{i=1}^{M}(\mathbf{x}_k-\hat{\mathbf{x}}_{k|k}^i)(\mathbf{x}_k-\hat{\mathbf{x}}_{k|k}^i)'} RMSE(x^)=M1?i=1∑M?(xk??x^k∣ki?)(xk??x^k∣ki?)′ ?
Position RMSE ( x ^ ) = 1 M ∑ i = 1 M ( x k ? x ^ k ∣ k i ) 2 + ( y k ? y ^ k ∣ k i ) 2 \text{Position RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum_{i=1}^{M}(x_k-\hat{x}_{k|k}^i)^2+(y_k-\hat{y}_{k|k}^i)^2} Position RMSE(x^)=M1?i=1∑M?(xk??x^k∣ki?)2+(yk??y^?k∣ki?)2 ?
Velocity RMSE ( x ^ ) = 1 M ∑ i = 1 M ( x ˙ k ? x ˙ ^ k ∣ k i ) 2 + ( y ˙ k ? y ˙ ^ k ∣ k i ) 2 \text{Velocity RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum_{i=1}^{M}(\dot{x}_k-\hat{\dot{x}}_{k|k}^i)^2+(\dot{y}_k-\hat{\dot{y}}_{k|k}^i)^2} Velocity RMSE(x^)=M1?i=1∑M?(x˙k??x˙^k∣ki?)2+(y˙?k??y˙?^?k∣ki?)2 ?
ANEES（average normalized estimation error square）, n n n 为状态维数， P k ∣ k i \mathbf{P}_{k|k}^i Pk∣ki?为第 i i i次仿真滤波器输出的估计协方差
3.2. 跟踪轨迹
$交互式多模型IMM|交互式多模型IMM算法实现难点——模型维数不同（基于CV\CT\CA模型的IMM算法）$
文章图片

3.3. 位置/速度RMSE
$交互式多模型IMM|交互式多模型IMM算法实现难点——模型维数不同（基于CV\CT\CA模型的IMM算法）$
文章图片

$交互式多模型IMM|交互式多模型IMM算法实现难点——模型维数不同（基于CV\CT\CA模型的IMM算法）$
文章图片

4.4. 模型概率
$交互式多模型IMM|交互式多模型IMM算法实现难点——模型维数不同（基于CV\CT\CA模型的IMM算法）$
文章图片

$交互式多模型IMM|交互式多模型IMM算法实现难点——模型维数不同（基于CV\CT\CA模型的IMM算法）$
文章图片

4.5. 部分代码
【交互式多模型IMM|交互式多模型IMM算法实现难点——模型维数不同（基于CV\CT\CA模型的IMM算法）】原创不易，路过的各位大佬请点个赞