📄 imm3_imm2_singel_kalman.m
字号:
%ATC跟踪问题：雷达只提供位置的测量值（经过极坐标到迪卡尔坐标的转换），每个笛卡尔坐标方向均有100m的RMS误差;
%样本间隔T=5s,假定飞行速度120m/s,标准转弯速率3度/s,在k=8时开始顺时针转弯90度
%IMM2: one nearly constant velocity model with noise leve foe slow turns
% and small linear accelerations,q1=0.004^2;
% one nearly coordinated turn model with expected turn rate
% 6.0度/s(ww01),standard deviation 4.0度/s(sigma01);
%IMM3: one nearly constant velocity model with noise leve foe slow turns
% and small linear accelerations q2=0.001^2;;
% one nearly coordinated turn model with expected turn rate
% 3.0度/s(ww02),standard deviation 2.0度/s(sigma02);
% one nearly coordinated turn model with expected turn rate
% 9.0度/s(ww03),standard deviation 3.0度/s(sigma03);
clear all;
close all;
%
% define the sample interval
delta=5; % s
% delta=3;
%define the number of samples
nsample=30;
%define the number of of simu_IMM2lation loops
nloops=100;
%define process noise intensity
q1=0.004^2;
q2=0.001^2;
% q2=10;
% q4=100;
q4=100;
%define measurement noise intensity
r=10000; %m^2
%define turn rate
turn0=-3; % 因为是顺时针所以为负值
turn=-3*pi/180;
% % turn0=-6; % 因为是顺时针所以为负值
% % turn=-6*pi/180;
%
% turn0=-10; % 因为是顺时针所以为负值
% turn=-10*pi/180;
%机动开始时间
T_m=8;
%机动经历时间
T_ml=90/(abs(turn0)*delta);
% IMM2
ww01=-6.0*pi/180; %IMM2 转弯模型期望转弯速率
sigma01=4.0*pi/180; %IMM2 转弯模型标准准偏差
% IMM3
ww02=-3.0*pi/180; %IMM3 CT1 转弯模型期望转弯速率
sigma02=2.0*pi/180; %IMM3 CT1 转弯模型标准准偏差
ww03=-9.0*pi/180; %IMM3 CT2 转弯模型期望转弯速率
sigma03=3.0*pi/180; %IMM3 CT2转弯模型标准准偏差
nx1=4; % one nearly constant velocity model 维数
nx2=4; % one nearly coordinated turn model 维数
nz=2; % 测量值的维数
% Markov chain transition matrix
rho_IMM2=[0.95,0.05;0.05,0.95];
rho_IMM3=[0.95,0.025,0.025;
0.05,0.7,0.25;
0.05,0.25,0.7];
xtrue=zeros(nx1,nsample); %真实轨迹；
zobs=zeros(nz,nsample); %观测值；
positionRMS_IMM2=zeros(1,nsample); %IMM2 position RMS eroor
positionRMS_IMM3=zeros(1,nsample); %IMM2 position RMS eroor
positionRMS_K=zeros(1,nsample); %single Kalman position RMS eroor
%**********************************************
% one nearly constant velocity model
%***********************************************
%define F matrix (transition matrix)
F_ncv=[1,delta,0,0;
0,1,0,0;
0,0,1,delta;
0,0,0,1];
%define H matrix(measurement matrix)
H_ncv=[1,0,0,0;
0,0,1,0];
%**********************************************
% one nearly coordinated turn model
%***********************************************
%define F matrix (transition matrix) for real track
f01=sin(turn*delta)/turn;
f02=(1-cos(turn*delta))/turn;
f03=cos(turn*delta);
f04=sin(turn*delta);
F_notr=[1,f01,0,-f02;
0,f03,0,-f04;
0,f02,1,f01;
0,f04,0,f03];
%define H matrix(measurement matrix)
H_notr=[1,0,0,0;
0,0,1,0];
G2=[delta^2,0;
delta,0;
0,delta^2;
0,delta];
%*********************************************
% ---------生成真实轨迹----------------------
%*********************************************
xtrue(:,1)=[2000,0,0,120]';
for ii=2:T_m
xtrue(:,ii)=F_ncv*xtrue(:,ii-1);
end
for ii=(T_m+1):(T_m+T_ml)
xtrue(:,ii)=F_notr*xtrue(:,ii-1);
end
for ii=(T_m+T_ml+1):nsample
xtrue(:,ii)=F_ncv*xtrue(:,ii-1);
end
figure(1)
plot(xtrue(1,:),xtrue(3,:));
title('real motion track');
axis([0 14000 0 7000]);
for kk=1:nloops
kk;
tic
%******************************************
% observation calculation
%*****************************************
wx=100*randn(1,nsample);%x方向的观测误差
wy=100*randn(1,nsample);%y方向的观测误差
zobs(1,1:nsample)=xtrue(1,1:nsample)+wx;
zobs(2,1:nsample)=xtrue(3,1:nsample)+wy;
ww1=ww01+sigma01*randn(1,nsample);
ww2=ww02+sigma02*randn(1,nsample);
ww3=ww03+sigma03*randn(1,nsample);
%******************************************
% IMM2
%******************************************
xhat_IMM2_01=zeros(nx1,nsample);
xup_IMM2_1=zeros(nx1,nsample); % 状态更新阵的初始化
xpre_IMM2_1=zeros(nx1,nsample); % 状态预计
xout_IMM2=zeros(4,nsample); %算法最终输出的状态
Pout_IMM2=zeros(4,4,nsample);
Phat_IMM2_01=zeros(nx1,nx1,nsample);
Pup_IMM2_1=zeros(nx1,nx1,nsample); %更新协方差
Ppre_IMM2_1=zeros(nx1,nx1,nsample); %预计协方差
ztilde_IMM2_1=zeros(nz,nsample); %新息矩阵初始化
S_IMM2_1=zeros(nz,nz,nsample); %新息方差矩阵初始化
xhat_IMM2_02=zeros(nx2,nsample); %状态滤波矩阵初始化
xup__IMM2_2=zeros(nx2,nsample); %状态滤波矩阵初始化
xpre_IMM2_2=zeros(nx2,nsample);
Phat_IMM2_02=zeros(nx2,nx2,nsample);
Ppre_IMM2_2=zeros(nx2,nx2,nsample); %预测误差方差矩阵初始化
Pup_IMM2_2=zeros(nx2,nx2,nsample); %滤波误差方差矩阵初始化
ztilde_IMM2_2=zeros(nz,nsample); %新息矩阵初始化
S_IMM2_2=zeros(nz,nz,nsample); %新息方差矩阵初始化
zpre_IMM2_1=zeros(nz,nsample); %测量值预测值
zpre_IMM2_2=zeros(nz,nsample);
xerror_IMM2=zeros(4,nsample);
K_IMM2_1=zeros(nx1,nz,nsample); % kalman filter gain
K_IMM2_2=zeros(nx2,nz,nsample);
mu_IMM2=zeros(2,nsample);
mu_IMM2_11=zeros(1,nsample);
mu_IMM2_12=zeros(1,nsample);
mu_IMM2_21=zeros(1,nsample);
mu_IMM2_22=zeros(1,nsample);
xup_IMM2_1(:,2)=[zobs(1,2) (zobs(1,2)-zobs(1,1))/delta zobs(2,2) (zobs(2,2)-zobs(2,1))/delta]';
r1=[r r/delta;r/delta 2*r/delta^2];
Pup_IMM2_1(:,:,2)=[r1,zeros(2);zeros(2),r1];
xup_IMM2_2(:,2)=xup_IMM2_1(:,2);
Pup_IMM2_2(:,:,2)=Pup_IMM2_1(:,:,2);
%-----------initial model probabilities for state-------
mu_IMM2(:,2)=[0.5;
0.5];
rho11_IMM2=rho_IMM2(1,1);
rho12_IMM2=rho_IMM2(1,2);
rho21_IMM2=rho_IMM2(2,1);
rho22_IMM2=rho_IMM2(2,2);
%-----------计算正规化常量阵-------------------
cbar_IMM2_1=rho11_IMM2*mu_IMM2(1,2)+rho21_IMM2*mu_IMM2(2,2);
cbar_IMM2_2=rho12_IMM2*mu_IMM2(1,2)+rho22_IMM2*mu_IMM2(2,2);
if cbar_IMM2_1<10^(-8) % prevent cbar_IMM2_1 from blowing up
cbar_IMM2_1=10^(-8);
else
cbar_IMM2_1=cbar_IMM2_1;
end
%mix probabilities
mu_IMM2_11(2)=rho11_IMM2*mu_IMM2(1,2)/cbar_IMM2_1;
mu_IMM2_21(2)=rho21_IMM2*mu_IMM2(2,2)/cbar_IMM2_1;
mu_IMM2_12(2)=rho12_IMM2*mu_IMM2(1,2)/cbar_IMM2_2;
mu_IMM2_22(2)=rho22_IMM2*mu_IMM2(2,2)/cbar_IMM2_2;
xhat_IMM2_01(:,2)=xup_IMM2_1(:,2)*mu_IMM2_11(2)+xup_IMM2_2(:,2)*mu_IMM2_21(2);
xhat_IMM2_02(:,2)=xup_IMM2_1(:,2)*mu_IMM2_12(2)+xup_IMM2_2(:,2)*mu_IMM2_22(2);
Phat_IMM2_01(:,:,2)=mu_IMM2_11(2)*(Pup_IMM2_1(:,:,2)+(xup_IMM2_1(:,2)-xhat_IMM2_01(:,2))*(xup_IMM2_1(:,2)-xhat_IMM2_01(:,2))')+...
mu_IMM2_21(2)*(Pup_IMM2_2(:,:,2)+(xup_IMM2_2(:,2)-xhat_IMM2_01(:,2))*(xup_IMM2_2(:,2)-xhat_IMM2_01(:,2))');
Phat_IMM2_02(:,:,2)=mu_IMM2_12(2)*(Pup_IMM2_1(:,:,2)+(xup_IMM2_1(:,2)-xhat_IMM2_02(:,2))*(xup_IMM2_1(:,2)-xhat_IMM2_02(:,2))')+...
mu_IMM2_22(2)*(Pup_IMM2_2(:,:,2)+(xup_IMM2_2(:,2)-xhat_IMM2_02(:,2))*(xup_IMM2_2(:,2)-xhat_IMM2_02(:,2))');
%******************************************
% IMM3
%******************************************
xhat_IMM3_01=zeros(nx1,nsample); %状态滤波矩阵初始化
xup_IMM3_1=zeros(nx1,nsample);
xpre_IMM3_1=zeros(nx1,nsample);
xout_IMM3=zeros(4,nsample);
Pout_IMM3=zeros(4,4,nsample);
Phat_IMM3_01=zeros(nx1,nx1,nsample);
Pup_IMM3_1=zeros(nx1,nx1,nsample); %更细协方差
Ppre_IMM3_1=zeros(nx1,nx1,nsample); %预计协方差
ztilde_IMM3_1=zeros(nz,nsample); %新息矩阵初始化
S_IMM3_1=zeros(nz,nz,nsample); %新息方差矩阵初始化
xhat_IMM3_02=zeros(nx2,nsample); %状态滤波矩阵初始化
xup_IMM3_2=zeros(nx2,nsample); %状态滤波矩阵初始化
xpre_IMM3_2=zeros(nx2,nsample);
Phat_IMM3_02=zeros(nx2,nx2,nsample);
Ppre_IMM3_2=zeros(nx2,nx2,nsample); %预测误差方差矩阵初始化
Pup_IMM3_2=zeros(nx2,nx2,nsample); %滤波误差方差矩阵初始化
ztilde_IMM3_2=zeros(nz,nsample); %新息矩阵初始化
S_IMM3_2=zeros(nz,nz,nsample); %新息方差矩阵初始化
xhat_IMM3_03=zeros(nx2,nsample); %状态滤波矩阵初始化
xup_IMM3_3=zeros(nx2,nsample); %状态滤波矩阵初始化
xpre_IMM3_3=zeros(nx2,nsample);
Phat_IMM3_03=zeros(nx2,nx2,nsample);
Ppre_IMM3_3=zeros(nx2,nx2,nsample); %预测误差方差矩阵初始化
Pup_IMM3_3=zeros(nx2,nx2,nsample); %滤波误差方差矩阵初始化
ztilde_IMM3_3=zeros(nz,nsample); %新息矩阵初始化
S_IMM3_3=zeros(nz,nz,nsample); %新息方差矩阵初始化
zpre_IMM3_1=zeros(nz,nsample); %测量值预测值
zpre_IMM3_2=zeros(nz,nsample);
zpre_IMM3_3=zeros(nz,nsample);
xerror_IMM3=zeros(4,nsample);
K_IMM3_1=zeros(nx1,nz,nsample); % kalman filter gain
K_IMM3_2=zeros(nx2,nz,nsample);
K_IMM3_3=zeros(nx2,nz,nsample);
mu_IMM3=zeros(3,nsample);
mu_IMM3_11=zeros(1,nsample);
mu_IMM3_12=zeros(1,nsample);
mu_IMM3_13=zeros(1,nsample);
mu_IMM3_21=zeros(1,nsample);
mu_IMM3_22=zeros(1,nsample);
mu_IMM3_23=zeros(1,nsample);
mu_IMM3_31=zeros(1,nsample);
mu_IMM3_32=zeros(1,nsample);
mu_IMM3_33=zeros(1,nsample);
xup_IMM3_1(:,2)=[zobs(1,2) (zobs(1,2)-zobs(1,1))/delta zobs(2,2) (zobs(2,2)-zobs(2,1))/delta]';
r1=[r r/delta;r/delta 2*r/delta^2];
Pup_IMM3_1(:,:,2)=[r1,zeros(2);zeros(2),r1];
xup_IMM3_2(:,2)=xup_IMM3_1(:,2);
xup_IMM3_3(:,2)=xup_IMM3_1(:,2);
Pup_IMM3_2(:,:,2)=Pup_IMM3_1(:,:,2);
Pup_IMM3_2(:,:,2)=Pup_IMM3_1(:,:,2);
%initial model probabilities for state
mu_IMM3(:,2)=[1/3;1/3;1/3];
%probabilities of changing state,Markove chain transition
rho11_IMM3=rho_IMM3(1,1);
rho12_IMM3=rho_IMM3(1,2);
rho21_IMM3=rho_IMM3(2,1);
rho22_IMM3=rho_IMM3(2,2);
rho13_IMM3=rho_IMM3(1,3);
rho31_IMM3=rho_IMM3(3,1);
rho32_IMM3=rho_IMM3(3,2);
rho33_IMM3=rho_IMM3(3,3);
rho23_IMM3=rho_IMM3(2,3);
%计算正规化常量阵
cbar_IMM3_1=rho11_IMM3/3+rho21_IMM3/3+rho31_IMM3/3;
cbar_IMM3_2=rho12_IMM3/3+rho22_IMM3/3+rho32_IMM3/3;
cbar_IMM3_3=rho13_IMM3/3+rho23_IMM3/3+rho33_IMM3/3;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -