Matlab.rar_MATLAB雷达跟踪_信号目标跟踪_雷达 信号相关_雷达 跟踪_雷达目标

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Fuzzy interacting multiple models target tracking in the multiple passive sensor system. %Writed by P.Fei Li %Date:2007.11.20 FIMMF (experiment) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Main Program function [target_position]=data(m_number) clear all clc %Initialize T=1; %the sample interval /s Pd=1; %the probability to detect the target Pg=0.99; %the probability to validate a target measurement within the gate times=100; %run times MC_number=1; %montle carlo simulation times NC=2; %the number of the sensors S1=[0 5 0]; %the position of sensor1 S2=[0 -5 0]; %the position of sensor2 g_sigma=13.27; %the validation gate 5 gamma=0.00035; %the density of the clutter sigma_angle_a1 = 1*10^(-3); %the measure precision of the angle sigma_dist = 0.01; %the measure precision of the distance clutter=2; m_number=3; delta=0.02; %the standard deviation of process noise R =[sigma_angle_a1^2 0 0 0;0 sigma_dist^2 0 0;0 0 sigma_angle_a1^2 0;0 0 0 sigma_dist^2]; %the covariance matrix of measurement noise inital_state = [2.0 8.0 1.0 0.15 0.25 0]'; %the inital state of target, the same to all models FF=zeros(6,6,3); FF(:,:,1)=[1 0 0 T 0 0;0 1 0 0 T 0; 0 0 1 0 0 T; 0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1]; w=1*3.14159/180; FF(:,:,2)=[1 0 0 sin(w)/w (cos(w)-1)/w 0;0 1 0 -(cos(w)-1)/w sin(w)/w 0; 0 0 1 0 0 1; 0 0 0 cos(w) -sin(w) 0; 0 0 0 sin(w) cos(w) 0;0 0 0 0 0 1]; w=-1*3.14159/180; FF(:,:,3)=[1 0 0 sin(w)/w (cos(w)-1)/w 0;0 1 0 -(cos(w)-1)/w sin(w)/w 0; 0 0 1 0 0 1; 0 0 0 cos(w) -sin(w) 0; 0 0 0 sin(w) cos(w) 0;0 0 0 0 0 1]; %the process noise covariance matrix is the same to all models Q=[T^4/4 0 0 T^3/2 0 0;0 T^4/4 0 0 T^3/2 0;0 0 T^4/4 0 0 T^3/2; T^3/2 0 0 T^2 0 0; 0 T^3/2 0 0 T^2 0;0 0 T^3/2 0 0 T^2 ]*delta^2; Nor_F=zeros(1,3); %normalising factors % initial Model M = 3; %the number of models Pr(:,1) = [ 0.98,0.01,0.01]'; %the inital prior probability of each model Pij = [0.98,0.01,0.01; 0.1,0.8,0.1; 0.1,0.1,0.8]; %the transiton probability of the model i to model j(结构和书上一样). x_filter1=zeros(6,M); %the filter results of each model x_filter=zeros(6,times,MC_number); %the fusion results S=zeros(4,4,3); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %generate the raw data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % caculating idea measurement data %the azimuth angles of two sensors double target_position=data(m_number); a = zeros(2,times); r = zeros(2,times); %the azimuth angles of two sensors a(1,:) = atan((target_position(2,:)-S1(2))./(target_position(1,:)-S1(1))+eps); a(2,:) = atan((target_position(2,:)-S2(2))./(target_position(1,:)-S2(1))+eps); %the distance between the measurements and two sensors for i = 1:times r(1,i) = sqrt((target_position(1,i)-S1(1))^2 + (target_position(2,i)-S1(2))^2); r(2,i) = sqrt((target_position(1,i)-S2(1))^2 + (target_position(2,i)-S2(2))^2) ; end z=[r(1,:);a(1,:);r(2,:);a(2,:)]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % main programme %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ass_prob=zeros(times,MC_number); Pr11=zeros(M,times,MC_number); ass_prob(1,:)=1; P=zeros(6,6,M); tic for MC=1:MC_number P(:,:,1)=diag([delta^2,0,delta^2,0,delta^2,0]); %the initial state covariance is the same to all models P(:,:,2)=diag([delta^2,0,delta^2,0,delta^2,0]); P(:,:,3)=diag([delta^2,0,delta^2,0,delta^2,0]); S(:,:,1)=R; %the initial innovation covariance is the same to all models S(:,:,2)=R; S(:,:,3)=R; x_filter1(:,1)=inital_state; %起始值作为第一个时刻的滤波值 x_filter1(:,2)=inital_state; x_filter1(:,3)=inital_state; x_filter(:,1,MC)=inital_state; Z(1,:) = z(1,:) + sigma_dist*randn(1,times); Z(2,:) = z(2,:) + sigma_angle_a1*randn(1,times); %the measurement set of the true target Z(3,:) = z(3,:) + sigma_dist*randn(1,times); Z(4,:) = z(4,:) + sigma_angle_a1*randn(1,times); %the measurement set of the true target %begin for n=2:times %第一时刻的作为初始值 %interact first(混合概率计算) in_u=zeros(M,M); %normalising factors Nor_F=Pij'*Pr(:,n-1); for i=1:M for j=1:M in_u(i,j)=Pij(j,i)*Pr(j,n-1)/Nor_F(i); %Mixing probabilities end end % 交互/混合计算 XK_init=zeros(6,M); %the mixing initial state and covariance P0=zeros(6,6,M); for i=1:M for j=1:M XK_init(:,i)=XK_init(:,i)+x_filter1(:,j)*in_u(i,j); end end for i=1:M for j=1:M %the mixing initial covariance P0(:,:,i)=P0(:,:,i)+in_u(i,j)*(P(:,:,j)+(x_filter1(:,j)-XK_init(:,i))*(x_filter1(:,j)-XK_init(:,i))'); end end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %filtering stage %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% x_predict=zeros(6,M); z_predict=zeros(4,M); P_predict=zeros(6,6,M); S=zeros(4,4,M); K=zeros(6,4,M); H=zeros(4,6,M); ss=zeros(4,4); %the computation of the association probabilities L_hood=zeros(1,3); %%generate the samples for i=1:M x_predict(:,i) = FF(:,:,i)*x_filter1(:,i) ; %predict value a1=[x_predict(1,i)-S1(1) x_predict(2,i)-S1(2) x_predict(3,i)-S1(3)]; a2=[x_predict(1,i)-S2(1) x_predict(2,i)-S2(2) x_predict(3,i)-S2(3)]; rn1=sqrt(a1(1)^2+a1(2)^2); r1=sqrt(rn1^2+a1(3)^2); rn2=sqrt(a2(1)^2+a2(2)^2); r2=sqrt(rn2^2+a2(3)^2); H(:,:,i)=[a1(1)/r1 a1(2)/r1 a1(3)/r1 0 0 0; -a1(2)/rn1^2 a1(1)/rn1^2 0 0 0 0; a2(1)/r2 a2(2)/r2 a2(3)/r2 0 0 0; -a2(2)/rn2^2 a2(1)/rn2^2 0 0 0 0 ]; P_predict(:,:,i)= FF(:,:,i)*P(:,:,i)*FF(:,:,i)'+Q; S(:,:,i) = H(:,:,i)*P_predict(:,:,i)*H(:,:,i)'+ R; K(:,:,i) = P_predict(:,:,i)*H(:,:,i)'*inv(S(:,:,i)); P_F(:,:,i) = (eye(6)-K(:,:,i)*H(:,:,i))*P_predict(:,:,i)*(eye(6)-K(:,:,i)*H(:,:,i))' + K(:,:,i)*R*K(:,:,i)'; %滤波误差协方差更新方程 z_predict(:,i) = feval('hfun',x_predict(:,i),S1,S2); %the predicted measurement end % n % Z(:,n) % z_predict(:,1