extended object tracking with radar for marine surveillance -凯发k8网页登录

this example shows how to generate a marine scenario, simulate radar detections from a marine surveillance radar, and configure a multi-target probability hypothesis density (phd) tracker to estimate the location and size of the simulated ships using the radar detections.

marine surveillance scenario

simulate a marine surveillance radar mounted at the top of a tower overlooking ships in a harbor. simulation of the tower's location and the motion of the ships in the scenario is managed by trackingscenario.

% create tracking scenario.
scenario = trackingscenario(stoptime = 30);
% define unit conversions.
nmi2m = 1852;           % nautical miles to meters
hr2s = 3600;            % hours to seconds
kts2mps = nmi2m/hr2s;   % knots to meters per second

marine surveillance radar

add a marine surveillance radar to the tower. the radar is mounted 20 meters above sea level (asl). the radar stares into the harbor, surveying a 30 degree azimuth sector. common specifications for a marine surveillance radar are listed:

sensitivity: 0 dbsm @ 5 km
field of view: 30 deg azimuth, 10 deg elevation
azimuth resolution: 2 deg
range resolution: 5 m

model the marine radar with the above specifications using the fusionradarsensor.

% create surveillance radar.
sensor = fusionradarsensor(sensorindex = 1, ...
    scanmode = 'no scanning', ...
    mountinglocation = [0 0 -20], ...      % 20 meters (asl)
    mountingangles = [45 0 0], ...         % [yaw pitch roll] deg
    fieldofview = [30 10], ...             % [az el] deg
    referencerange = 5e3, ...              % m
    azimuthresolution = 2, ...             % deg
    rangeresolution = 5, ...               % m
    hasins = true,...                      % report ins information
    targetreportformat = 'detections',...  % detections without clustering
    rangelimits = [0 15e3], ...           % m
    detectioncoordinates = 'sensor spherical');

add the tower to the scenario as a stationary platform with the radar mounted on top of it.

platform(scenario, sensors = sensor);
tower = scenario.platforms{1}

tower = 
  platform with properties:
       platformid: 1
          classid: 0
         position: [0 0 0]
      orientation: [0 0 0]
       dimensions: [1x1 struct]
             mesh: [1x1 extendedobjectmesh]
       trajectory: [1x1 kinematictrajectory]
    poseestimator: [1x1 inssensor]
         emitters: {}
          sensors: {[1x1 fusionradarsensor]}
       signatures: {[1x1 rcssignature]  [1x1 irsignature]  [1x1 tssignature]}

add three ships in the harbor within the radar's surveillance sector. the two smaller ships are turning at 20 and 30 knots, the large ship is traveling at a constant heading at 10 knots.

% define the dimensions for the two small ships.
dim = struct( ...
    length = 80, ... % m
    width = 15, ...  % m
    height = 5, ...  % m
    originoffset = [0 0 5/2]); % [x y z] m
% model the radar cross section (rcs) of the small ships as 30 dbsm.
rcs = rcssignature('pattern',30);
% create a turning trajectory.
speed = 20;     % knots
inityaw = 130;  % deg
initpos = [1050 790 0];
radius = 200;   % m
initorient = quaternion([inityaw 0 0], 'eulerd', 'zyx', 'frame');
initvel = speed*kts2mps*rotatepoint(initorient,[1 0 0])';
accbody = [0 (speed*kts2mps)^2/radius 0];
angvelbody = [0 0 speed*kts2mps/radius];
traj = kinematictrajectory(position = initpos, velocity = initvel, orientation = initorient, ...
    accelerationsource = 'property', acceleration = accbody, ...
    angularvelocitysource = 'property', angularvelocity = angvelbody);
% add the first small ship, traveling at 20 knots to the scenario. this is
% the closest ship to the radar tower.
platform(scenario, dimensions = dim, signatures = rcs, trajectory = traj);
% create the other small ship, traveling at 30 knots. this is the ship
% which is farthest from the radar tower.
speed = 30;     % knots
inityaw = 120;  % deg
initpos = [1410 1180 0];
radius = 400;   % m
initorient = quaternion([inityaw 0 0],'eulerd','zyx','frame');
initvel = speed*kts2mps*rotatepoint(initorient,[1 0 0])';
accbody = [0 (speed*kts2mps)^2/radius 0];
angvelbody = [0 0 speed*kts2mps/radius];
traj = kinematictrajectory(position = initpos, velocity = initvel, orientation = initorient, ...
    accelerationsource = 'property', acceleration = accbody, ...
    angularvelocitysource = 'property', angularvelocity = angvelbody);
platform(scenario, dimensions = dim, signatures = rcs, trajectory = traj);
% define the dimensions for the large ship.
dim = struct( ...
     length = 400, ... % m
     width = 60, ...   % m
     height = 15, ...  % m
     originoffset = [0 0 15/2]); % [x y z] m
% model the radar cross section (rcs) of the large ship as 75 dbsm.
rcs = rcssignature(pattern = 75);
% create the large ship's trajectory, traveling at a constant heading at 10 knots.
speed = 10;     % knots
inityaw = -135; % degs
initpos = [1150 1100 0];
initorient = quaternion([inityaw 0 0],'eulerd','zyx','frame');
initvel = speed*kts2mps*rotatepoint(initorient,[1 0 0])';
traj = kinematictrajectory(position = initpos, velocity = initvel, orientation = initorient, ...
    accelerationsource = 'property', angularvelocitysource = 'property');
% add the large ship to the scenario.
platform(scenario, dimensions = dim, signatures = rcs, trajectory = traj);
% create a display to show the true, measured, and tracked positions of the ships.
theaterdisplay = helpermarinesurveillancedisplay(scenario, ...
     issea = true, distanceunits = 'm', ...
     xlim = 450*[-1 1] 1e3, ylim = 450*[-1 1] 1e3, zlim = [-1000 10], ...
     movie = 'marinesurveillanceexample.gif');
slcttrkpos = zeros(3,7); slcttrkpos(1,1) = 1; slcttrkpos(2,3) = 1; slcttrkpos(3,6) = 1;
slcttrkvel = circshift(slcttrkpos,[0 1]);
theaterdisplay.trackpositionselector = slcttrkpos;
theaterdisplay.trackvelocityselector = slcttrkvel;
theaterdisplay();
snapnow(theaterdisplay);

multi-target ggiw-phd tracker

create a trackerphd to form tracks from the radar detections generated from the three ships in the harbor. the phd tracker enables the estimation of the size of the ships by allowing multiple detections to be associated to a single object. this is important in situations such as marine surveillance where the size of the objects detected by the sensor is greater than the sensor's resolution, resulting in multiple detections generated along the surfaces of the ships.

the tracker uses the filterinitfcn supporting function to initialize a constant turn-rate gamma gaussian inverse wishart (ggiw) phd filter. filterinitfcn adds birth components to the phd-intensity at every time step. these birth components are added uniformly inside the field of view of the sensor. their sizes and expected number of detections are specified using prior information about the types of ships expected in the harbor.

the tracker uses the gamma distribution of the ggiw-phd components to estimate how many detections should be generated from an object. the tracker also calculates the detectability of each component in the density using the sensor's limits. use trackingsensorconfiguration to model the sensor's configuration for trackerphd.

%create a trackingsensorconfiguration from the platform.
sensorconfig = trackingsensorconfiguration(tower, sensortransformfcn = @ctmeas);
% set filterinitializationfcn
sensorconfig{1}.filterinitializationfcn = @(varargin)filterinitfcn(varargin{:},sensorconfig{1}.sensortransformparameters);
% set detectionprobablity for trackingsensorconfiguration
sensorconfig{1}.detectionprobability = 0.99;
% % noise covariance corresponding to a resolution cell of the radar.
resolutionnoise = diag((sensorconfig{1}.sensorresolution/2).^2);
% create a phd tracker using the trackingsensorconfiguration.
tracker = trackerphd(sensorconfigurations = sensorconfig, ...
    hassensorconfigurationsinput = true, ...
    partitioningfcn = @(x)partitiondetections(x,2,6), ...
    extractionthreshold = 0.75,...
    deletionthreshold = 1e-6,...
    birthrate = 1e-5);

simulate and track ships

the following loop advances the positions of the ships until the end of the scenario. for each step forward in the scenario, the tracker is updated with the detections from the ships in the radar's field of view.

% initialize scenario and tracker.
restart(scenario);
reset(tracker);
% set simulation to advance at the update rate of the radar.
scenario.updaterate = sensor.updaterate;
% set random seed for repeatable results.
rng(2019,'twister');
% run simulation.
snaptimes = [2 7 scenario.stoptime]; % seconds
while advance(scenario)
    % get current simulation time.
    time = scenario.simulationtime;
    % generate detections from the tower's radar.
    [dets,~,config] = detect(tower,time);
    % update measurement noise of detections to match radar's resolution.
    dets = updatemeasurementnoise(dets,resolutionnoise);
    % update tracker.
    tracks = tracker(dets,config,time);
    % update display with current beam position, detections, and track positions.
    theaterdisplay(dets,config,tracks);
    % take snapshot.
    snapfigure(theaterdisplay,any(time==snaptimes));
end
writemovie(theaterdisplay);

the following figure shows the radar detections, shown as red dots, and the estimated track locations, shown as yellow squares annotated with the track id, and the estimated tracked object's extent, shown as a yellow ellipse. the radar tower is located at the origin, (0,0), which is not shown in the figure. the radar's field of view is indicated by the two red lines crossing the top and bottom of the figure. all of the ships lie within the radar's field of view, and because the size of the ships is much larger than the radar's range and azimuth resolution, multiple detections are made along the faces of the ships visible to the radar.

showsnapshot(theaterdisplay,1)

because the ships are modeled as extended objects and not point targets, detections of a ship can be occluded by the presence of another ship between the ship and the radar. this is shown in the following figure. in this case, the smaller ship at the top of the figure is not detected by the radar. the radar's line of sight is occluded by both the other small ship at the bottom of the figure and the large ship in the center. the tracker maintains an estimate of the occluded ship and associates detections in the following steps to the track without ever dropping the track.

showsnapshot(theaterdisplay,2)
axis([1250 1450 1150 1350]); view([-90 90]);

the following figure shows the phd estimate of the smaller ship located closest to the radar in the scenario. you can verify the phd estimate of the ship's location lies near the center of the ship and the estimated size is reasonably close to the actual size of the ship, indicated by the overlap of the track's ellipse with the ship.

showsnapshot(theaterdisplay,3)
axis([650 850 700 900]); view([-90 90]);

the next figure also shows that the phd tracker estimated both the location, size, and heading of the other small ship in the scenario. this is the ship that was previously occluded by the other two ships. despite the occlusion, the estimated location, size, and orientation closely matches the ship.

showsnapshot(theaterdisplay,3)
axis([900 1100 1250 1450]); view([-90 90]);

the track states of the 3 ships report the estimated size of each ship using a 3d positional covariance matrix. take the eigen decomposition of the covariance matrices to compute the estimated length, width, and height for each of the ships.

numtrks = numel(tracks);
trackid = [tracks.trackid]';
length = zeros(numtrks,1);
width = zeros(numtrks,1);
height = zeros(numtrks,1);
for itrk = 1:numtrks
    ext = tracks(itrk).extent;
    [q,d] = eig(ext);
    d = 2*sqrt(diag(d));
    idims = 1:3;
    up = [0 0 -1];
    [~,iup] = max(abs(up*q));
    height(itrk) = d(idims(iup));
    idims(iup) = [];
    length(itrk) = max(d(idims));
    width(itrk) = min(d(idims));
end
% display a table of the estimated dimensions of the ships.
dims = table(trackid,length,width,height)

dims =
  3x4 table
    trackid    length    width     height
    _______    ______    ______    ______
       1       98.986    17.426    16.321
       2       473.55    55.782    8.8344
       3          101    18.791     17.39

recall that the true dimensions of the ships are given by:

large ship

length: 400 m
width: 60 m
height: 15 m

small ship

length: 80 m
width: 15 m
height: 5 m

the tracker is able to differentiate between the size of the large and smaller ships by estimating the shape of each ship as an ellipse. in the simulation, the true shape of each ship is modeled using a cuboid. this mismatch between the shape assumption made by the tracker and the true shapes of the modeled ships results in overestimates the length and width of the ships. the radar is a 2d sensor, only measuring range and azimuth, so the height of each ship is not observable. this results in inaccurate height estimates reported by the tracker.

summary

this example shows how to generate a marine scenario, simulate radar detections from a marine surveillance radar, and configure a multi-target phd tracker to track the simulated ships using the radar detections. in this example, you learned how to model extended objects in a scenario, generating multiple detections from these objects. you also learned how to use a multi-target phd tracker to process the information provided by the multiple detections to estimate not only the location but also the size of the tracked objects.

supporting functions

updatemeasurementnoise

sets the measurement noise of the detections based on the specified noise covariance.

function dets = updatemeasurementnoise(dets,noise)
    for idet = 1:numel(dets)
        dets{idet}.measurementnoise(:) = noise(:);
    end
end

filterinitfcn

modifies the filter returned by initctggiwphd to match the velocities and expected number of detections for the ships being tracked.

function phd = filterinitfcn(measparam,varargin)
% this function uses the predictive birth density only to simulate birth in
% the scenario.
if nargin == 1 % set predictive birth uniformly inside the fov.
    phddefault = initctggiwphd;
    % 1. create uniformly distributed states using azimuth and range.
    az = 0; % all components at azimuth.
    ranges = linspace(1000,10000,5); % 5 components for range
    [az,r] = meshgrid(az,ranges);
    % create a phd filter to allocate memory.
    phd = ggiwphd(zeros(7,numel(az)),repmat(eye(7),[1 1 numel(az)]),...
         scalematrices = repmat(eye(3),[1 1 numel(az)]),...
         statetransitionfcn = @constturn, statetransitionjacobianfcn = @constturnjac,...
         measurementfcn = @ctmeas, measurementjacobianfcn = @ctmeasjac,...
         positionindex = [1 3 6], extentrotationfcn = phddefault.extentrotationfcn,...
         hasadditiveprocessnoise = false, processnoise = 2*eye(4),...
         temporaldecay = 1e3, gammaforgettingfactors = 1.1*ones(1,numel(az)),...
         maxnumcomponents = 10000);
    for i = 1:numel(az)
        [sensorx,sensory,sensorz] = sph2cart(deg2rad(az(i)),0,r(i));
        globalpos = measparam(1).orientation'*[sensorx;sensory;sensorz]   measparam(1).originposition(:);
        phd.states([1 3 6],i) = globalpos;
        phd.statecovariances([1 3 6],[1 3 6],i) = diag([1e5 1e5 1000]); % cover gaps between components using position covariance
    end
    % 2. you have described the "kinematic" states of each of the ships
    % inside the field of view. next, add information about their sizes and
    % expected number of detections.
    %
    % it is expected that there are 2 types of ships in the sea, small and
    % large. you create components for each size.
    phdsmall = phd;
    % clone the phd filter for large ships.
    phdlarge = clone(phd);
    % set initial number of components.
    numcomps = phdsmall.numcomponents;
    % for small ships, the expected size is about 100 meters in length and
    % 20 meters in width. as the orientation is unknown, we will create 4
    % orientations for each size. first, you must add components to the
    % density at same states. this can be done by simply appending it
    % setup values for small boats
    append(phdsmall,phdsmall);
    append(phdsmall,phdsmall);
    % degrees of freedom for defining shape. a large number represents
    % higher certainty in dimensions.
    dof = 1000;
    % covariance in vx, vy and omega.
    smallstatecov = diag([300 300 50]);
    % scale matrix for small boats
    smallshape = (dof - 4)*diag([100/2 20/2 10].^2); % l, w and h
    % create 4 orientations at 45 degrees from each other.
    for i = 1:4
        thisindex = (i-1)*numcomps   (1:numcomps);
        r = rotmat(quaternion([45*(i-1) 0 0],'eulerd','zyx','frame'),'frame');
        phdsmall.scalematrices(:,:,thisindex) = repmat(r*smallshape*r',[1 1 numcomps]);
        phdsmall.statecovariances([2 4 5],[2 4 5],thisindex) = repmat(r*smallstatecov*r',[1 1 numcomps]);
        phdsmall.statecovariances([6 7],[6 7],thisindex) = repmat(diag([100 100]),[1 1 numcomps]);
    end
    % small ships generate approximately 10-20 detections.
    expnumdets = 15;
    uncertainty = 5^2;
    phdsmall.rates(:) = expnumdets/uncertainty;
    phdsmall.shapes(:) = expnumdets^2/uncertainty;
    phdsmall.degreesoffreedom(:) = dof;
    % follow similar process for large ships.
    append(phdlarge,phdlarge);
    append(phdlarge,phdlarge);
    largestatecov = diag([100 5 10]);
    largeshape = (dof - 4)*diag([500/2 100/2 10].^2);
    for i = 1:4
        thisindex = (i-1)*numcomps   (1:numcomps);
        r = rotmat(quaternion([45*(i-1) 0 0],'eulerd','zyx','frame'),'frame');
        phdlarge.scalematrices(:,:,thisindex) = repmat(r*largeshape*r',[1 1 numcomps]);
        phdlarge.statecovariances([2 4 5],[2 4 5],thisindex) = repmat(r*largestatecov*r',[1 1 numcomps]);
        phdlarge.statecovariances([6 7],[6 7],thisindex) = repmat(diag([100 100]),[1 1 numcomps]);
    end
    % generate approximately 100-200 detections.
    expnumdets = 150;
    uncertainty = 50^2;
    phdlarge.rates(:) = expnumdets/uncertainty;
    phdlarge.shapes(:) = expnumdets^2/uncertainty;
    phdlarge.degreesoffreedom(:) = dof;
    % add large ships to small ships to create total density. this density
    % is added to the total density every step.
    phd = phdsmall;
    append(phd,phdlarge);
end
% when called with detection input i.e. the adaptive birth density, do not
% add any new components.
if nargin > 1
    % this creates 0 components in the density.
    phd = initctggiwphd;
end
end