waypointTrajectory orientation is not matching

Two trajectories were generated considering the monocular visual-inertial example https://de.mathworks.com/help/fusion/ug/visual-inertial-odometry-using-synthetic-data.html

The first trajectory is same as in example

% track without orientation angles
traj = waypointTrajectory(wayPoints, 'TimeOfArrival', t, ...
    'Velocities', velocities, 'SampleRate', sampleRate);

The second trajectory the orientation is added to the same values in example

Ori = [0 0 0;...
       0 0 0;... 
       90 0 0;... 
       90 0 0;... 
       0 0 0;...  
       0 0 0;...  
       0 0 0;... 
       90 0 0;... 
       180 0 0;... 
       180 0 0;...
       180 0 0];
% track with orientation angles 
traj_Ori= waypointTrajectory(wayPoints, ...
    'TimeOfArrival',t, ...
    'Orientation',quaternion(Ori,'eulerd','ZYX','frame'), ...
    'Velocities', velocities,'SampleRate',sampleRate);

Later both trajectories were plotted together to check if they have same values. The position is exactly matching but the orientation-axis doesn't.

The Roll, Pitch and Yaw angles are following:

What is the correct way to generate the trajectory with given orientation angels?

Answer 1

[Brian Fanous]

Hi Ravindra

In the first example you are not specifying the Orienation so the waypointTrajectory infers that the vehicle is pointed in the direction of travel. In the second example you are explicitly specifying the orientation so the waypointTrajectory uses the Ori array. Try:

help waypointTrajectory/Orientation

Best,

Brian

[Ravindra]

Hi Brian,

Thank you very much for your reply.

I created two trajectories as per instructions: first with the positions, velocity and timeofarrival. The second trajectory with the same positions, velocities, timeofarrival and orientation.

But the calculated attitude of the second trajectory doesn't match with the first trajectory. Whereas, the attitude from first trajectory is correctly matching with the vehicles path.

I couldn't explain this!

I can provide the data as mat files, in case if you want to have a look on it?

Thanks, hope to hear from you

[Brian Fanous]

The calculated attitude (orientation) of the second trajectory should not match the first.

In the first trajectory, by not specifying the orientation, the waypointTrajectory object tries to force the orientation to be such that the vehicle is pointing in the direction of travel. You have left it to the internals of the waypointTrajectory object to compute that orientation.

In the second trajectory you have explicitly specified the orientation you want along your trajectory, The waypointTrajectory object uses your supplied Ori and tries to interpolate an orientation curve to match that. It uses a cubic quaternion spline to fit the supplied orientation. This spline does not exactly match up with the vehicle direction of travel.

If you want the heading to match the direction of travel, it’s best to let the waypointTrajectory compute it and avoid supplying an Orientation property.

[Ravindra]

Thanks that explains the different orientations for both trajectories.

But in case I would like to switch the axis of the vehicle. I mean by default the waypointTrajectory vehicle's x-axis points to front, y-axis to the left and z-axis towards up.

If I would like to calculate a trajectory by switching axis as shown in figure below. How should I proceed?

In this case I guess we should provide all the three angles!

[Paulo Ricardo Marques de Araujo]

Having the same challenge because I am working with data coming from NovAtel systems. Any thoughts on how to model the motion direction to follow the y-axis of the car?