> 文档中心 > 交互式多模型IMM算法实现难点——模型维数不同(基于CV\CT\CA模型的IMM算法)

交互式多模型IMM算法实现难点——模型维数不同(基于CV\CT\CA模型的IMM算法)


交互式多模型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机动目标跟踪算法设计最重要的核心部分主要包括:

  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 ] Xk +Wk 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=10000100T0100T01Xk+Wk
其中 W k W_k Wk为零均值白噪声,其方差为:
Qk =qk2 [ 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=qk2T3/3T2/200T2/2T0000T3/3T2/200T2/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=10000100T0100T01, F kcv = [ F k 1 0 2 ] Fk_{cv}=\begin{bmatrix}Fk_1& \\& 0_2 \end{bmatrix} Fkcv=[Fk102]

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 ) ] Xk +Wk 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)ω1cos(ωT)sin(ωT)0010ω1cos(ωT)sin(ωT)ωsin(ωT)cos(ωT)Xk+Wk
其中 W k W_k Wk为零均值白噪声,其方差为:
Qk =qk2 [ 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=qk2T3/3T2/200T2/2T0000T3/3T2/200T2/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)ω1cos(ωT)sin(ωT)0010ω1cos(ωT)sin(ωT)ωsin(ωT)cos(ωT), F kct = [ F k 2 0 2 ] Fk_{ct}=\begin{bmatrix}Fk_2& \\& 0_2 \end{bmatrix} Fkct=[Fk202]

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 ] Xk +Wk 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=100000010000T010000T0100T2/20T0100T2/20T01Xk+Wk
其中 W k W_k Wk为零均值白噪声,其方差为:

Qk3=q3^2*[T^5/20   0      T^4/8  0T^3/6   0;   0T^5/20  0      T^4/8   0T^3/6;   T^4/8   0T^3/3  0T^2/2   0;   0T^4/8   0      T^3/3   0T^2/2;   T^3/6   0T^2/2  0T0   0T^3/6   0      T^2/2   0T];

定义矩阵
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=100000010000T010000T0100T2/20T0100T2/20T01, F kca = 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 = Fk c v Xk +Wk X_{k+1}=Fk_{cv}X_{k} + W_k Xk+1=FkcvXk+Wk
第二阶段:
X k + 1 = Fk c t Xk +Wk X_{k+1}=Fk_{ct}X_{k} + W_k Xk+1=FkctXk+Wk
第三阶段:
X k + 1 = Fk c a Xk +Wk X_{k+1}=Fk_{ca}X_{k} + W_k Xk+1=FkcaXk+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运动模型中,Fk c tFk_{ct} FkctFk c vFk_{cv} Fkcv最后两行均为0,因此CVCT模型对加速并没有产生任何作用,加速度的引入只是为了满足状态维数。

同样,Gk c tGk_{ct} GkctGk c vGk_{cv} Gkcv最后两行均为0,为6x4的矩阵,为了让CVCT的4路噪声满足6维状态。

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

2.5 IMM各模型滤波器设计

思路:采用变维思想。针对不同模型的局部滤波器,只对该模型真实包含的状态进行滤波更新,其余状态保持不变。

CV模型局部滤波:x^ k∣k cv = [ 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^kkcv=[x^kk(1),x^kk(2),x^kk(3),x^kk(4)]T
x ^k ∣ kc v = x ^ k ∣ k − 1 c v+ K z( z k−z ^ k ∣ k − 1 ) P k ∣ kc v = P k ∣ k − 1− K kS kK 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^kkcvPkkcv=x^kk1cv+Kz(zkz^kk1)=Pkk1KkSkKk(5)
注意:滤波中采用的参数矩阵为 F k 1 ∈ R 4 Fk1\in\mathbb{R}^4 Fk1R4 Q k 1 ∈ R 4 Qk1\in\mathbb{R}^4 Qk1R4,上面右定义并给出。

CT模型局部滤波:x^ k∣k ct = [ 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^kkct=[x^kk(1),x^kk(2),x^kk(3),x^kk(4)]T
x ^k ∣ kc t = x ^ k ∣ k − 1 c t+ K z( z k−z ^ k ∣ k − 1 ) P k ∣ kc t = P k ∣ k − 1− K kS kK 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^kkctPkkct=x^kk1ct+Kz(zkz^kk1)=Pkk1KkSkKk(5)
注意:滤波中采用的参数矩阵为 F k 2 ∈ R 4 Fk2\in\mathbb{R}^4 Fk2R4 Q k 2 ∈ R 4 Qk2\in\mathbb{R}^4 Qk2R4,上面右定义并给出。

CA模型局部滤波:x^ k∣k ca = x^ k∣k \hat{x}_{k|k}^{ca}=\hat{x}_{k|k} x^kkca=x^kk
x ^k ∣ kc a = x ^ k ∣ k − 1 c a+ K z( z k−z ^ k ∣ k − 1 ) P k ∣ kc a = P k ∣ k − 1− K kS kK 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^kkcaPkkca=x^kk1ca+Kz(zkz^kk1)=Pkk1KkSkKk(5)
注意:滤波中采用的参数矩阵为 F k 3 ∈ R 4 Fk3\in\mathbb{R}^4 Fk3R4 Q k 3 ∈ R 4 Qk3\in\mathbb{R}^4 Qk3R4,上面右定义并给出。

代码实现:

%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    for  k=t2+1:steps X=Fk3*X+Gk_ca*sqrtm(Qk3)*randn(6,1); X_true(:,k,index)=X;    end

二、测量模型:2D主动雷达
在二维情况下,雷达量测为距离和角度
rkm =rk + r ~ k bkm =bk + b ~ k {r}_k^m=r_k+\tilde{r}_k\\ b^m_k=b_k+\tilde{b}_k rkm=rk+r~kbkm=bk+b~k
其中
rk = ( x k− x 0 ) +( y k− y 0 ) 2) bk = tan ⁡− 1y 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=(xkx0)+(yky0)2) bk=tan1xkx0yky0
[ 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]。雷达量测方差为
Rk = cov (vk ) =[ σ 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)=[σr200σ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=500x^ k∣k i \hat{x}_{k|k}^i x^kki为第 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^)=M1i=1M(xkx^kki)(xkx^kki)
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^)=M1i=1M(xkx^kki)2+(yky^kki)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^)=M1i=1M(x˙kx˙^kki)2+(y˙ky˙^kki)2
ANEES(average normalized estimation error square), n n n 为状态维数, Pk∣k i \mathbf{P}_{k|k}^i Pkki为第 i i i次仿真滤波器输出的估计协方差

3.2. 跟踪轨迹

交互式多模型IMM算法实现难点——模型维数不同(基于CV\CT\CA模型的IMM算法)

3.3. 位置/速度RMSE

交互式多模型IMM算法实现难点——模型维数不同(基于CV\CT\CA模型的IMM算法)
交互式多模型IMM算法实现难点——模型维数不同(基于CV\CT\CA模型的IMM算法)

4.4. 模型概率

交互式多模型IMM算法实现难点——模型维数不同(基于CV\CT\CA模型的IMM算法)

交互式多模型IMM算法实现难点——模型维数不同(基于CV\CT\CA模型的IMM算法)

4.5. 部分代码

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