environment.asv

目标跟踪的仿真对于产生量测、噪声和杂波以及目标运动各种模型的matlab代码

close all
clear all
clc


%%%%%%%%%%%%%%%%--------------　 基本参数设定(1)  仿真环境模拟参数 -----------
MonteCarloCount = 100;

N = 67;    % 采样拍数
ts = 1;   % 采样间隔
Pd = 0.75;                              
FalseAlarmProbability = 1e-6;

ObjectMoveStyle=3;
MaxX=100;
MinX=30;
MaxY=1.0;
MinY=0.3;
RangeLowBound=[MinX  MinY ]';                 
RangeUpBound=[MaxX MaxY ]'; 
ClutterVolume=(MaxX-MinX)*(MaxY-MinY);
% ClutterAverNum=100;
MeasureVar=[0.01 0;0 1.6e-5];  % 量测为径向距和方位角

UnitCount=((MaxX-MinX)/sqrt(MeasureVar(1,1)))*((MaxY-MinY)/sqrt(MeasureVar(2,2)));
ClutterDensity=1*1e-4;
ClutterAverNum=floor(UnitCount*ClutterDensity+0.5);


disp('--------------------------------------------------------------------------------------------------------------------')
%         numOfUniformMoveObjects=input('请输入匀速运动目标数目:');
numOfUniformMoveObjects=0;
if numOfUniformMoveObjects>=1
    UniformMoveObjects=zeros(4,numOfUniformMoveObjects);
    for i=1:1:numOfUniformMoveObjects
        UniformMoveObjects(1,i)=0.2;
        UniformMoveObjects(2,i)=0.025;
        UniformMoveObjects(3,i)=0.2;
        UniformMoveObjects(4,i)=0;
    end
end
disp('--------------------------------------------------------------------------------------------------------------------')

disp('--------------------------------------------------------------------------------------------------------------------')
% numOfAddMoveObjects=input('请输入匀加速运动目标数目:');
numOfAddMoveObjects=1;
if numOfAddMoveObjects>=1
    AddMoveObjects=zeros(6,numOfAddMoveObjects);
    AccBeginAndEndTime=zeros(2,numOfAddMoveObjects);
    for i=1:1:numOfAddMoveObjects
        AddMoveObjects(1,i)=40;
        AddMoveObjects(2,i)=0.04;
        AddMoveObjects(3,i)=0.02;
        AddMoveObjects(4,i)=25;
        AddMoveObjects(5,i)=0.05;
        AddMoveObjects(6,i)=0.00;
        AccBeginAndEndTime(1,i)=20;
        AccBeginAndEndTime(2,i)=40;
    end
end
disp('--------------------------------------------------------------------------------------------------------------------')


disp('--------------------------------------------------------------------------------------------------------------------')
% numOfAddMoveObjects=input('请输入Jerk or Kink 运动目标数目:');
numOfJerkMoveObjects=0;
if numOfJerkMoveObjects>=1
    JerkMoveObjects=zeros(6,numOfJerkMoveObjects);
    for i=1:1:numOfJerkMoveObjects
        %                 charTemp=sprintf('**************请输入第%d个目标的径向距、径向速率、径向Jerk速度、方位角、方位角速率、方位角Jerk速度、Jerk起始时刻和Jerk终止时刻!',i);
        %                 disp(charTemp)
        JerkMoveObjects(1,i)=0.2;
        JerkMoveObjects(2,i)=0.015;
        JerkMoveObjects(3,i)=2e-3;
        JerkMoveObjects(4,i)=0.2;
        JerkMoveObjects(5,i)=0.02;
        JerkMoveObjects(6,i)=2e-3;
        JerkBeginTime(1,i)=32;
    end
end
disp('--------------------------------------------------------------------------------------------------------------------')

disp('--------------------------------------------------------------------------------------------------------------------')
% numOfTurnMoveObjects=input('请输入转弯运动目标数目:');
numOfTurnMoveObjects=0;
if numOfTurnMoveObjects>=1
    TurnMoveObjects=zeros(4,numOfTurnMoveObjects);
    TurnBeginAndEndTime=zeros(2,numOfTurnMoveObjects);
    for i=1:1:numOfTurnMoveObjects
        charTemp=sprintf('**************请输入第%d个目标的径向距、径向速率、方位角、方位角速率、转弯起始时刻和转弯终止时刻!***************',i);
        disp(charTemp)
        TurnMoveObjects(1,i)=input('径向距:');
        TurnMoveObjects(2,i)=input('径向转弯速率:');
        TurnMoveObjects(3,i)=input('方位角:');
        TurnMoveObjects(4,i)=input('方位转弯速率:');
        TurnBeginAndEndTime(1,i)=input('转弯起始时刻:');
        TurnBeginAndEndTime(2,i)=input('转弯终止时刻:');        
    end
end
disp('--------------------------------------------------------------------------------------------------------------------')


%******************************************产生杂波
Clutter = cell(N,MonteCarloCount);
Clutter=ClutterProduce(MonteCarloCount,N,RangeLowBound,RangeUpBound,ClutterAverNum);
fileName=GetDataFileName(MonteCarloCount,0,0,N,0,FalseAlarmProbability,0,0,0,0);

for l=1:MonteCarloCount
    for i=1:N
        [m,n]=size(Clutter{i,l});
        if(m~=0)                    
            Clutter{i,l}=[Clutter{i,l};zeros(1,n)];
        end
    end
end

%*****************************************地理坐标系下的状态真值
if numOfUniformMoveObjects>=1
    TrackOfUniformMoveObjects=zeros(4,N,numOfUniformMoveObjects);
    for i=1:1:numOfUniformMoveObjects
        TrackOfUniformMoveObjects(:,:,i)=UniformMove(UniformMoveObjects(:,i),[UniformMoveObjects(2,i);UniformMoveObjects(4,i)],N,ts);
    end
end

if numOfAddMoveObjects>=1
    TrackOfAddMoveObjects=zeros(4,N,numOfAddMoveObjects);
    for i=1:1:numOfAddMoveObjects
        TrackOfAddMoveObjects(:,:,i)=AddMove([AddMoveObjects(1,i);AddMoveObjects(4,i)],[AddMoveObjects(2,i);AddMoveObjects(5,i)],...
            [AddMoveObjects(3,i);AddMoveObjects(6,i)],AccBeginAndEndTime(1,i),AccBeginAndEndTime(2,i),N,ts);
    end
end

if numOfJerkMoveObjects>=1
    TrackOfJerkMoveObjects=zeros(4,N,numOfJerkMoveObjects);
    for i=1:1:numOfJerkMoveObjects
        TrackOfJerkMoveObjects(:,:,i)=JerkMove([JerkMoveObjects(1,i) JerkMoveObjects(3,i)],[JerkMoveObjects(2,i) JerkMoveObjects(4,i)],[JerkMoveObjects(5,i) JerkMoveObjects(6,i)],JerkBeginTime(1,i),N,ts);
    end
end


if numOfTurnMoveObjects>=1
    TrackOfTurnMoveObjects=zeros(4,N,numOfTurnMoveObjects);
    for i=1:1:numOfTurnMoveObjects
        TrackOfTurnMoveObjects(:,:,i)=TurnMove(TurnMoveObjects(:,i),[TurnMoveObjects(2,i);TurnMoveObjects(4,i)],TurnBeginAndEndTime(1,i),...
            TurnBeginAndEndTime(2,i),N,ts);
    end
end

numOfAllObjects= numOfUniformMoveObjects+numOfAddMoveObjects+numOfTurnMoveObjects+numOfJerkMoveObjects;

X = zeros(N,numOfAllObjects);
X_v = zeros(N,numOfAllObjects);
X_vv=zeros(N,numOfAllObjects);

Y = zeros(N,numOfAllObjects);
Y_v = zeros(N,numOfAllObjects);
Y_vv = zeros(N,numOfAllObjects);

iIndex=0;
if numOfUniformMoveObjects>=1
    for i=1:1:numOfUniformMoveObjects
        X(:,i)=TrackOfUniformMoveObjects(1,:,i)';
        X_v(:,i)=TrackOfUniformMoveObjects(2,:,i)';
        Y(:,i)=TrackOfUniformMoveObjects(3,:,i)';
        Y_v(:,i)=TrackOfUniformMoveObjects(4,:,i)';
    end
    iIndex=i;
end

if numOfAddMoveObjects>=1
    for i=iIndex+1:1:numOfAddMoveObjects+iIndex
        X(:,i)=TrackOfAddMoveObjects(1,:,i)';
        X_v(:,i)=TrackOfAddMoveObjects(2,:,i)';
        
        Y(:,i)=TrackOfAddMoveObjects(3,:,i)';
        Y_v(:,i)=TrackOfAddMoveObjects(4,:,i)';
        
    end
    iIndex=i;
end


if numOfJerkMoveObjects>=1
    for i=iIndex+1:1:numOfJerkMoveObjects+iIndex
        X(:,i)=TrackOfJerkMoveObjects(1,:,i)';
        X_v(:,i)=TrackOfJerkMoveObjects(2,:,i)';
        Y(:,i)=TrackOfJerkMoveObjects(3,:,i)';
        Y_v(:,i)=TrackOfJerkMoveObjects(4,:,i)';
    end
    iIndex=i;
end


if numOfTurnMoveObjects>=1
    for i=iIndex+1:1:numOfTurnMoveObjects+iIndex
        X(:,i)=TrackOfTurnMoveObjects(1,:,i)';
        X_v(:,i)=TrackOfTurnMoveObjects(2,:,i)';
        Y(:,i)=TrackOfTurnMoveObjects(3,:,i)';
        Y_v(:,i)=TrackOfTurnMoveObjects(4,:,i)';
    end
end
Main_TrueTrack_G=zeros(6,N);
% 此处为人为添加
for i=AccBeginAndEndTime(1,1)+1:1:AccBeginAndEndTime(2,1)
    X_vv(i,1)=AddMoveObjects(3,1);
    Y_vv(i,1)=AddMoveObjects(6,1);
end
Main_TrueTrack_G=[X';X_v';X_vv';Y';Y_v';Y_vv'];

for j=1:1:MonteCarloCount
    rand('state',sum(100*clock));   % Shuffle the pack!
    randn('state',sum(100*clock));   % Shuffle the pack!
    for t=1:1:N
        if t>3
            Main_TrueTrack_R{t,j}=MeasureFunc(Main_TrueTrack_G(:,t),Pd,MeasureVar);
        else
            Main_TrueTrack_R{t,j}=MeasureFunc(Main_TrueTrack_G(:,t),1,MeasureVar);
        end
        Final_Measure{t,j}=[Main_TrueTrack_R{t,j} Clutter{t,j}];
    end
end

Main_Pd=Pd;
Main_Pf= FalseAlarmProbability;
Main_N=N;
Main_Ts=ts;
Main_M=MonteCarloCount;
save(fileName,'ClutterVolume', 'Main_TrueTrack_G','Main_TrueTrack_R','RangeLowBound','RangeUpBound',...
    'Final_Measure', 'Main_Pd','Main_Pf', 'Main_N','Main_Ts','MonteCarloCount','MeasureVar','Main_M','ClutterDensity') 

% 画图
figure(1)
i=1:1:Main_N;
plot(Main_TrueTrack_G(1,:),Main_TrueTrack_G(4,:),'r*')
figure(2)
for i=1:1:Main_N
    if ~isempty(Final_Measure{i,1})
        if Final_Measure{i,1}(3,1)==1
            plot(Final_Measure{i,1}(1,1),Final_Measure{i,1}(2,1),'ob')
            hold on
        end
    end
end


figure(1)
for i=1:1:Main_N
    if ~isempty(Final_Measure{i,1})
        if Final_Measure{i,1}(3,1)==1
            plot(Final_Measure{i,1}(1,1),Final_Measure{i,1}(2,1),'or')
            hold on
        end 
    end
end
hold on
Tmp=size(Final_Measure{1,1},2);
for i=1:1:Tmp
    if Final_Measure{1,1}(3,i)==0
        plot(Final_Measure{1,1}(1,i),Final_Measure{1,1}(2,i),'*b')
        hold on
    end 
end