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

原创不易，路过的各位大佬请点个赞

针对机动目标跟踪的探讨、技术支持欢迎联系，也可以站内私信
WX: ZB823618313

基于IMM机动目标跟踪算法设计最重要的核心部分主要包括：

1. IMM框架
2. 滤波器选择：(这里基于UKF)
3. 目标运动模型：（这里基于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^2begin{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&Tend{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^2begin{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&Tend{bmatrix} Qk​=qk2​⎣⎢⎢⎡​T3/3T2/200​T2/2T00​00T3/3T2/2​00T2/2T​⎦⎥⎥⎤​
或者为(两种形式都可以用，下面一代码形式给出)

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;];

定义矩阵

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/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];

定义矩阵
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
    for  k=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_zleft(z_k-hat{z}_{k|k-1}right)\ P_{kmid k}^{cv}&=P_{kmid 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 Fk1inmathbb{R}^4 Fk1∈R4， Q k 1 ∈ R 4 Qk1inmathbb{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_zleft(z_k-hat{z}_{k|k-1}right)\ P_{kmid k}^{ct}&=P_{kmid 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 Fk2inmathbb{R}^4 Fk2∈R4， Q k 2 ∈ R 4 Qk2inmathbb{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_zleft(z_k-hat{z}_{k|k-1}right)\ P_{kmid k}^{ca}&=P_{kmid 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 Fk3inmathbb{R}^4 Fk3∈R4， Q k 3 ∈ R 4 Qk3inmathbb{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\ast \pi /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
    for  k=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$， x ^ k ∣ k i hat{x}_{k|k}^i x^k∣ki​为第$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$ 为状态维数， P k ∣ k i mathbf{P}_{k|k}^i Pk∣ki​为第$i$次仿真滤波器输出的估计协方差

3.2. 跟踪轨迹

3.3. 位置/速度RMSE

4.4. 模型概率

4.5. 部分代码

原创不易，路过的各位大佬请点个赞