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

WX: ZB823618313

1. IMM框架
2. 滤波器选择：(这里基于UKF)
3. 目标运动模型：（这里基于CV CT）

1- 非线性滤波器的选择和集成

2- 模型集合多样、不统一

## 2. 设计思路/解决方案

### 2.1 CV模型： X = [ x , y , x ˙ , y ˙ ] T {X}=[x, y, dot{x}, dot{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=10000100T0100T01Xk+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=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

k

c

v

=

[

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

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)ω1cos(ωT)sin(ωT)0010ω1cos(ω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=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

k

c

t

=

[

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

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=100000010000T010000T0100T2/20T0100T2/20T01Xk+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=100000010000T010000T0100T2/20T0100T2/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=FkcvXk+Wk

X

k

+

1

=

F

k

c

t

X

k

+

W

k

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

Xk+1=FkctXk+Wk

X

k

+

1

=

F

k

c

a

X

k

+

W

k

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运动模型中，

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维状态。

### 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^kkcv=[x^kk(1),x^kk(2),x^kk(3),x^kk(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^kkcvPkkcv=x^kk1cv+Kz(zkz^kk1)=Pkk1KkSkKk(5)

F

k

1

R

4

Fk1inmathbb{R}^4

Fk1R4

Q

k

1

R

4

Qk1inmathbb{R}^4

Qk1R4，上面右定义并给出。

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^kkct=[x^kk(1),x^kk(2),x^kk(3),x^kk(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^kkctPkkct=x^kk1ct+Kz(zkz^kk1)=Pkk1KkSkKk(5)

F

k

2

R

4

Fk2inmathbb{R}^4

Fk2R4

Q

k

2

R

4

Qk2inmathbb{R}^4

Qk2R4，上面右定义并给出。

CA模型局部滤波：

x

^

k

k

c

a

=

x

^

k

k

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

x^kkca=x^kk

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^kkcaPkkca=x^kk1ca+Kz(zkz^kk1)=Pkk1KkSkKk(5)

F

k

3

R

4

Fk3inmathbb{R}^4

Fk3R4

Q

k

3

R

4

Qk3inmathbb{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. 仿真参数

5

π

/

180

;

5*pi/180;

5π/180;

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


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~kbkm=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=(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]。雷达量测方差为

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)=[σ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=500

x

^

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 为状态维数，

P

k

k

i

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

Pkki为第

i

i

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

THE END