Call For SITL Experts: Connecting Ardupilot SITL to a MATLAB/Simulink-Based Simulation

Hello!

I am looking for some help developing an interface between the Ardupilot SITL simulator and MATLAB/Simulink.

I am part of a research team in the early stages of developing a high-fidelity model of multi-rotor aircraft. We have access to state-of-the-art wind tunnels and measurement equipment, and several individuals involved are very experienced in aerodynamic modeling. I personally have significant experience modeling and simulating multi-rotor aircraft; for example I was the lead designer for the "Quad-Sim" project when I was completing my undergraduate studies (Mathworks File Exchange Entry, Git-Hub repository,). Since the Quad-Sim project ended I have made a number of significant advances in the area of modeling and simulating multi-rotor aircraft, and I expect the upcoming simulation I am working on (and its derivative works) to have many new and desirable features that I believe will make it useful to a broad audience. Among other things, this upcoming simulation will include wind-tunnel derived flight performance data in order to realistically model the multi-rotor aircraft performance in "off-nominal" conditions, and will be designed to support multi-rotors with various n-motor arrangements (though obviously the wind tunnel data will only apply directly to the configurations we can test). While it is relatively straightforward to simulate the aircraft dynamics in Simulink, simulating the flight controller in a highly accurate and realistic manner is less convenient within this environment. Instead of creating a controller within the Simulink environment to approximate the behavior of the real on-board controller, I would like to link the Simulink model directly to a SITL instance of ArduCopter.

An interface between Simulink and SITL would provide several things to the user community:

  • SITL would now have an interface to the MATLAB/Simulink environment, which I suspect would be of interest to many people, from undergraduate students to professional engineers and researchers. Simulink can be a great environment for assessing the behavior of complex dynamic systems, and I am sure a large number of users would find uses for this interface if it existed.
  • Combined with the new multi-rotor simulation currently under development, users would have a functioning and documented version of a realistic multi-copter simulation which "out-of-the-box" would be compatible with SITL, which could be used as a building block for their own work in this area.
  • Allow simulation of complex real-world scenarios to evaluate how well Ardupilot can handle difficult flight conditions, such as sensor malfunctions, motor failures, severe turbulance, etc. Of course, current SITL simulations could be modified to allow some degree of simulating these failures, but the organizational structure of the MATLAB/Simulink environment makes it a particularly appealing for investigating such areas.

To reiterate, I am proposing to have MATLAB/Simulink be the platform for simulating the dynamics of the vehicle in flight (and simulating the associated feedback from on-board sensors) and letting SITL ArduCopter "fly" the Simulink simulation. Communication between the two environments would be done via the same UDP interfacing methods currently used by SITL to communicate with the physics simulations. Simulink has a toolbox which I have access to (and which is included in current "Student License" downloads) called "Instrument Control Toolbox" which appears to allow two way UDP communication from within Simulink models. I believe Simulink would also be able to be run in real-time modes which should allow it to provide high frame-rate feedback, though details of this are not clear yet. Presumably, simulation output visualization could be done in whatever program the user prefers, such as FlightGear, MAVProxy, or even MATLAB/Simulink.

Please let me know if you think you can help. I have very little experience with the type of programming that will be required to enable this functionality on the Ardupilot side of things. While I have looked through the Ardupilot code quite a bit, I am still unsure how much work would be entailed in setting up this interface from the Ardupilot side, though I believe it should be feasible and perhaps not even particularly difficult for someone with experience. Given some guidance on required communication behavior I believe I can handle the Simulink side of things, though I'm open to any and all collaborators.

I think a number of folks in the community would be very pleased to have this functionality, so please get in touch if you think you can help.


Thanks for reading, and happy flying!
David

Views: 1283

Reply to This

Replies to This Discussion

Hi David,

That sounds like a very nice project!

Adding a new SITL backend is not difficult, assuming you know a bit of C++. The existing SITL backends are here:

https://github.com/ArduPilot/ardupilot/tree/master/libraries/SITL

we support a wide range of backends, some of them using external programs (like yours would) and some implemented entirely internally.

The key things you need to decide on are:

  • what method you will use to communicate with matlab. Will be it a TCP socket? Or UDP? Or shared memory?
  • how will you handle time? Will it use lock-step scheduling? Will time be driven by the ArduPilot side of the simulation or the matlab side?
  • how will absolute position be handled? Will the matlab simulation provide latitude/longitude/altitude and a world with terrain? Or will it just give a relative NED position from startup and let the higher level ArduPilot SITL code map that to a real world position?
  • how will demanded rotor speeds be represented? As PWM values? As thrust values from 0 to 1?

Once you have decided on those 3 things I can recommend which existing backend you should use as a template for your new backend.

I'm the SITL maintainer for ArduPilot. If you need to talk to me more directly then feel free to join our gitter channel or mumble chat.

Cheers, Tridge

my apologies, I see you've already decided on UDP. That is good.

The next key thing is timing. As you are aiming for this to be a high fidelity simulation the handling of time is absolutely critical. What you need is "lock step scheduling". The way that works is that the physics simulation would only advance in time when it receives new input from ArduPilot. Then when the physics simulation has computed a new state for the sensors (acceleration, gyro, position etc) it would send those to ArduPilot along with a new timestamp.

In this way the simulation time between the physics engine and ArduPilot are "locked" together, and the simulation becomes independent of any timing jitter from the operating system. Without using this lock-step method of timing you can still fly, but it will not produce a really accurate simulation, no matter how good the physics simulation is.

Thank you so much for your thoughtful reply, Tridge! It seems I have gotten the right person's attention!

Regarding your questions:

what method you will use to communicate with matlab. Will be it a TCP socket? Or UDP? Or shared memory?

As you noticed in your follow-up post, I thought that UDP seemed to be the standard being used for SITL, so I planned to accommodate that. I'm not really familiar with TCP (or any of these protocols for that matter), but from the small amount of reading I've done I don't see any reason that TCP would be more appropriate. I will mention, however, that I think some people may like to accommodate multiple instances of these SITL simulations running on a single computer to study multi-agent applications. Can you think of any reason UDP would not be able to accommodate this?

how will you handle time? Will it use lock-step scheduling? Will time be driven by the ArduPilot side of the simulation or the matlab side?

and from the second post:

The next key thing is timing. As you are aiming for this to be a high fidelity simulation the handling of time is absolutely critical. What you need is "lock step scheduling". The way that works is that the physics simulation would only advance in time when it receives new input from ArduPilot. Then when the physics simulation has computed a new state for the sensors (acceleration, gyro, position etc) it would send those to ArduPilot along with a new timestamp.

As you pointed out, the Simulink model and ArduPilot need to progress through time together for an accurate simulation. I am looking into how to run Simulink in lock-step with an external program. It doesn't seem to be a commonly attempted thing, so I may need to experiment a bit to get it working. I think it should be feasible to run the model within a triggered block, so that an external block can trigger the aircraft physics to only progress when commands are received from the ArduPilot.

I really don't know how/if things are synchronized with sensor feedback within ArduPilot. I think I will need to better understand this in order to figure out the timing issues and preserve realism while maintaining a reasonable simulation execution speed. At this point I envision maybe something like this, starting from an ArduPilot command update:

  1. ArduPilot computes commands (motor speeds, etc.) and sends a UDP Packet containing commands to Simulink. Lets say this happens at 50 Hz.
  2. Simulink simulates response of vehicle to these commands, computing sensor feedback at whatever rate is needed by ArduPilot (say 1000 Hz)
  3. Simulink sends this stream of sensor data over UDP to ArduPilot, which continues to update its EKF and other internal states.
  4. Simulink stops simulating after the 1/50th of a send has passed, and waits to recieve a packet from ArduPilot
  5. ArduPilot has a current estimate of the vehicle state in its EKF, and is ready to return to step 1.

I realize this might look naive to someone with a better knowledge of ArduPilot's structure, but it's roughly my current understanding.


how will absolute position be handled? Will the matlab simulation provide latitude/longitude/altitude and a world with terrain? Or will it just give a relative NED position from startup and let the higher level ArduPilot SITL code map that to a real world position?

I think at this stage simulating the world including terrain elevation changes is relatively low-priority. However, the more I think about this the more I suspect having Simulink handle all of this will ultimately be the most logical approach, particularly since I would like to let Simulink handle the GPS signals (and potentially GPS outages) at some point. I think either approach you mentioned could be acceptable, so I'd like to hear your opinion.

how will demanded rotor speeds be represented? As PWM values? As thrust values from 0 to 1?

Another good question. I think for simplicity a throttle command between 0 and 1 (essentially a percentage) is acceptable. This can be used to drive motor response dynamics which in turn will determine RPM. I don't think actual simulation of PWM signals is warranted at this stage, except that I do plan to restrict the command update rate to the rate at which the PWM signal is updated by ArduPilot and able to be read by the ESC (I used 50 Hz in my example above, but it can be whatever is realistic). In between command updates, I will assume the ESC uses a zero-order hold.

Thanks again for your feedback, and I look forward to your reply. I will check out Gitter and Mumble chat, and if you think that is a more appropriate place to continue our conversation, I will reply there from now on.

David

Hi David,

but from the small amount of reading I've done I don't see any reason that TCP would be more appropriate.

The main advantage of TCP is it is lossless. If you use UDP then you will need to cope with the possibility of lost packets. Some of the existing SITL backends use TCP and some use UDP.

Simulation systems often use UDP as it has lower overhead, but it really doesn't matter as you will be covering the timing by using lock-step scheduling.

I will mention, however, that I think some people may like to accommodate multiple instances of these SITL simulations running on a single computer to study multi-agent applications. Can you think of any reason UDP would not be able to accommodate this?

that can be done with both UDP and TCP (and is supported by SITL now)

ArduPilot computes commands (motor speeds, etc.) and sends a UDP Packet containing commands to Simulink. Lets say this happens at 50 Hz.
It will need to happen a lot faster than 50Hz. Our main SITL models run at 1200Hz, and you can run them with a speedup factor of up to 10x (depending how fast your computer is). So that is doing 12000 interactions with the physics model per second of wall clock time.
You should aim for at least 400Hz, preferably more. ArduPilot will provide actuator commands at the full rate. As the main loop of ArduCopter will only be running at 400Hz you will get duplicate actuator commands, but that is fine.
  1. Simulink simulates response of vehicle to these commands, computing sensor feedback at whatever rate is needed by ArduPilot (say 1000 Hz)

it should run at the same rate as the actuators from ArduPilot

  1. Simulink sends this stream of sensor data over UDP to ArduPilot, which continues to update its EKF and other internal states.

I'd expect it to send just one UDP packet per iteration. It will contain all of the sensor data.

  1. Simulink stops simulating after the 1/50th of a send has passed, and waits to recieve a packet from ArduPilot

I don't know what you mean by "1/50th of a send" ?

  1. ArduPilot has a current estimate of the vehicle state in its EKF, and is ready to return to step 1.

you really don't need to think about the EKF or any other details of ArduPilot for what you are doing. From your point of view ArduPilot is a black box that provides you with actuator values in exchange for sensor data. Each time ArduPilot sends you a new set of actuator values you should step the physics model one time step forward and then send a single UDP packet with the new sensor data. You just need to work out how you will format the sensor data.

I think at this stage simulating the world including terrain elevation changes is relatively low-priority. However, the more I think about this the more I suspect having Simulink handle all of this will ultimately be the most logical approach, particularly since I would like to let Simulink handle the GPS signals (and potentially GPS outages) at some point. I think either approach you mentioned could be acceptable, so I'd like to hear your opinion.
The ArduPilot SITL framework already has the ability to simulate GPS outages and other sensor errors. I don't think you should try to replicate that. Have a look at the SIM_* parameters in ArduPilot.
I also suggest you try flying the existing copter simulator a bit, and learn to use SITL before you extend it.
Cheers, Tridge

Hi, 

I'm working on a project now as my graduation thesis and I would like do SITL using my Simulink model for a hexacopter but I have little experience with C++, so I'm wondering if you have a backend file already written for communicating with Matlab/Simulink using Arducopter as a controller. 

Thanks.

Reply to Discussion

RSS

Groups

Season Two of the Trust Time Trial (T3) Contest 
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service