Validate Multirotor UAV Model by Playing Back Flight Log in Simulink

This example shows how to validate a UAV attitude controller and plant model using a flight log collected by the UAV from a test flight. You can use the validated controller and plant model to design higher order navigation controllers, and tune the attitude controller to handle different UAV payload weights. You can also use this model as a baseline to redesign controller architecture. For example, you could use it to replace PID control with model predictive control (MPC).

Background

To gather the flight test data, this example used a UVify IFO-S mounted on a Eureka Dynamics FFT GYRO 450 Carbon Pro test platform. While attached to the platform, the UAV attempted to fly in stabilized mode, and recorded the corresponding roll and pitch commands to track the attitude controller response for roll and pitch. Because the FFT GYRO test platform has substantial inertia, the trial was repeated with the UAV mounted in two distinct configurations, to mitigate the effect of inertia on the roll and pitch data:

• Roll controller response configuration — Roll axis of the UAV aligned with the central mounting plate rotation axis.

• Pitch controller response configuration — Pitch axis of the UAV aligned with the central mounting plate rotation axis.

After conducting several test flights for both configurations, PX4 autopilot generated a ULOG file containing the data collected from the UAV.

Flight Log Data Preprocessing

Select which configuration to use the data set from to validate the Simulink® controller and plant mode.

checkRoll = true;

Download the ULOG for the selected controller.

if checkRoll
    logData = ulogreader("rollTracking.ulg");
else
    logData = ulogreader("pitchTracking.ulg");
end

Extract the logged signals for the vehicle attitude, attitude rate, corresponding setpoints, and actuator control commands, and synchronize them to the sample rate of 250 Hz, to match the sample time configuration of the mPlaybackFlightLogToSimulatedUAV Simulink plant model.

data = logData.readTopicMsgs(TopicNames= ...
                            ["vehicle_attitude", ...
                             "vehicle_attitude_setpoint", ...
                             "vehicle_rates_setpoint", ...
                             "vehicle_angular_velocity", ...
                             "actuator_outputs", ...
                             "actuator_controls_0", ...
                             "rate_ctrl_status", ...
                             "manual_control_setpoint"]);

Get the setpoint control inputs from the data, containing the roll angle, pitch angle, and thrust force of the UAV. Then, use the timestamps and setpoints to create timetables.

attitudeSetPoint = data{strcmp(data.TopicNames,"vehicle_attitude_setpoint"),"TopicMessages"}{1};
rollSetPoint = timetable(attitudeSetPoint.timestamp,attitudeSetPoint.roll_body);
pitchSetPoint = timetable(attitudeSetPoint.timestamp,attitudeSetPoint.pitch_body);
thrustSetPoint = timetable(attitudeSetPoint.timestamp,-attitudeSetPoint.thrust_body(:,3));

Get the attitude rate setpoints that contain the setpoints for the roll, pitch, and yaw rates. Then, use the timestamps and rate setpoints to create timetablesfor the roll, pitch, and yaw rates.

attitudeRateSetPoint = data{strcmp(data.TopicNames,"vehicle_rates_setpoint"),"TopicMessages"}{1};
rollRateSetPoint = timetable(attitudeRateSetPoint.timestamp,attitudeRateSetPoint.roll);
pitchRateSetPoint = timetable(attitudeRateSetPoint.timestamp,attitudeRateSetPoint.pitch);
yawRateSetPoint = timetable(attitudeRateSetPoint.timestamp,attitudeRateSetPoint.yaw);

Define a simulation sample rate of 250 Hz. This sample rate is the same as the PX4 flight controller update rate.

FS = 250;

Get the UAV flight states, such as attitude, angular velocity, actuator control commands, and integrators. Use the exampleHelperSlerpData helper function to get the attitude data as quaternions.

attitude = data{strcmp(data.TopicNames,"vehicle_attitude"),"TopicMessages"}{1};
timesamples = seconds(attitude.timestamp);
newtimesamples = (timesamples(1):1/FS:timesamples(end))';
q = exampleHelperSlerpData(attitude.q,timesamples,newtimesamples);
quaternionOutput = timetable(seconds(newtimesamples),q);

angularVelocity = data{strcmp(data.TopicNames,"vehicle_angular_velocity"),"TopicMessages"}{1};
rollRate = timetable(angularVelocity.timestamp,angularVelocity.xyz(:,1));
pitchRate = timetable(angularVelocity.timestamp,angularVelocity.xyz(:,2));
yawRate = timetable(angularVelocity.timestamp,angularVelocity.xyz(:,3));

actuator = data{strcmp(data.TopicNames,"actuator_outputs"),"TopicMessages"}{1};
motorCmds = timetable(actuator.timestamp,actuator.output(:,1:4));

actuatorControls = data{strcmp(data.TopicNames,"actuator_controls_0"),"TopicMessages"}{1};
actuatorCmds = timetable(actuatorControls.timestamp_sample,actuatorControls.control(:,1:4));

controlIntegrators = data{strcmp(data.TopicNames,"rate_ctrl_status"),"TopicMessages"}{1};
rollIntegrator = timetable(controlIntegrators.timestamp,controlIntegrators.rollspeed_integ);
pitchIntegrator = timetable(controlIntegrators.timestamp,controlIntegrators.pitchspeed_integ);
yawIntegrator = timetable(controlIntegrators.timestamp,controlIntegrators.yawspeed_integ);

Since the flight log may contain invalid data as a result of external perturbations on the test platform or unexpected control commands to the UAV, you must select a section of the log that contains clean data. For example, a clean section could be one in which the UAV performs only pitch rotations or only roll rotations.

Depending on which response configuration you select, the example uses a different time range of the data to show the results.

if checkRoll
    % roll tracking time for rollTracking.ulg
    TS = seconds(30) + logData.StartTime;
    TF = seconds(39) + logData.StartTime;
else
    % pitch tracking time for pitchTracking.ulg
    TS = seconds(5) + logData.StartTime;
    TF = seconds(17) + logData.StartTime;
end

Use the synchronize function, specifying linear interpolation, to combine all the signals into one timetable and synchronize them to the same sample rate. Set the timestamps of the timetable to start at 0 seconds for consistency with the UAV model which also starts at 0 seconds.

combinedData = synchronize(rollSetPoint,pitchSetPoint, ...
                           yawRateSetPoint,thrustSetPoint, ...
                           quaternionOutput, ...
                           rollRate,pitchRate,yawRate, ...
                           motorCmds,actuatorCmds, ...
                           rollRateSetPoint,pitchRateSetPoint, ...
                           rollIntegrator,pitchIntegrator,yawIntegrator, ...
                           "regular","linear",SampleRate=FS);
combinedData = combinedData(combinedData.Time>TS & combinedData.Time<TF,:);
combinedData.Time = combinedData.Time - combinedData.Time(1);

Verify Controller Model Using Open-Loop Simulation

The UAV model in this example contains two parts: the flight controller and the plant dynamics. The first goal is to verify the accuracy of these two parts using flight log. To do this, you must use an open-loop simulation to compare the controller model and flight log results. Once you verify that the simulated controller performance matches the controller output from the flight test, compare the closed-loop response of both the controller and plant dynamics in the simulation to the flight test results.

First, set the ECS protocol parameters for the UVify IFO-S motor.

DShotRange = 1000;
DShotMid = 1000;
MaxRPM = 13702.726;
MaxOmega = MaxRPM*2*pi/60;
MotorCmdToOmegaSqured = MaxOmega^2/DShotRange;

Open the mPlaybackFlightLogToSimulatedUAV model, which simulates the UAV controller and plant. The attitude controller generates roll, pitch, and yaw actuator setpoints that enable the UAV to fly in stabilized mode. This controller architecture is similar to a PX4 controller, and you can use the PX4 controller parameters to setup your PID controller blocks. Additionally, setting the openLoopSim parameter allows you to switch between running this model as an open-loop simulation by disabling the feedback, or as a closed-loop simulation by enabling the feedback.

open_system("mPlaybackFlightLogToSimulatedUAV")

Open the stabilized model controller, and then open the subsystem containing the attitude controller. This subsystem contains a proportional outer control loop for the roll and pitch angle control, and a PID inner control loop for roll, pitch, and yaw rate control.

open_system("mStablizedModeController")
open_system("mStablizedModeController/Controller/Attitude controller")

Read the PX4 parameters from the log data. Extract multicopter parametrs by selecting parameters that start with "MC_".

px4Params = logData.readParameters;
px4Params = px4Params(startsWith(px4Params.Parameters,"MC_"),:)

px4Params=36×2 table
        Parameters        Values
    __________________    ______

    "MC_ACRO_EXPO"          0.69
    "MC_ACRO_EXPO_Y"        0.69
    "MC_ACRO_P_MAX"          720
    "MC_ACRO_R_MAX"          720
    "MC_ACRO_SUPEXPO"        0.7
    "MC_ACRO_SUPEXPOY"       0.7
    "MC_ACRO_Y_MAX"          540
    "MC_AIRMODE"               0
    "MC_AT_EN"                 0
    "MC_BAT_SCALE_EN"          0
    "MC_MAN_TILT_TAU"          0
    "MC_PITCHRATE_D"      0.0016
    "MC_PITCHRATE_FF"          0
    "MC_PITCHRATE_I"         0.2
    "MC_PITCHRATE_K"           1
    "MC_PITCHRATE_MAX"       220
      ⋮

Set up attitude and rate parameters for the roll, pitch, and yaw rate in the mStablizedModeController model by using PX4 configuration values from the log data.

rollRateP = px4Params.Values(strcmp(px4Params.Parameters,"MC_ROLLRATE_P"));
rollRateI = px4Params.Values(strcmp(px4Params.Parameters,"MC_ROLLRATE_I"));
rollRateISaturation = px4Params.Values(strcmp(px4Params.Parameters,"MC_RR_INT_LIM"));
rollRateD = px4Params.Values(strcmp(px4Params.Parameters,"MC_ROLLRATE_D"));
rollP = px4Params.Values(strcmp(px4Params.Parameters,"MC_ROLL_P"));
pitchRateP = px4Params.Values(strcmp(px4Params.Parameters,"MC_PITCHRATE_P"));
pitchRateI = px4Params.Values(strcmp(px4Params.Parameters,"MC_PITCHRATE_I"));
pitchRateISaturation = px4Params.Values(strcmp(px4Params.Parameters,"MC_PR_INT_LIM"));
pitchRateD = px4Params.Values(strcmp(px4Params.Parameters,"MC_PITCHRATE_D"));
pitchP = px4Params.Values(strcmp(px4Params.Parameters,"MC_PITCH_P"));
yawRateP = px4Params.Values(strcmp(px4Params.Parameters,"MC_YAWRATE_P"));
yawRateI = px4Params.Values(strcmp(px4Params.Parameters,"MC_YAWRATE_I"));
yawRateISaturation = px4Params.Values(strcmp(px4Params.Parameters,"MC_YR_INT_LIM"));
yawRateD = px4Params.Values(strcmp(px4Params.Parameters,"MC_YAWRATE_D"));

To verify that the controller modelworks similarly to the PX4 firmware, you can run an open-loop simulation by playing back the flight control signals and UAV states to the controller. Then, compare the actuator signal logged by the PX4 firmware to that of the Simulink simulation.

Set up the initial simulation states, and run the model in an open-loop simulation. Ignore the warnings for unsaved model changes.

initialAttitudeEuler = quat2eul(combinedData.q(1,:));
initialAngleRate = double(combinedData{1,6:8});
initialRollIntegrator = double(combinedData{1,13});
initialPitchIntegrator = double(combinedData{1,14});
initialYawIntegrator = double(combinedData{1,15});

openLoopSim = true;
setActiveConfigSet("mStablizedModeController","Simulation")

outOpenLoop = sim("mPlaybackFlightLogToSimulatedUAV");

### Searching for referenced models in model 'mPlaybackFlightLogToSimulatedUAV'.
### Found 2 model references to update.

Warning: Model 'mPlaybackFlightLogToSimulatedUAV' or its dependencies have unsaved changes, which increase the time required to compile the model hierarchy and check whether model reference targets are up to date. The unsaved changes also prevent parallel building of referenced models.

Warning: Model 'mStablizedModeController' has unsaved changes. 
Suggested Actions:
    • Save model 'mStablizedModeController'. - Fix

### Starting serial model reference simulation build.
### Successfully updated the model reference simulation target for: mMultirotorPlantModel
### Successfully updated the model reference simulation target for: mStablizedModeController

Build Summary

Simulation targets built:

Model                     Action                        Rebuild Reason                                       
=============================================================================================================
mMultirotorPlantModel     Code generated and compiled.  mMultirotorPlantModel_msf.mexa64 does not exist.     
mStablizedModeController  Code generated and compiled.  mStablizedModeController_msf.mexa64 does not exist.  

2 of 2 models built (0 models already up to date)
Build duration: 0h 1m 25.583s

Compare the logged roll and pitch angles to the roll and pitch angle setpoints.

logAttitudeZYX = quat2eul(combinedData.q);
figure
tiledlayout("flow");
ax = nexttile;
hold on
plot(ax,combinedData.Time,logAttitudeZYX(:,3))
plot(ax,combinedData.Time,combinedData.Var1_rollSetPoint,"--")
legend("Log","Setpoint")
title("Roll")
ylim(ax,[-0.4 0.4]);

ax = nexttile;
hold on
plot(ax,combinedData.Time,logAttitudeZYX(:,2))
plot(ax,combinedData.Time,combinedData.Var1_pitchSetPoint,"--")
legend("Log","Setpoint")
title("Pitch")
ylim(ax,[-0.4 0.4])

Compare the logged and simulated angular velocity setpoints for roll and pitch. This comparison verifies that the roll and pitch angle P-controllers in Simulink resemble the PX4 attitude angle controllers. However, you can expect some discrepancies in pitch or roll rate setpoints due the differences in the integrator implementation of the Simulink controller model and the PX4 controller.

simulationRollRateSetpoint = timetable(seconds(outOpenLoop.rollRateSetPoint.Time),outOpenLoop.rollRateSetPoint.Data);
simulationPitchRateSetpoint = timetable(seconds(outOpenLoop.pitchRateSetPoint.Time),outOpenLoop.pitchRateSetPoint.Data);
figure
tiledlayout("flow");
ax = nexttile;
hold on
plot(ax,combinedData.Time,combinedData.Var1_rollRateSetPoint)
plot(ax,simulationRollRateSetpoint.Time,simulationRollRateSetpoint.Var1)
legend("Log","Setpoint")
title("Roll Rate")
ylim(ax,[-1 1]);

ax = nexttile;
hold on
plot(ax,combinedData.Time,combinedData.Var1_pitchRateSetPoint)
plot(ax,simulationPitchRateSetpoint.Time,simulationPitchRateSetpoint.Var1)
legend("Log","Setpoint")
title("Pitch Rate")
ylim(ax,[-1 1]);

Compare the logged thrust, yaw, pitch, and roll values to the simulated actuator commands for thrust, yaw, pitch, and roll torque. Note that the simulation output closely resemble the PX4 logged output.

titleText = ["Thrust","Yaw","Pitch","Roll"];
simulationPreMixOutput = timetable(seconds(outOpenLoop.ActuatorControl.Time),squeeze(outOpenLoop.ActuatorControl.Data));

figure(Name="ActuatorControl")
tiledlayout("flow");
for idx = 1:4
ax = nexttile;
hold on
logActuatorData = timetable(combinedData.Time,combinedData.Var1_actuatorCmds(:,4-idx+1));
plot(ax,logActuatorData.Time,logActuatorData.Var1,"--")

simActuatorData = timetable(simulationPreMixOutput.Time,simulationPreMixOutput.Var1(:,idx));
plot(ax,simActuatorData.Time,simActuatorData.Var1)
legend("Log","Setpoint")
title(ax,titleText(idx))

if idx > 1
    ylim(ax,[-0.3 0.3])
end
end

Verify Plant Model Using Closed-Loop Simulation

Use a closed-loop simulation to compare the plant model and flight log results. In this example, the UAV was attached to a test rig that could freely rotate in the roll, pitch, yaw axes. Thus, when modeling the plant, input only the torques generated from propellers to the 6-DOF model. The model also adds the inertia of the test rig to the inertia of the UAV.

open_system("mMultirotorPlantModel")
open_system("mMultirotorPlantModel/Plant Model")

Open the mMultirotorPlantDataDictionary data dictionary, and get the parameters to model the UVify IFO-S UAV, mounted on the Eureka Dynamics FFT GYRO test rig. If you want to model a different UAV with a different test rig, you must modify these parameters to match your UAV to ensure accurate results. The UVify IFO-S UAV specifications and Eureka Dynamics FFTGyro 450 Pro specifications are provided under their respective license permissions: UVify IFO-S Spec Data Set License.txt and FFT Gyro 450 Pro Carbon Data Sheet License.txt.

datadictionary = Simulink.data.dictionary.open("mMultirotorPlantDataDictionary.sldd");
datasection = getSection(datadictionary,"Design Data");
VehicleEntry = getEntry(datasection,"Vehicle");
VehicleVal = getValue(VehicleEntry);
VehicleVal.Airframe.mass = 1.319;

Set the inertia of the UAV, test rig plate, inner gimbal, and outer gimbal.

VehicleVal.Airframe.inertia = [0.005329 -0.000039 0.000126;
                              -0.000039 0.006984 -0.000047;
                               0.000126 -0.000047 0.010358];
plateInertia = 1.83e-3;
innerGimbalInertia = 1.17e-1;
outerGimbalInertia = 1.56e-1;

Set the total inertia for the test rig based on the testing configuration you selected.

if checkRoll
    testRigInertia = [plateInertia 0 0;
                      0 innerGimbalInertia 0;
                      0 0  outerGimbalInertia];
else
    testRigInertia = [innerGimbalInertia 0 0;
                      0 plateInertia 0;
                      0 0  outerGimbalInertia];
end

Add the UAV frame inertia to the total testing rig inertia.

VehicleVal.Airframe.inertia = VehicleVal.Airframe.inertia + testRigInertia;

Set the UAV dimensions. These dimensions are for the UVify IFO-S UAV.

% Diameter is used for computing drag
VehicleVal.Airframe.diameter = 0.366918;
% XY and H are motor offset to center of the UAV
% H is negative since the body frame of the UAV is Z -down
VehicleVal.Airframe.xy = 0.111194;
VehicleVal.Airframe.h = -0.025943;
VehicleVal.Airframe.Cdx = 0;
% Rotor parameters used for computing thrust and torque generated by
% propellers
VehicleVal.Rotor.radius = 0.089;
VehicleVal.Rotor.area = 0.0248845554;
VehicleVal.Rotor.Ct = 0.018209784484015;
VehicleVal.Rotor.Cq = 0.002774692311888;
% Motor parameters used for converting DShot signals back to rotor
% rotational speed
VehicleVal.Motor.thrustToMotorCommand = 1000;
VehicleVal.Motor.commandToW2Gain = MotorCmdToOmegaSqured;
VehicleVal.Motor.minLimit = 1;
VehicleVal.Motor.maxLimit = 1000;

Update the data dictionary with these parameters.

setValue(VehicleEntry,VehicleVal);
setValue(getEntry(datasection,"vehicleInertia"),VehicleVal.Airframe.inertia);
setValue(getEntry(datasection,"RotorCt"),VehicleVal.Rotor.Ct);
setValue(getEntry(datasection,"RotorCq"),VehicleVal.Rotor.Cq);

Run the closed-loop simulation with both the controller and plant, and compare the response from the simulation to the logged results. Ignore the warning for unsaved model changes.

openLoopSim = false;

initialRollIntegrator = 0;
initialPitchIntegrator = 0;
initialYawIntegrator = 0;

outCloseLoop = sim("mPlaybackFlightLogToSimulatedUAV");

### Searching for referenced models in model 'mPlaybackFlightLogToSimulatedUAV'.
### Found 2 model references to update.

Warning: Model 'mPlaybackFlightLogToSimulatedUAV' or its dependencies have unsaved changes, which increase the time required to compile the model hierarchy and check whether model reference targets are up to date. The unsaved changes also prevent parallel building of referenced models.

Warning: Model 'mStablizedModeController' has unsaved changes. 
Suggested Actions:
    • Save model 'mStablizedModeController'. - Fix

### Starting serial model reference simulation build.
### Model reference simulation target for mMultirotorPlantModel is up to date.
### Successfully updated the model reference simulation target for: mStablizedModeController

Build Summary

Simulation targets built:

Model                     Action                        Rebuild Reason                                       
=============================================================================================================
mStablizedModeController  Code generated and compiled.  Model or library mStablizedModeController is dirty.  

1 of 2 models built (1 models already up to date)
Build duration: 0h 0m 30.539s

Compare the roll and pitch tracking performance between the logged experiment and simulation.

logAttitudeZYX = quat2eul(combinedData.q);
simulationAttitudeZYX = quat2eul(squeeze(outCloseLoop.quaternion_m.Data)');

figure
tiledlayout("flow");
ax = nexttile;
hold on
plot(ax,combinedData.Time,logAttitudeZYX(:,3))
plot(ax,combinedData.Time,combinedData.Var1_rollSetPoint,"--")
plot(ax,seconds(outCloseLoop.quaternion_m.Time),simulationAttitudeZYX(:,3))
legend("Log","Setpoint","Simulation")
title("Roll")
ylim(ax,[-0.5 0.5]);

ax = nexttile;
hold on
plot(ax,combinedData.Time,logAttitudeZYX(:,2))
plot(ax,combinedData.Time,combinedData.Var1_pitchSetPoint,"--")
plot(ax,seconds(outCloseLoop.quaternion_m.Time),simulationAttitudeZYX(:,2))
legend("Log","Setpoint","Simulation")
title("Pitch")
ylim(ax,[-0.5 0.5]);

Controller Deployment from Simulink

Now that you have verified the performance of the controller in simulation compared to the original PX4 firmware, you can deploy it to the UVify IFO-S UAV using the PX4 Hardware Support Package.

open_system("mQuadcopterAttitudeController")
open_system("mQuadcopterAttitudeController/Attitude Controller")

Use the same stabilized mode controller model for deployment. First, change its configuration for hardware deployment.

setActiveConfigSet("mStablizedModeController","PX4Deployment")

Configure the model to connect with the UVify IFO-S target. Connect the PX4 autopilot board of the UAV to the MATLAB host computer. Then, on the Hardware tab of the Simulink toolstrip, click Build, Deploy & Start to build the firmware and upload it to the UVify IFO-S.

Test the UAV, using the deployed controller, on the test rig. Download the ULOG for the deployed controller to analyze the pitch and roll perfomance.

deployedData = ulogreader("rollTrackingDeployed.ulg");
deployedFlightSignals = deployedData.readTopicMsgs(TopicNames=["manual_control_setpoint","vehicle_attitude"]);
deployedFlightAttitudeSetPoint = deployedFlightSignals{strcmp(deployedFlightSignals.TopicNames,"manual_control_setpoint"),"TopicMessages"}{1};
deployedFlightAttitude = deployedFlightSignals{strcmp(deployedFlightSignals.TopicNames,"vehicle_attitude"),"TopicMessages"}{1};
deployedTS = deployedData.StartTime;
deployedTF = deployedData.StartTime;
deployedFlightAttitudeZYX = quat2eul(deployedFlightAttitude.q);
figure
tiledlayout("flow");
ax = nexttile;
hold on
plot(ax,deployedFlightAttitude.timestamp,deployedFlightAttitudeZYX(:,3))
plot(ax,deployedFlightAttitudeSetPoint.timestamp,max(min(deployedFlightAttitudeSetPoint.y,20/180*pi),-20/180*pi),"--")
legend("Log","Setpoint")
title("Roll")
ylim(ax,[-0.5 0.5])

ax = nexttile;
hold on
plot(ax,deployedFlightAttitude.timestamp,deployedFlightAttitudeZYX(:,2))
plot(ax,deployedFlightAttitudeSetPoint.timestamp,max(min(deployedFlightAttitudeSetPoint.x,20/180*pi),-20/180*pi),"--")
legend("Log","Setpoint")
title("Pitch")
ylim(ax,[-0.5 0.5])

Once you have a Simulink model that can realistically simulate the UAV attitude controller response, you can use this model to further optimize the controller configuration. You can optimize the controller configuration by improving the PID parameter tuning, or by changing the controller architecture to either a linear quadratic regulator (LQR) controller or an MPC controller. When making improvements, using a simulation enables you to quickly iterate on controller design and code generation. Additionally, deployment from a Simulink model enables you to update the autopilot firmware without manually coding the controllers again.

Close all models without saving changes.

close_system("mQuadcopterAttitudeController",0)
close_system("mPlaybackFlightLogToSimulatedUAV",0)
close_system("mMultirotorPlantModel",0)
close_system("mStablizedModeController",0)
discardChanges(datadictionary)
close(datadictionary)