Simulation of UAV trajectory results in discontinuity in measurements

I am trying to simulate a trajectory for a fixed wing UAV to test my model. I have tried multiple of the included functions in UAV toolbox, but all of them results in sudden "jumps" in the orientation and acceleration values. I also tried one of the new functions in R2022b, fixedwingFlightTrajectory, but the discontinuity also appeard here. It is important for my project to have smooth measurements in order to simulate a real case fixed wing UAV flight.

I see that these jumps are present in the example code presented on MathWorks Documentation for WaypointTrajectory (https://se.mathworks.com/help/uav/ref/waypointtrajectory-system-object.html), but I want to have smooth measurements. I have enabled autoPitch and autoBank to true, and it still shows a lot of discontinuities. The jumps happen when the simulation reaches the waypoints, but I don't know how to get rid of them.

The code implemented is:

waypoints = [0 0 0; 0 0 0; 37.5 5 -9; 70 9 -20; 79 -20 -25; 120 20 -30];

traj = waypointTrajectory("Waypoints", waypoints ,"TimeOfArrival",[0 0.001 5.5 10 15 20], "AutoPitch", true, "AutoBank", true, "SampleRate", 250); 

[pos, orient, vel, acc, angvel] = traj();

  i = 1;
  spf = traj.SamplesPerFrame;
  while ~isDone(traj)
      idx = (i+1):(i+spf);
      [pos(idx,:), orient(idx,:), ...
          vel(idx,:), acc(idx,:), angvel(idx,:)] = traj();
      i = i+spf;
  end
 orientationMeasurements = eulerd(orient,'ZYX','frame');

When i then try to plot the outcome, i get this:

The position is fine, but the orientation and acceleration has sudden jumps which is not realistic, hence I cannot use it for my model.

Is there any way of obtaining a smooth simulation using any of the simuation options in UAV Toolbox, or is there any other methods I should try?

[Greg Dionne]

Hi Andrea,

There are a few ideas that may interest you depending on how you answer the following questions:

1. Are you taking your waypoints/time-of-arrival from recorded data?
2. Are you trying to free-hand sketch a path and smoothly go through it? (It looks like you need three derivatives)
3. Do you have time-of-arrival information for the waypoints, or are you trying to free-hand those as well?
4. Is the auto-orientation facility of the waypointTrajectory important to you? (it looks like you're configuring it for fixed-wing craft).

[Andrea Schnell]

Hi, thanks for the reply.

I do not have any recorded data, so the waypoints I am using is just sketched to simulate a path that is as realistic as possible for a fixed-wing. The time-of-arrival values are also free-handed.

The auto-orientation is set to true because I want the orietnation values to correctly represent how the fixed-wing is orientet throughout the trajectory.

don't know if this answered your questions, but my goal with the simulation is to use the position and orientation values to derive acceleration and angular velocity values to use as IMU measurements. Then I will use the position and the IMU measurements in a Kalman filter to estimate the position (given some added noise to the measurements).

[Greg Dionne]

Hi Andrea,

Thanks for the reply.

Of the tools in R2022b UAV Toolbox I think one might be of interest to you - this would be the minimum jerk polynomial trajectory minjerkpolytraj. That should let you use a set of waypoints and have it automatically compute velocities based upon your free-hand time-of-arrival input. Once you have those, you can resample the resulting trajectory with the same time-of-arrival input and take the resulting positions, velocities, and time-of-arrival as input to the waypointTrajectory (which has the desired banking controls you're after). The waypointTrajectory uses cubic Hermite interpolation in the x-y plane with respect to time-of-arrival when you feed it groundspeed or velocity information. My hunch is that will minimize discontinuities in the third derivative - especially if you re-use the time-of-arrival instants that you used to construct the original polynomial.

Maybe give that a try?

Answer 1

[Jim Riggs]

The discontinuities that you see in the trajectory data are, in fact, realistic, and correspond to command inputs. These happen when the air vehicle changes control mode from straight/level flight to a banked turn. If the controller is "fast", the control forces change very suddenly, resulting in a discontinuity in the accelerations and derivatives (angle rates/linear velocity). For example, a control surface may be moved in a fraction of a second, resulting in a large change in the aerodynamic force and moment. This happens at the start and end of a banked maneuver for the case of a UAV flying a racetrack trajectory, as the UAV goes from level to banked orientation and back again.

There is nothing unrealistic about this, in fact, this is quite to be expected for a UAV under automated control, since the control modes tend to change in a discrete fashion whereas a human pilot would naturally tend to ease into a maneuver change with less abruptness.