%% Generate INS Measurements for Radar Scenario
% Generate INS measurements using the insSensor System object™. Use waypointTrajectory to generate the ground-truth path. Use radarScenario to organize the simulation and visualize the motion.
% Specify the ground-truth trajectory as a figure-eight path in the North-East plane. Use a 50 Hz sample rate and 5 second duration.

Fs = 50;
duration = 5;
numSamples = Fs*duration;
t = (0:(numSamples-1)).'/Fs;

a = 2;

x = a.*sqrt(2).*cos(t) ./ (sin(t).^2 + 1);
y = sin(t) .* x;
z = zeros(numSamples,1);

waypoints = [x,y,z];

path = waypointTrajectory('Waypoints',waypoints,'TimeOfArrival',t);

%
% Create an insSensor System object to model receiving INS data. Set the PositionAccuracy to 0.1.

ins = insSensor('PositionAccuracy',0.1);

%
% Create a radar scenario with a single platform whose motion is defined by path.

scenario = radarScenario('UpdateRate',Fs);
plat = platform(scenario);
plat.Trajectory = path;

%
% Create a theater plot to visualize the ground-truth platform motion and the platform motion measurements modeled by insSensor.

tp = theaterPlot('XLimits',[-3, 3],'YLimits', [-3, 3]);
platPlotter = platformPlotter(tp, ...
    'DisplayName', 'Ground-Truth Motion', ...
    'Marker', 's', ...
    'MarkerFaceColor','blue');
insPlotter = detectionPlotter(tp, ...
    'DisplayName','INS Measurement', ...
    'Marker','d', ...
    'MarkerFaceColor','red');

%
% In a loop, advance the scenario until it is complete. For each time step, get the current motion sample, model INS measurements for the motion, and then plot the result.

while advance(scenario)
    motion = platformPoses(scenario,'quaternion');
    
    insMeas = ins(motion);
    
    plotPlatform(platPlotter,motion.Position);
    plotDetection(insPlotter,insMeas.Position);
    
    pause(1/scenario.UpdateRate)
end

% Copyright 2020 The MathWorks, Inc.
%