Tuesday, May 1, 2012

AVC: GPS Lag, Gyro, Heading Estimation


You may recall that Data Bus was behaving erratically, steering in wild circles. Why?

With only 4 days left until the big demo, I better figure it out and fix it, fast!

A number of people have written with some excellent thoughts and insight that have been invaluable in understanding the problem and various solutions. Here are some clues...


Here's the first clue. I changed the steering control's feedback gain and got much better results as seen in the video below. No more drunken butterfly but there's quite a bit of steering overshoot.


Here's another clue. Check this plot of heading data from the gyro and the GPS and the detail plot as well.


The shift between gyro and GPS is because GPS data lags reality by a full second. The GPS is doing its own filtering, quite a bit of it, in fact judging by the smoothness of the curve compared to that of the gyro. That filtering introduces lag (or phase shift if you like to think in those terms).

One thing I've learned in the last couple years is that I must think very carefully about, basically, everything, or else Murphy bites me in the butt.

I knew there was a GPS lag but had hoped I could get away with ignoring it for a little while. I can't. Not completely.

Several options (thanks to the very smart folks who have suggested most of these)

  • Try the Venus "high dynamic range" firmware hoping it will have less lag
  • Use a GPS with less lag, like a GS407, uBlox 5
  • Change the path following algorithm 
  • Accommodate for the lag in software
I will try the Venus firmware soon and see if it has any impact on lag.

I don't have $90 burning a hole in my pocket at the moment so the uBlox will have to wait until after the May 5 demo.

As for path following, some improvements can be made. It's a topic all unto its own so look for that in the future.

Accommodating GPS lag in software is doable. The main utility of the GPS is to provide an absolute reference for heading, with which, I can correct gyro drift. (Let's ignore position for this discussion).

If the GPS lag is fixed, software can keep a historical record of gyro readings and compare the most recent GPS data (representing heading 1000ms ago) with the gyro reading 1000ms ago to calculate bias error. Once determined, the correction can be applied to the most recent gyro data.

Gyro drift is slow with a time constant that is surely much longer than 1 second. So a feedback system that has lags by 1 second is no big deal. Then gyro becomes the main heading sensor.

There's more to consider but hopefully in the next 4 days I can pull all this together. We'll see...

Epilog: I was able to pull it off right before my demo. Stored historical gyro data, send that and GPS data into Kalman Filter, use this historical heading estimate to correct position and heading up to the present time. Et voila.

1 comment:

  1. One approach is to delay the gyro data so that it's "aligned" with the GPS data. Then you can compute an error term that is used to correct for gyro bias. Since the gyro drift is so slow, this should work and is essentially a complementary filter with a hack added to deal with the time delay in the GPS data. Control systems with delays are notoriously difficult and require special approaches.

    Good luck!

    Tobor doesn't use the GPS heading information at all. I found that it wasn't very reliable, so I ignore it. GPS coordinates over time along with two-wheel odometry is used to correct for gyro drift via the magic of an EKF.

    I'm hoping to make it to your demo, but I'm in Ohio right now for work. I may have to come straight from the airport!

    ReplyDelete