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:
https://github.com/pbraswell/ArduPlane
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.
TIA,
Peter
You need to be a member of diydrones to add comments!
Replies
Hello all,
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).
Cheers
Sam
Your work looks great. Any progress since the last post?
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
Sam
ekf_4.ino
ExtendedKalmanFilter.cpp
ExtendedKalmanFilter.h
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.
Roger
Hi Peter,
Nice to see so much interest in autonomous soaring! I have been doing a lot of research into this in the last few weeks (finals are coming up so I need something to prevent study!). I have been experimenting with implementing Kalman filters etc. for thermal seeking and it works pretty well in my crude Matlab simulation.
Basically the aircraft is provided with horizontal position and velocity information (mimicking GPS etc.) along with noisy vertical velocity e.g. from GPS or barometer. The Extended Kalman filter maintains a model of where it thinks the thermal is relative to the aircraft along with estimated of strength and size.
The aircraft is in one of four states - one for flying a search pattern (a spiral), one for estimating thermal position once updraft is located (a set circular manoeuvre - this allows the filter to estimate the thermal position more accurately), a thermalling mode where the aircraft orbits the thermal centre, and a cruise mode where the aircraft tracks to the next waypoint.
The aircraft switches state depending on the local updraft (low-pass filtered), estimated thermal size and strength, current altitude etc. The simulation nearly always centres thermals once found (note the thermals are based on the Gaussian distribution of updraft found in the literature, and a global sink is added to enforce mass conservation).
I want to test this algorithm with FlightGear but unfortunately my laptop just won't cut the mustard.
Anyway take a look at the video and if you'd like to have a go collaborating at implementing some of the ideas on Arduplane let me know and I can send code etc. I don't have much experience with C and don't have an APM (yet) but I'm interested as to how feasible it would be to implement a Kalman filter: it involves quite a few matrix multiplication and inverting a 4x4 matrix. I have a few other ideas as well, such as optimising aircraft speed (speed for minimal sinkrate when thermalling, speed for max L/D when cruising, corrected for local air sink) and using aircraft roll rate as well as vertical speed to detect thermals.
I
Great research, if it comes to fruition then it could be combined with flight for powered models for long distance autonomous flights
Some of my planes under manual control can stay in the air indefinately but with the current software it cannot do that automatically and is therefore restircted to battery capacity/efficiency
I wrote some code for a paraglider pilot vario (uses an IIR filter + linear regression). It is reasonably well tested (>3K regular users). If you need a way to get a good vario reading from the barometer in the APM you might try stealing this algorithm. See getVerticalSpeed in this class:
https://github.com/geeksville/Gaggle/blob/master/src/com/geeksville...
The code is pretty straightforward Java and all in that project. You just need the IIR and and regression code.
The standard rule for paraglider pilots is: when the vario starts making the happy noise, fly for up to four seconds (you'd want to make that # adjustable) or until lift starts going down, then start a right (or left) turn. Alter the diameter of the turn to optimize rate of climb (to core the thermal).
I'm more on the copter side rather than the plane side but I do hear people talking about autonomous soaring now and then on the forums so there is some interest out there.
The ArduPlane code is pretty mature now (i.e. very few bugs) and still has plenty of free RAM and CPU cycles so I'm pretty sure it's possible to extend it for what you're talking about...but personally i'm not in a position to help with that..