%% Generate INS Measurements for a Turning Platform
% Generate INS measurements using the insSensor System object™. Use waypointTrajectory to generate the ground-truth path.
% Specify a ground-truth orientation that begins with the sensor body x-axis aligned with North and ends with the sensor body x-axis aligned with East. Specify waypoints for an arc trajectory and a time-of-arrival vector for the corresponding waypoints. Use a 100 Hz sample rate. Create a waypointTrajectory System object with the waypoint constraints, and set SamplesPerFrame so that the entire trajectory is output with one call.

eulerAngles = [0,0,0; ...
               0,0,0; ...
               90,0,0; ...
               90,0,0];
orientation = quaternion(eulerAngles,'eulerd','ZYX','frame');

r = 20;
waypoints = [0,0,0; ...
             100,0,0; ...
             100+r,r,0; ...
             100+r,100+r,0];

toa = [0,10,10+(2*pi*r/4),20+(2*pi*r/4)];

Fs = 100;
numSamples = floor(Fs*toa(end));

path = waypointTrajectory('Waypoints',waypoints, ...
    'TimeOfArrival',toa, ...
    'Orientation',orientation, ...
    'SampleRate',Fs, ...
    'SamplesPerFrame',numSamples);

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

ins = insSensor('PositionAccuracy',0.1);

%
% Call the waypoint trajectory object, path, to generate the ground-truth motion. Call the INS simulator, ins, with the ground-truth motion to generate INS measurements.

[motion.Position,motion.Orientation,motion.Velocity] = path();
insMeas = ins(motion);

%
% Convert the orientation returned by ins to Euler angles in degrees for visualization purposes. Plot the full path and orientation over time.

orientationMeasurementEuler = eulerd(insMeas.Orientation,'ZYX','frame');

subplot(2,1,1)
plot(insMeas.Position(:,1),insMeas.Position(:,2));
title('Path')
xlabel('North (m)')
ylabel('East (m)')

subplot(2,1,2)
t = (0:(numSamples-1)).'/Fs;
plot(t,orientationMeasurementEuler(:,1), ...
     t,orientationMeasurementEuler(:,2), ...
     t,orientationMeasurementEuler(:,3));
title('Orientation')
legend('Yaw','Pitch','Roll')
xlabel('Time (s)')
ylabel('Rotation (degrees)')

%
% Copyright 2020 The MathWorks, Inc.