Research by Ryan Beall and Chaz Henderson:
I did a little more refining of the model from the data previously described/collected from the Pixhawk. The following are the results of System Identification and PID optimization.
The overall performance of the bank angle controller was suboptimal for our first flights. It appeared to oscillate in numerous log files (2-3 degrees amplitude with a period of 0.5-1 seconds). This was indicative of gains that were too high. A significantly more accurate model was produced by finding a very clean step response in the data as seen below:
This was generated with regression so you can see that it doesn’t fit perfectly. However, it was clear that this model was a drastic improvement to the previously submitted model. The regression was also used to model the servo delay (0.04 seconds). This was more of a black box approach and neglected any cross coupling in the yaw axis (fairly good assumption for this airframe) The model above is:
As expected, this model is slightly over damped as seen by the pole-zero plot
This model was then implemented in Simulink to verify performance with PID values used during flight:
Using gains that were close to default (what was used on initial flight), the following was the estimated response for a +-25 degree roll step response:
Roll_P: 0.4
Roll_I: 0.05
Roll_D: 0.05
This performance is very close to what was observed in the flight data logs which is very reassuring that the model is a remarkably close approximation of the aircraft/autopilot system as a whole.
Updating/tuning the PID values based on this approximate model, resulted in the following improved simulated performance
The PID values in the above illustration are as follows and will be used for the next flight evolution.
Roll_P: 0.13
Roll_I: 0.02
Roll_D: 0.04
Note that the P gain was cut in half and the I and D were decreased slightly. These are pretty low gains compared to most aircraft but this airframe has an absolutely INSANE roll rate and my intuition of cutting the gain in half was well in alignment with the model's prediction. Hopefully we see verified results on the next test flight... until then...
Fly Safe Fly Fast
-Beall
Comments
We flew again today with the new gains. Perfect! Solid performance as expected. We don't have any video but it's crazy small in the sky at 4000' agl so it would be hard to see anyway :)
Found it : Rapid Prototyping and Quick System Identification (part 1)
Is it gliding yet ? any videos ? Really cool project man
Is that a kind of mini shuttle / SpaceShipOne attached under the wing ?
This was unapologetically a low fidelity model and I was simply trying to smartly narrow my options. I set the N at zero but you theoretically can run the PID in discrete mode which should help your described issue. I don't think it would be unreasonable to align the low pass in Simulink with the low pass in the PID code. I wasn't so concerned with D in this case as much as I was P considering the dynamics of this aircraft are slightly further from the norm than most. The D and I gains would likely follow suit somewhere an order of magnitude less both in simulation and in practice so I'm not too concerned now that I'm probably in the proverbial 'box'. As I'm not in need of a high fidelity model, I may not push those boundaries until i do i guess. But I think Tridge is right, the duality of having a method that can provide you simulator stability derivatives as well as tune your PIDs would be ideal on many fronts!
I think the hardest PID gain to match with Simulink is D. Simulink will accompany D with N (The low-pass filter time constant) which also depends on sampling time.
Did you find any difficulties there?
Along similar lines, I have been thinking it would be nice to expose a parameter vector of floats (along with a vector of names) from the various SITL simulated aircraft, and build an optimisation system that creates parameters for SITL to match a dataflash log file. So you could take a log and create a simulated aircraft that responds in a similar fashion. Then you could play with tuning that model.
I don't know if the very simplistic fixed wing model we have in SIM_Plane.cpp is good enough to try this on, but perhaps with a bit more effort it could be. It sure would be handy to auto-build a sim for any aircraft you have a log for.
Also, given how responsive your aircraft is, maybe running the plane code at more than 50Hz is worthwhile? You can set that in SCHED_LOOP_RATE in 3.5.0. That would also give you more detailed logs.
Yeah that's sort of the goal for this project at face value. I did account for 'some' of the APM specific PID specific scalings, but I'm sure you could help me find more (IE Feed Forward as recommended). In a perfect world, the param file would clue the python script into all of these nuances. I did it all pseudo manually in simulink, but it could automated with a little thought. I think a more pointed approached of flying code real time that estimates the pole positions would be a more methodical way of doing it. I think a quick and dirty (stable) way would even be as simple as a look up table for ranges of pole positions etc. The big boys update the plant model in flight the whole time similarly to how the ekf is approached so it's technically all feasible.
very nice analysis! Does this take into account some of the oddities in the ArduPilot PID code? There are quite a few weird things in their for historical reasons.
Note also that APM:Plane now supports RLL2SRV_FF as a simpler alternative to a P gain. It is a true feed-forward, rather than the rather complex expression for P gain.
It would be really nice to put together a program (maybe a python script?) that users could use to auto-estimate good PID values from a log.