PX4 SITL Dynamic Model Implementation in Simulink

1

I am trying to implement a quadcopter plant in simulink for some initial controls test work with the eventual plan to implement in px4 sitl. I am having issues validating the dynamic model - the drone state output of my implemented model does not line up with the state output from px4 sitl. For px4 sitl, I am using the Iris drone in gazebo. Below are some additional details about my setup that might shed some light on my problem.

  1. Dynamic Model - I am using the dynamic model referenced in the paper on the px4 SIH page. I didn’t see any reference to a dynamic model in the px4 sitl section, so I assumed it uses the same one as SIH. Is my assumption incorrect?

  2. Setup - My simulink model takes the current drone state (position, attitude quaternion, linear velocity, and angular velocity) and motor speeds, evaluates the drone dynamic equations, and outputs the next drone state after integration. The motor speeds (actuator_outputs) are recorded at 250 Hz and grabbed from a QGC flight log files. The speeds are normalized based on the equation (u_i - 1000)/(2000-1000). 1000 and 2000 are the min and max Iris motor speeds, respectively. My simulink loop is set for 250 Hz, and the initial condition in my model is set to the initial state according to the log file. For the dynamic equation parameters (mass, L_pitch, L_roll, etc), I am using the default as referenced in the SIH parameters. Back to my question in 1, if my assumption is incorrect, then I’d assume that I am also using the wrong parameters for the Iris.

  3. Sample Output - Below is a screenshot of the output plot for the z position state. For the test flight, the drone takes off, hovers at an altitude of ~2.5 m, and then lands. The blue line represents the output from px4_sitl, while the red is the output coming from my model. Given that there is nothing in my model telling the drone there is a ground (and positive z is downward according to the model), it makes sense that the the z position shows an initial “free fall”. However, when the drone begins to hover it shows a pretty large fluctuation in altitude and then rises even higher before shooting up again when the drone lands.

    image
    image1464×1472 50.5 KB

Any assistance on where I might be going wrong would be greatly appreciated.

2

Hello,
I am curious to see if you continued your project. Did you find questions to your answers?
I am in the process changing the architecture of PX4 and I am in the early stages of developing a Simulink model of both the autopilot and the plant. Your findings would help me a lot. I am specifically interested in the answer to your first question.

Thank you.

3

hi @Grueling2504 and @Pat,

I have created a similar project:

I have implemented some aspects of the PX4 control architecture in this project, while the dynamic model was imported from:

If anyone’s interested in joining in or collaborating, feel free to jump in.