Team,
After experimenting with several dead-reckoning algorithms over the past 2 years, I finally have a simple algorithm that I like. The above pictures show a comparisons of a GPS reported trajectory (without compensation for GPS latency), with the corresponding dead-reckoning (IMU) computed trajectory (which includes compensation for GPS latency). In the first picture, the GPS is an EM406 reporting locations at 1 Hz. The IMU is computing locations at 40 Hz. The IMU track is plotted at 4 Hz. The aircraft is an EasyStar flying at 8 meters/second in gusty, 4 meters/second winds from the northeast. The autopilot is the UAVDevBoard running "MatrixPilot" with both wind estimation and dead reckoning turned on. At the beginning of this flight segment, the plane was flying away from waypoint 0 in stabilized mode, and was commanded to fly to waypoint 0. It made a smooth "U" turn, was buffeted by gusty winds after it crossed over the tree line, and then recovered.
The second picture is taken from one of Ric Kuebler's flights of his FunCub (thank you, Ric), flying at about 20 meters/second in winds with peak gusts of 10 meters per second.
The code for the algorithm is attached, deadReckoning.c. It is trivially simple:
1. Use the direction cosine matrix to transform body frame accelerometer measurements into earth frame, and account for gravity. (not shown in the code.)
2. Integrate the acceleration to get velocity in earth frame. Integrate velocity to get location.
3. Compute the difference between GPS and IMU velocity and position. Use simple proportional feedback to acceleration and velocity to close the loop and prevent drift.
Actually, I had tried this algorithm once before, and it did not work very well in heavy winds, so I put it on the shelf and tried some other techniques, which did not work either. I recently realized that the key to making any dead-reckoning algorith work is to have accurate values for roll and pitch and GPS latency effects to start with. So, my recent efforts have been to improve the fundamentals in MatrixPilot. Here are two key recent changes:
1. An improved method for accounting for GPS latency. It is a non-linear "feed-forward" technique, based on geometrical concepts that take into account forward velocity and turning rate.
2. An improvement in accounting for forward and centrifugal acceleration effects. I have known for some time that acceleration adjustments should be based on air speed, not ground speed, but the first version of MatrixPilot was using ground speed because at the time I had not yet figured out how to estimate wind speed. I finally got around to making the necessary revisions, and it has made a huge improvement in performance.
By the way, MatrixPilot does not have a direct measurement of airspeed. It infers it from wind speed and ground speed using a "wind estimation" algorithm. If you have a direct airspeed measurement, the algorithm should perform even better.
Best regards,
Bill
Comments
http://en.wikipedia.org/wiki/Monte_Carlo_method
Would this have any utility in bench testing the algorithms?
Would using the Red Version 3 board with Matrix Pilot Version 3 be a helpful learning for one not having flown rc models prior but having flown private aircraft? I have a trainer transmitter with software - crashes a lot.
John
Your system sounds impressive, be sure to post some telemetry tracks when you have some data.
One testing challenge I faced was coming up with a way to determine the accuracy of the results. For example, suppose the GPS disagrees with your IMU, how do you know which one is right? You need a test pilot with good eyes, keen observational powers, and a photographic memory. Either that, or you can do some ground tests with a bicycle or car over a known path. In my case, I relied on the test pilots in the UAVDevBoard community, especially Ric Kuebler, to tell me when my latest scheme was not working. By the way, once I got it working, the IMU positions agreed rather well with the raw GPS positions, with just a latency adjustments. So if your IMU trajectory does not follow the reported GPS trajectory, you have a problem somewhere.
Regarding GPS latency, I tried about 4 different ways to account for it, until I found a simple one that seems to work rather well. What I did was analyze the errors in GPS velocity and position reports during a steady turn, and expressed them in terms of the GPS information that is available. It turns out you can get get a pretty accurate estimate of the latency errors using two sets of GPS data, the most recent one, and the previous one. You then add the computed errors to the actual data. Most of the analysis is obvious and straight forward, but some care is required in reflecting turn effects into the position error estimates. There is a cute principle that you can use for position error: when the GPS is moving through a turn, three points in a row define two chords. The two chords will be about the same length, and the angle between them will be equal to the change in course over ground. You can figure out the rest from there if you want to use the same method.
Best regards,
Bill
I found the simple method that I am using now to be just as accurate as the more sophisticated methods that I tried, it is more robust, and more tolerant of error sources. I also found that there are many subtle sources of error that may be difficult to get into a simulation. As you may know, there are many UAVDevboard users who do lots of simulations using a hardware-in-the-loop simulator. They report much better performance in simulations than we see in actual flights. So, I will be very interested in your results.
When I was working on the first few ideas I had, I imagined that I might be able to maintain accuracy for some substantial period of time without a GPS. Those ideas were characterized as IMU computation of location, with GPS drift compensation.
When I realized that there was no hope of completing a mission without GPS, I revised my thinking, and my later ideas are characterized as a fusion of GPS and IMU information using a complementary filter, with a crossover frequency of a few Hertz.
As I mentioned, one of the keys to getting this sort of thing to work is to have an accurate estimate of attitude in all possible orientations of the plane. When you say you are using rate integration to get attitude, I am not entirely sure what method you are using. Extended Kalman filter? DCM? Quaternions? I assume that you take into account the nonlinear properties of the rotation group?
Best regards,
Bill
You are absolutely right, it is loosely-coupled GPS/INS integration. My main goal was to produce position estimates at 40 Hz to decouple the navigation performance from the GPS reporting rate, to enhance the performance at high air speeds (say 200 miles per hour) when using low GPS reporting rates (such as 1 Hz with the EM406.) I think it will provide stable control all the way up to the speed of sound, though you might need to run the algorithm at a higher rate, and use gyros and accelerometers with higher bandwidth than what most of us are using at the moment. The point is, it is possible to use the sensors rather than the GPS for high bandwidth navigation.
I have been working on this for 2 years. There were 3 previous versions that did not work very well.
I have not done any GPS removed testing, its on my list of things to do. Error in velocity will grow linearly with time after GPS is removed, error in position will grown quadratically. The only question is what the proportionality factors are.
The code was attached in the original post, so you can look at it if you are interested in details.
The drift compensation is trivially simple. Whenever GPS velocity and location information becomes available, it is compensated for latency, and then compared with IMU information to form error signals for velocity and acceleration. The error signals are used until 1 of 2 things happen: when a valid GPS report comes in, they are updated, and if the GPS goes off line, the errors are set to zero. While the GPS is off line, then the computations really are dead-reckoning.
I tried doing the compensation corrections in the body frame, and assign them as biases to the sensors. That performed the worst of all the ways I attempted. It produced an annoying, if somewhat interesting, problem: there was a feedback loop involving the calculations, the sensors, and the motion of the plane that would create a low frequency oscillation that would grow in amplitude until everything went crazy.
There were several sophisticated ideas that seemed to work on paper, that just did not work out in the air.
In the end, I found that out of the 4 methods that I tried, the simplest one worked the best. After transforming sensor data into the earth frame of reference, all of the rest of the computations are done in the earth frame.
The real key to making this method (or probably any other variation) is to have accurate attitude information to start with. And the key to that is to use airspeed rather than ground speed to account for acceleration.
Best regards.
Bill
I have not done any tests or simulations to see how long dead reckoning will work without a GPS. However, the dead reckoning code in MatrixPilot is set up to continue to navigate without the GPS if the GPS does not produce a valid navigation solution. This should work just fine if the GPS drops out for short periods of time. For longer periods, there are several sources of error that work against you, mainly sensor offsets and calibration errors.
In MatrixPilot, we are using a combination of "integrate and dump" and oversampling to get the raw gyro drift rate down to less than 0.2 degrees per minute. That leaves the gyro gains and the accelerometer offsets as the limits. In principle, it is possible to compute the exact gyro gains in flight (that is on my list of things to do). For the accelerometer offsets, you could do manual calibration.
So, it is possible to extend the length of time you could manage without a GPS, but I think the best you would ever be able to do would be around 1 minute. After that, the double integration involved in the dead reckoning computations would accumulate too much error. The error in location is proportional to the square of the length of time without a GPS.
Best regards,
Bill
Hi,
normally I was reading about dead reckoning considering flying under no GPS conditions for some time. Did you try or simulate how your algorhythm would perorm in that situation? Or if only GPS gives you erraneous values for some seconds (happened to me in flight...).
Probably the sensors have to much drift/noise for this kind of job, but would be worth a try.
Cheers
Marc
Hi Ole,
Regarding implementation of the dead-reckoning algorithm in ArduPilot Mega, you will have to ask the ArduPilot team. Certainly, I would be willing to help them.
Best regards,
Bill
Hi,
here is an appetizer, just to watch at my first results with this new improvement.
Ric
-
1
-
2
-
3
of 3 Next