📄 cv_ct_imm3.m
字号:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%《基于无源时差定位系统的机动目标跟踪算法》 陈 玲, 李少洪 系统工程与电子技术
%%% TDoA在二唯平面上需要4个观测站(一个主站和3个辅站) ,令状态矢量为：[x Vx y Vy]
%%% CT模型参考《A comparative study of multiple-model algorithms for maneuvering target》
%%% IMM算法流程可以参考《Survey of Maneuvering Target Tracking - Part V Multiple-Model Methods》TABLE II
%%% 用了3个模型，其他和CT_CV_IMM文件一样
%%% 刘兆霆 2007，11，14
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear
clf
posit_radar0=[0 5];
posit_radar1=[-30 0];
posit_radar2=[30 0];
posit_radar3=[0 -5]; % position of 4 radars
%%%%%%%%%%%% 计算目标真实运动轨迹 %%%%%%%%%%%%
%%% Step1 %%%
All_time=400;
Step1_Velocity_x=0;
Step1_Velocity_y=-0.2;
Begin_posit_x=10;
Begin_posit_y=100;
True_x(1)=Begin_posit_x;
True_y(1)=Begin_posit_y;
for k=2:200
True_x(k)=True_x(k-1)+Step1_Velocity_x;
True_y(k)=True_y(k-1)+Step1_Velocity_y;
end
%%% Step2 %%%
Velocity=0.094;
Velocity_angle=0.157;
angle(200)=-pi/2;
for k=201:220
angle(k)=angle(k-1)+Velocity_angle;
Step2_Velocity_x(k)=Velocity*cos(angle(k));
Step2_Velocity_y(k)=Velocity*sin(angle(k));
True_x(k)=True_x(k-1)+Step2_Velocity_x(k);
True_y(k)=True_y(k-1)+Step2_Velocity_y(k);
end
%%% Step3 %%%
Step3_Velocity_x=0;
Step3_Velocity_y=0.2;
for k=221:300
True_x(k)=True_x(k-1)+Step3_Velocity_x;
True_y(k)=True_y(k-1)+Step3_Velocity_y;
end
%%% Step4 %%%
Velocity=0.054;
Velocity_angle=-pi/30;
angle(300)=pi/2;
for k=301:330
angle(k)=angle(k-1)+Velocity_angle;
Step2_Velocity_x(k)=Velocity*cos(angle(k));
Step2_Velocity_y(k)=Velocity*sin(angle(k));
True_x(k)=True_x(k-1)+Step2_Velocity_x(k);
True_y(k)=True_y(k-1)+Step2_Velocity_y(k);
end
%%% Step5 %%%
Step3_Velocity_x=0;
Step3_Velocity_y=-0.2;
for k=331:400
True_x(k)=True_x(k-1)+Step3_Velocity_x;
True_y(k)=True_y(k-1)+Step3_Velocity_y;
end
figure(1)
plot(True_x,True_y)
grid
xlabel('x')
ylabel('y')
title('目标真实运动轨迹')
axis([8 14 40 120])%axis([8 14 58 6
%%%%%%%%%%%% IMM_CV_CT filter %%%%%%%%%%%%
Model_number=3;%模型的总数
T=1;
angle=pi/20;%CT运动角速度,就是上面的Velocity_angle
F(:,:,1)=[1 T 0 0
0 1 0 0
0 0 1 T
0 0 0 1];%CV模型x(k)=F*X(k-1)+G*u(k-1)+G*w(k-1)
G(:,:,1)=[T^2/2 0
T 0
0 T^2/2
0 T];%CV模型
F(:,:,2)=[1 sin(angle*T)/angle 0 -(1-cos(angle*T))/angle
0 cos(angle*T) 0 -sin(angle*T)
0 (1-cos(angle*T))/angle 1 sin(angle*T)/angle
0 sin(angle*T) 0 cos(angle*T)];%CT模型
angle2=-pi/30;
F(:,:,3)=[1 sin(angle2*T)/angle2 0 -(1-cos(angle2*T))/angle2
0 cos(angle2*T) 0 -sin(angle2*T)
0 (1-cos(angle2*T))/angle2 1 sin(angle2*T)/angle2
0 sin(angle2*T) 0 cos(angle2*T)];%CT模型
G(:,:,2)=G(:,:,1);%CT模型
G(:,:,3)=G(:,:,1);
syms x y Vx Vy %开始求非线性状态方程线性化的Jacobian矩阵
R0=((x-posit_radar0(1))^2+(y-posit_radar0(2))^2)^(1/2);
R1=((x-posit_radar1(1))^2+(y-posit_radar1(2))^2)^(1/2);
R2=((x-posit_radar2(1))^2+(y-posit_radar2(2))^2)^(1/2);
R3=((x-posit_radar3(1))^2+(y-posit_radar3(2))^2)^(1/2);
R10=R1-R0;
R20=R2-R0;
R30=R3-R0;
Jacobian_H=zeros(3,4);
Jacobian_H=jacobian([R10;R20;R30],[x Vx y Vy]);
c=3*10^5;%光速度（公里）
deta_T=3*10^(-8);%观测时间的标准方差
deta_r=deta_T*c;%因为 r=c*gap_time 所以时间的方差可以转化成为距离差的方差
R=eye(3)*deta_r^2;% R：观测噪声V的协方差矩阵
PI=[0.9 0.05 0.05
0.05 0.9 0.05
0.05 0.05 0.9];%Transfer_probability is given
Mode_probability_u=zeros(Model_number,All_time);
Mode_probability_u(:,1)=1/3; % Mode probability to be initialized %%%%%for any model?这可能有问题：初始模型概率和应该等于1
Updated_state_X=zeros(4,Model_number,All_time);%[x Vx y Vy]
Overall_estimate_X(:,1)=[10 0 100 -0.2]';
Updated_state_X(:,1,1)=[10 0 100 -0.2]';
Updated_state_X(:,2,1)=[10 0 100 -0.2]';
Updated_state_X(:,3,1)=[10 0 100 -0.2]';% Updated state to be initialized for any model
Updated_covariance_P(:,:,1,1)=eye(4);
Updated_covariance_P(:,:,2,1)=eye(4);
Updated_covariance_P(:,:,3,1)=eye(4);% Updated covariance to be initialized for any model
for n=1:10;%为求RMS，经过100 次蒙特卡洛实验
r0=((True_x-posit_radar0(1)).^2+(True_y-posit_radar0(2)).^2).^(1/2);
r1=((True_x-posit_radar1(1)).^2+(True_y-posit_radar1(2)).^2).^(1/2);
r2=((True_x-posit_radar2(1)).^2+(True_y-posit_radar2(2)).^2).^(1/2);
r3=((True_x-posit_radar3(1)).^2+(True_y-posit_radar3(2)).^2).^(1/2);
r10=r1-r0;
r20=r2-r0;
r30=r3-r0;
Disturbed_r10=r10+deta_r*randn(1,All_time);
Disturbed_r20=r20+deta_r*randn(1,All_time);
Disturbed_r30=r30+deta_r*randn(1,All_time);
for k=2:All_time %时间周期循环开始
% for i=1:Model_number %模式周期循环开始
%%%%%%%% 1. Model-conditioned reinitialization (for i = 1,2,: : : ,Model_number): %%%%%%%
for i=1:Model_number
Predicted_mode_probability_u(i,k)= PI(1,i)*Mode_probability_u(1,k-1);
Predicted_mode_probability_u(i,k)=Predicted_mode_probability_u(i,k)+PI(2,i)*Mode_probability_u(2,k-1);
Predicted_mode_probability_u(i,k)=Predicted_mode_probability_u(i,k)+PI(3,i)*Mode_probability_u(3,k-1);
end
for i=1:Model_number
for j=1:Model_number
Mixing_weight_u(j,i,k-1)=PI(j,i)*Mode_probability_u(j,k-1)/Predicted_mode_probability_u(i,k);
end
end
for i=1:Model_number
Mixing_estimate_X(:,i,k-1)= Updated_state_X(:,1,k-1).*Mixing_weight_u(1,i,k-1);
Mixing_estimate_X(:,i,k-1)=Mixing_estimate_X(:,i,k-1)+Updated_state_X(:,2,k-1).*Mixing_weight_u(2,i,k-1);
Mixing_estimate_X(:,i,k-1)=Mixing_estimate_X(:,i,k-1)+Updated_state_X(:,3,k-1).*Mixing_weight_u(3,i,k-1);
Mixing_covariance_P(:,:,i,k-1)= (Updated_covariance_P(:,:,1,k-1)+(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,1,k-1))*(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,1,k-1))')*Mixing_weight_u(1,i,k-1);
Mixing_covariance_P(:,:,i,k-1)=Mixing_covariance_P(:,:,i,k-1)+(Updated_covariance_P(:,:,2,k-1)+(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,2,k-1))*(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,2,k-1))')*Mixing_weight_u(2,i,k-1);
Mixing_covariance_P(:,:,i,k-1)=Mixing_covariance_P(:,:,i,k-1)+(Updated_covariance_P(:,:,3,k-1)+(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,3,k-1))*(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,3,k-1))')*Mixing_weight_u(3,i,k-1);
%%%%%%%% 2. Model-conditioned filtering (for i = 1,2,: : : ,Model_number): %%%%%%%%%%%
Predicted_state_X(:,i,k)=F(:,:,i)*Mixing_estimate_X(:,i,k-1)+G(:,:,i)*[1 ; 1]*0.00001;
Predicted_covariance_P(:,:,i,k)=F(:,:,i)*Mixing_covariance_P(:,:,i,k-1)*F(:,:,i)'+G(:,:,i)*G(:,:,i)'*0.00001;
x=Predicted_state_X(1,i,k);
y=Predicted_state_X(3,i,k);
Vx=Predicted_state_X(2,i,k);
Vy=Predicted_state_X(4,i,k);
H(:,:,i,k)=eval(Jacobian_H); %计算k时刻的Jacobian矩阵
Measure_z(:,k)=[Disturbed_r10(k) Disturbed_r20(k) Disturbed_r30(k)]';
rr0(i,k)=((Predicted_state_X(1,i,k)-posit_radar0(1))^2+(Predicted_state_X(3,i,k)-posit_radar0(2))^2)^(1/2);
rr1(i,k)=((Predicted_state_X(1,i,k)-posit_radar1(1))^2+(Predicted_state_X(3,i,k)-posit_radar1(2))^2)^(1/2);
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -