交互式多模型IMM算法实现难点——模型维数不同（基于CVCTCA模型的IMM算法）

admin • 2022-05-22 00:30 • 人工智能

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

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

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

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

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

基于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模型： $dot{y}]^T X=[x,y,x˙,y˙]T$

[

]

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

$X_{k + 1} = ⎣ ⎢ ⎢ ⎡ 10000100 T 010 0 T 01 ⎦ ⎥ ⎥ ⎤ X_{k} + W_{k}$
其中

W_k

$W_{k}$ 为零均值白噪声，其方差为：

[

]

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}

$Q_{k} = q_{k 2} ⎣ ⎢ ⎢ ⎡ T^{3} / 3 T^{2} / 2 00 T^{2} / 2 T 00 00 T^{3} / 3 T^{2} / 2 00 T^{2} / 2 T ⎦ ⎥ ⎥ ⎤$
定义矩阵

[

]

Fk_1=begin{bmatrix}1&0&T&0\0&1&0&T\0&0&1&0\0&0&0&1 end{bmatrix}

$F k_{1} = ⎣ ⎢ ⎢ ⎡ 10000100 T 010 0 T 01 ⎦ ⎥ ⎥ ⎤$ ,

[

]

Fk_{cv}=begin{bmatrix}Fk_1& \& 0_2 end{bmatrix}

$F k_{c v} = [F k_{1} 0_{2}]$

2.2 CT模型： $dot{y}]^T X=[x,y,x˙,y˙]T$

[

sin

⁡

(

)

−

cos

⁡

(

)

cos

⁡

(

)

−

sin

⁡

(

)

−

cos

⁡

(

)

sin

⁡

(

)

sin

⁡

(

)

cos

⁡

(

)

]

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

$X_{k + 1} = ⎣ ⎢ ⎢ ⎡ 1000 \frac{sin ( ω T )}{ω} cos (ω T) \frac{1 - cos ( ω T )}{ω} sin (ω T) 0010 - \frac{1 - cos ( ω T )}{ω} - sin (ω T) \frac{sin ( ω T )}{ω} cos (ω T) ⎦ ⎥ ⎥ ⎤ X_{k} + W_{k}$
其中

W_k

$W_{k}$ 为零均值白噪声，其方差为：

[

]

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}

$Q_{k} = q_{k 2} ⎣ ⎢ ⎢ ⎡ T^{3} / 3 T^{2} / 2 00 T^{2} / 2 T 00 00 T^{3} / 3 T^{2} / 2 00 T^{2} / 2 T ⎦ ⎥ ⎥ ⎤$
或者为(两种形式都可以用，下面一代码形式给出)

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

定义矩阵

[

sin

⁡

(

)

−

cos

⁡

(

)

cos

⁡

(

)

−

sin

⁡

(

)

−

cos

⁡

(

)

sin

⁡

(

)

sin

⁡

(

)

cos

⁡

(

)

]

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}

$F k_{2} = ⎣ ⎢ ⎢ ⎡ 1000 \frac{sin ( ω T )}{ω} cos (ω T) \frac{1 - cos ( ω T )}{ω} sin (ω T) 0010 - \frac{1 - cos ( ω T )}{ω} - sin (ω T) \frac{sin ( ω T )}{ω} cos (ω T) ⎦ ⎥ ⎥ ⎤$ ,

[

]

Fk_{ct}=begin{bmatrix}Fk_2& \& 0_2 end{bmatrix}

$F k_{c t} = [F k_{2} 0_{2}]$

2.3 CA模型： $ddot{y}]^T X=[x,x,x˙,y˙,x¨,y¨]T$

[

]

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

$X_{k + 1} = ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎡ 100000010000 T 01000 0 T 0100 T^{2} / 2 0 T 010 0 T^{2} / 2 0 T 01 ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎤ X_{k} + W_{k}$
其中

W_k

$W_{k}$ 为零均值白噪声，其方差为：

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

定义矩阵

[

]

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}

$F k_{3} = ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎡ 100000010000 T 01000 0 T 0100 T^{2} / 2 0 T 010 0 T^{2} / 2 0 T 01 ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎤$ ,

Fk_{ca}=Fk_3

$F k_{c a} = F k_{3}$

2.4 IMM滤波器状态维数设计

滤波状态设计为：

[

]

{X}=[x, x, dot{x},dot{y},ddot{x}, ddot{y}]^T

$X = [x, x, x ˙, y ˙, x ¨, y ¨]^{T}$

目标真实航迹生成方程：
第一阶段：

X_{k+1}=Fk_{cv}X_{k} + W_k

$X_{k + 1} = F k_{c v} X_{k} + W_{k}$
第二阶段：

X_{k+1}=Fk_{ct}X_{k} + W_k

$X_{k + 1} = F k_{c t} X_{k} + W_{k}$
第三阶段：

X_{k+1}=Fk_{ca}X_{k} + W_k

$X_{k + 1} = F k_{c a} X_{k} + W_{k}$
代码：

%% 产生真实轨迹
    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}

$F k_{c t}$ 和

F

k

c

v

Fk_{cv}

$F k_{c v}$ 最后两行均为0，因此CVCT模型对加速并没有产生任何作用，加速度的引入只是为了满足状态维数。

同样，

G

k

c

t

Gk_{ct}

$G k_{c t}$ 和

G

k

c

v

Gk_{cv}

$G k_{c v}$ 最后两行均为0，为6x4的矩阵，为了让CVCT的4路噪声满足6维状态。

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

2.5 IMM各模型滤波器设计

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

CV模型局部滤波：

∣

[

∣

(

)

∣

(

)

∣

(

)

∣

(

)

]

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 ∣ k c v} = [x^_{k ∣ k} (1), x^_{k ∣ k} (2), x^_{k ∣ k} (3), x^_{k ∣ k} (4)]^{T}$

∣

−

(

−

∣

−

)

∣

−

′

(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 ∣ k c v} P_{k ∣ k c v} = x^_{k ∣ k - 1 c v} + K_{z} (z_{k} - z^_{k ∣ k - 1}) = P_{k ∣ k - 1} - K_{k} S_{k} K_{k'} (5)$
注意：滤波中采用的参数矩阵为

∈

Fk1inmathbb{R}^4

$F k 1 \in R^{4}$ ，

∈

Qk1inmathbb{R}^4

$Q k 1 \in R^{4}$ ，上面右定义并给出。

CT模型局部滤波：

∣

[

∣

(

)

∣

(

)

∣

(

)

∣

(

)

]

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 ∣ k c t} = [x^_{k ∣ k} (1), x^_{k ∣ k} (2), x^_{k ∣ k} (3), x^_{k ∣ k} (4)]^{T}$

∣

−

(

−

∣

−

)

∣

−

′

(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 ∣ k c t} P_{k ∣ k c t} = x^_{k ∣ k - 1 c t} + K_{z} (z_{k} - z^_{k ∣ k - 1}) = P_{k ∣ k - 1} - K_{k} S_{k} K_{k'} (5)$
注意：滤波中采用的参数矩阵为

∈

Fk2inmathbb{R}^4

$F k 2 \in R^{4}$ ，

∈

Qk2inmathbb{R}^4

$Q k 2 \in R^{4}$ ，上面右定义并给出。

CA模型局部滤波：

∣

hat{x}_{k|k}^{ca}=hat{x}_{k|k}

$x^_{k ∣ k c a} = x^_{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 ∣ k c a} P_{k ∣ k c a} = x^_{k ∣ k - 1 c a} + K_{z} (z_{k} - z^_{k ∣ k - 1}) = P_{k ∣ k - 1} - K_{k} S_{k} K_{k'} (5)$
注意：滤波中采用的参数矩阵为

∈

Fk3inmathbb{R}^4

$F k 3 \in R^{4}$ ，

∈

Qk3inmathbb{R}^4

$Q k 3 \in R^{4}$ ，上面右定义并给出。

代码实现：

%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，角速度：

∗

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主动雷达
在二维情况下，雷达量测为距离和角度

{r}_k^m=r_k+tilde{r}_k\ b^m_k=b_k+tilde{b}_k

$r_{k m} = r_{k} + r ~_{k} b_{k m} = b_{k} + b ~_{k}$
其中

(

−

)

(

−

)

tan

⁡

−

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}}\

$r_{k} = (x_{k} - x_{0})^{+} (y_{k} - y_{0})^{2})$

bk=tan−1xk−x0yk−y0

[

]

[x_0,y_0]

$[x_{0}, y_{0}]$ 为雷达坐标，一般情况为0。雷达量测为

[

]

′

z_k=[r_k,b_k]'

$z_{k} = [r_{k}, b_{k}]^{'}$ 。雷达量测方差为

cov

(

)

[

]

R_k=text{cov}(v_k)=begin{bmatrix}sigma_r^2 & 0 \0 & sigma_b^2 end{bmatrix}

$R_{k} = cov (v_{k}) = [σ_{r 2} 0 0 σ_{b 2}]$ 且

sigma_r=70m

$σ_{r} = 70 m$ ，

sigma_b=0.3^o

$σ_{b} = 0 . 3^{o}$ 。

三、性能评估
RMSE(Root mean-squared error):蒙塔卡罗次数

500

M=500

$M = 500$ ，

∣

hat{x}_{k|k}^i

$x^_{k ∣ k i}$ 为第

$i$ 次仿真得到的估计。

RMSE

(

)

∑

(

−

∣

)

(

−

∣

)

′

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^) = \frac{1}{M} i = 1 \sum M (x_{k} - x^_{k ∣ k i}) (x_{k} - x^_{k ∣ k i})^{'}$

Position RMSE

(

)

∑

(

−

∣

)

(

−

∣

)

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^) = \frac{1}{M} i = 1 \sum M (x_{k} - x^_{k ∣ k i})^{2} + (y_{k} - y^_{k ∣ k i})^{2}$

Velocity RMSE

(

)

∑

(

−

∣

)

(

−

∣

)

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^) = \frac{1}{M} i = 1 \sum M (x ˙_{k} - x ˙^_{k ∣ k i})^{2} + (y ˙_{k} - y ˙^_{k ∣ k i})^{2}$

ANEES（average normalized estimation error square）,

$n$ 为状态维数，

∣

mathbf{P}_{k|k}^i

$P_{k ∣ k i}$ 为第

$i$ 次仿真滤波器输出的估计协方差

3.2. 跟踪轨迹

3.3. 位置/速度RMSE

4.4. 模型概率

4.5. 部分代码

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

本图文内容来源于网友网络收集整理提供，作为学习参考使用，版权属于原作者。

THE END

二维码

机器学习入门-一元线性回归模型的骚操作

< <上一篇

阿里云短信服务(解决个人无法申请问题)

下一篇>>

搜索内容

交互式多模型IMM算法实现难点——模型维数不同（基于CVCTCA模型的IMM算法）

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

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

1. 难点分析

2. 设计思路/解决方案

2.1 CV模型： $dot{y}]^T X=[x,y,x˙,y˙]T$

2.2 CT模型： $dot{y}]^T X=[x,y,x˙,y˙]T$

2.3 CA模型： $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算法实现难点——模型维数不同（基于CVCTCA模型的IMM算法）

交互式多模型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. 部分代码

最新文章

分类

标签云

2.1 CV模型： $dot{y}]^T X=[x,y,x˙,y˙]T$

2.2 CT模型： $dot{y}]^T X=[x,y,x˙,y˙]T$

2.3 CA模型： $ddot{y}]^T X=[x,x,x˙,y˙,x¨,y¨]T$