%% Create Default kinematicTrajectory
% Create a default kinematicTrajectory System object™ and explore the relationship between input, properties, and the generated trajectories.

trajectory = kinematicTrajectory

%
% By default, the kinematicTrajectory object has an initial position of [0 0 0] and an initial velocity of [0 0 0]. Orientation is described by a quaternion one (1 + 0i + 0j + 0k).
% The kinematicTrajectory object maintains a visible and writable state in the properties Position, Velocity, and Orientation. When you call the object, the state is output and then updated.
% For example, call the object by specifying an acceleration and angular velocity relative to the body coordinate system.

bodyAcceleration = [5,5,0];
bodyAngularVelocity = [0,0,1];
[position,orientation,velocity,acceleration,angularVelocity] = trajectory(bodyAcceleration,bodyAngularVelocity)

%
% The position, orientation, and velocity output from the trajectory object correspond to the state reported by the properties before calling the object. The trajectory state is updated after being called and is observable from the properties:

trajectory

%
% The acceleration and angularVelocity output from the trajectory object correspond to the bodyAcceleration and bodyAngularVelocity, except that they are returned in the navigation coordinate system. Use the orientation output to rotate acceleration and angularVelocity to the body coordinate system and verify they are approximately equivalent to bodyAcceleration and bodyAngularVelocity.

rotatedAcceleration = rotatepoint(orientation,acceleration)
rotatedAngularVelocity = rotatepoint(orientation,angularVelocity)

%
% The kinematicTrajectory System object™ enables you to modify the trajectory state through the properties. Set the position to [0,0,0] and then call the object with a specified acceleration and angular velocity in the body coordinate system. For illustrative purposes, clone the trajectory object before modifying the Position property. Call both objects and observe that the positions diverge.

trajectoryClone = clone(trajectory);
trajectory.Position = [0,0,0];

position = trajectory(bodyAcceleration,bodyAngularVelocity)
clonePosition = trajectoryClone(bodyAcceleration,bodyAngularVelocity)

%
% Copyright 2018 The MathWorks, Inc.
%