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
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
Hi Happy,
Wind vector (X, Y, and Z components) in centimeters per second is already in the UDB extra output in the F2 message. It has been there for quite some time. For example:
:wvx-237:wvy270:wvz231
Hi Morli,
I am sorry to hear you are having trouble with your ICD2. Are you trying to use it to program your UDB? I am not sure what you mean by "write to UDB". If you are having trouble programming your UDB with an Olimex ICD2 that you bought from SparkFun, I suggest you contact them, explain the problem, and see if they will take it in exchange toward a PICkit. We have found the PICkits work rather well, but the ICD2s have a history of problems.
Best regards,
Bill
Hi Bill,
yes, this really rocks !
Today during my flights I was impressed how the plane flew.
Thank you very much for this great improvement.
I'm going to publish a blog about my FunCub, but this will take some time.
Time to go for another Funjet round ?
Best regards,
Ric
Good to hear from you, man. I saw your posting the other day with the question about how the GY401 heading-hold gyro works. If you ever find out, I would love to know.
Best regards,
Bill
-
1
-
2
-
3
-
4
of 4 Next