Hey guys. This may be slightly off-topic but I thought I'd cast a wide net and see what I could come up with.
I've been experimenting with autonomous soaring based on modified code running in on ArduPilot hardware. I've had some limited success with simple algorithms that look at the current rate of climb indications and circle / cruise accordingly. Here's a link to the fork:
I've run across a number of academic papers describing in graphic detail some of the mathematics of autonomous soaring but they all go way over my head. Most talk about Kalman filters and a bunch of other smoothing logic. Anyway, my question has to to with taking this to the next level. Not sure if you all are aware of any resources or projects, code or libraries that I might be able to leverage. Like I said, a bit of a long shot but thought I'd try.
Good stuff! I had a look at your code, its encouraging that altitude can be gained just by switching from CIRCLE mode and back. Essentially the only things that happen differently in my simulation is that the circle centre is a specified coordinate (i.e. LOITER mode) which is continually updated by the Kalman filter.
The drift of the thermal downwind can be accommodated in two ways: i) adjusting the predicted state to take account of wind drift (requiring an estimate of wind speed and direction of course) ii) specifying a high enough state variance. The this also allows for changes in estimated thermal strength and size as thermals apparently widen at altitude.
I am going to spend a bit of time cleaning up the code and then I can send you the Matlab code.
Yes I've looked at the InertialNav library and it's more or less the same as a Kalman filter: the Kalman filter however also maintains an estimate of the variances of its state estimate and computes the gains at each iteration.
Yeah, my code is embarrassingly simple but (in simulation anyway) is surprisingly affective! I've been able to setup simulated 10KM tasks simulated in XPlane and fly them successfully on what I would consider moderate thermal activity ... so I'm encouraged!
Anxiously awaiting your MATLAB code and I'll look at translating that ... Thrilled to death that there are actually some twisted souls out there like me who are interested in this stuff!
Projects of particular interest in case you haven't run across them yet:
I never thought of using circle to send my glider off with the thermal. That would be monster simple. If a little worrying. Big down wind geo fence called for. I think the PX4 might have a little more time on its hands to fiddle with the abacus
I look forward to the results of your in-flight testing. I too hope to be able to do autonomous soaring with Adrupilot. I will be following your work, and that of Jean-louis Naudin with interest.
I spent some time this weekend implementing the Extended Kalman Filter in Arduino. It runs on my pro mini 328 in about 2ms per update, using the MatrixMath library http://playground.arduino.cc/Code/MatrixMath. I wrote a quick sketch to interface it with my Matlab simulation via serial (exactly the same as before but now the EKF update is done on the Arduino) to validate the code: I get the same results as using the Matlab EKF code so pretty happy with that.
I'm sure theres a lot of things that could be done to optimise the code so any suggestions welcome. Does 2ms seem acceptable to run in the ArduPlane loop?
All the best
Dude! You rock! I suck! I've been sooooo busy and just a bit under the weather this week. I PROMISE PROMISE PROMISE to look at your work this week. I'd really like to get it incorporated into a HIL sim in Xplane to see what it does.
By the way, have you seen the new OpenPilot board here: OpenPilot.org?
I haven't looked at the code yet, but the platform looks pretty robust and promising. I just put this board into a Quad Copter and had it configured and flying in less than 1 hour. Pretty impressive considering other experiences I've had ...
At any rate, don't want to upset the apple cart. I'll see about incorporating all this into an APM 2.5 hardware sim in the next couple of weeks.
You would be better off looking at the PX4 for future proofing of the Ardu line.
Haha no worries Peter! It would be nice to get it integrated and see how much it helps ( and if the overhead is okay). I might have a look at incorporating accelerometer data into the vario estimate in the next week or so.
The OpenPilot board looks pretty cool - especially the Revolution with the FPU and mag/pressure sensors. The software is well documented too by the looks of it.
I have been working on the autonomous thermalling project on and off when I have time, and recently I have made some progress on integrating it into the Arduplane code, using Peter's modified version of 2.68 as a basis. I finally got HIL simulation working with FlightGear, which has allowed some testing. It has taken some experimentation to get the filter working properly, but it is now behaving reasonably well and the aircraft is able to climb without difficulty during a typical thermal encounter.
Getting it working in a more realistic simulation required some extra bits of code, such as getting the airspeed controller working well in HIL and implementing a netto variometer calculation to correct the vario reading for aircraft speed and bank angle.
Here is an image of a typical thermal encounter in Google Earth:
This uses FlightGear's ‘thermal_demo’ scenario, which is quite useful for testing since the exact position of the thermal is known (to me, not to the autopilot!). The filter inputs and other flight data are printed to the console, which allows evaluating the filter's performance in Matlab. You can see the measurement is quite noisy but the Extended Kalman Filter does a pretty good job of moving the estimate of the thermal position in the right direction as the aircraft circles (the real thermal is at [0,0]). The netto rate measurement uses the raw rate of change of barometric altitude, corrected using the expected aircraft sink rate.
The filter performance is quite sensitive to the initial state covariance as well as the measurement and process noises. Having a very large initial covariance on the position states helps a lot with getting the initial estimate of thermal position, while adding a very small process noise keeps the ability to adjust the thermal estimate to changing conditions e.g. the drop off in strength at the top of the thermal, which you can see the estimate captures pretty well. The estimate of radius is a bit odd and isn't adjusted much from the initial value, this is mainly because the expected up draft strength is quite a weak function of the thermal radius and so the filter is more inclined to adjust the other states. Increasing the initial covariance of this state may help a bit.
The implementation of the Kalman filter in Arduplane is exactly the same as that in Matlab, so the filter can be re-run in Matlab using the same flight path and measurements and it will give the same result. This allows a direct comparison of filters using different setting for initial covariances and measurement and process noise which is very useful for finding combinations of parameters which work well, such as in the plot below where four filters were run on the same data (the filter running in Arduplane is the blue line below, which is covered by the red line.The red line is produced by the Matlab filter running with the same parameters as used in the Arduplane filter, showing that the results are identical).
Anyway I thought this might be interesting to people around here, I have found it quite an interesting project and it has taught me a lot about Kalman filter theory and about the Arduplane code. There is a still quite a lot to be done, such as testing in simulation with a variety of thermal sizes and strengths. I would also like to move to using the latest version of Arduplane, although I had some issues getting HIL working with it, possibly due to the removal of the ATTITUDE_HIL setting. Then with a bit of luck it may be possible to do some flight tests when the weather improves.
Your work looks great. Any progress since the last post?