Kalman Filter for gyro/gps heading with bias

[Michael Shimniok]

For what it's worth, I think I finally got a Kalman Filter put together that handles gyro bias properly. I wrote a Python script to simulate a noisy, biased gyro and running the KF on it seems to produce the correct bias, heading, and heading rate estimates. The state matrix consists of heading, heading rate, and bias.



The state transition model, A, comes from the following state space equations:





Bias doesn't come into play for the x and omega equations because I wanted the omega state to represent the unbiased heading rate.

However, the measurement matrix, z, is measuring heading (from GPS) and/or biased heading rate (from gyro); theta plus B. It's a 2 by 1 matrix and the measurement noise matrix, R, is 2 by 2. I used a 2 by 3 matrix for the observation model, H, to keep the C code simpler (I'll explain if I'm not making sense). So in the code I'd use one of these:



The covariance matrix, P, and process noise matrix, Q, are 3 by 3. I set the noise (Q) on the bias term much lower than the noise for heading and heading rate so that it's unlikely to change rapidly. The Kalman gain matrix, K, is 3 by 2.

The python code simulates my robot at the starting line where the Kalman Filter is fed a fixed, initial heading, as well as gyro readings.

#!/usr/bin/python
# Attempt to simulate gyro unbiasing with KF

import numpy as np
import Gnuplot
import Gnuplot.funcutils

# Simulate heading/gyro measurements
dt = 0.050
x = np.matrix('250.0; 0.0; 0.0')
z = np.matrix('0.0; 0.0')
A = np.matrix('1.0, 0.010, 0.0; 0.0, 1.0, 0.0; 0.0, 0.0, 1.0')
H = np.matrix('0.0, 0.0, 0.0; 0.0, 0.0, 0.0')
K = np.matrix('0.0, 0.0; 0.0, 0.0; 0.0, 0.0')
Q = np.matrix('1.0, 0.0, 0.0; 0.0, 0.01, 0.0; 0.0, 0.0, 0.0001')
R = np.matrix('1.0, 0.0; 0.0, 0.01')
P = np.matrix('1.0, 0.0, 0.0; 0.0, 1000.0, 0.0; 0.0, 0.0, 1000.0')
I = np.matrix('1.0, 0.0, 0.0; 0.0, 1.0, 0.0; 0.0, 0.0, 1.0')

def kf():
    global x, z, A, H, K, P, Q, R
    x = A * x
    P = A * P * A.T + Q
    tmp = H*P*H.T + R
    K = P * H.T * np.linalg.inv(tmp)
    x = x + K * (z - H * x)
    P = (I - K * H) * P

for i in range(0, 500):
    z[0] = 250.0
    z[1] = 0
    H = np.matrix("1.0, 0.0, 0.0; 0.0, 0.0, 0.0")
    kf()
    for j in range(0, 5):
        z[0] = 0.0
        z[1] = np.random.normal(loc=2.0, scale=0.2, size=None)
        H = np.matrix('0.0, 0.0, 0.0; 0.0, 1.0, 1.0')
        kf()
        print "{0} {1} {2} {3}".format(x.item(0), z.item(1), x.item(1), x.item(2))

And the results it spit out looks (more or less) like this:

Heading       Gyro Meas.   Gyro Estimate  Bias estimate
------------- -----------  -------------- -------------
250.000803122 2.068865221  0.059612669932 1.99414280292
250.001096855 1.984590479  0.016170658023 1.99370628247
250.002010814 2.196609645  0.131137258287 1.99485809281
250.003306030 2.121820431  0.128574296969 1.99483244498
250.005630843 2.396441600  0.296119221915 1.99650834720
250.003423340 2.161971804  0.202052218422 1.99581619362
250.003479512 1.748572189 -0.080147381578 1.99298057953
250.004171524 2.283563084  0.148101634804 1.99526731895
250.003521646 1.592751190 -0.189919752743 1.99188470709
250.001741991 1.833376847 -0.170644179744 1.99207751484
249.999908724 2.158480786  0.071353215462 1.99452645037
250.001422094 2.248822533  0.186258274093 1.99568103552
250.000634635 1.524116374 -0.218746687000 1.99162346017
250.000032581 2.182543527  0.032745611072 1.99414016393
249.999867536 1.897471796 -0.046667895640 1.99334581497

Note the gyro measurement versus the gyro estimate. The estimate centers around 0--unbiased. The bias term is a close match to the 2.0 I set in the np.random.normal() call. Of course now I have to test it on the actual robot...

[Ted Meyers]

Whoa!   That's cool, you're better at math than I am.  What's cool (and surprising to me) is that I thought that a KF was a sensor fusion algorithm, but you're using it with one sensor.  Interesting.  I would think that it wouldn't take much to add odometer data to this.

Ted

[Michael Shimniok]

Sent from my iPhone

Whoa!   That's cool, you're better at math than I am.

It's all because of Sebastian Thrun's class :) 

 What's cool (and surprising to me) is that I thought that a KF was a sensor fusion algorithm, but you're using it with one sensor.  Interesting.  I would think that it wouldn't take much to add odometer data to this.

Actually it fuses gps heading and gyro. ;)

By the way I should've verified that it would work with changing heading before posting. Luckily for me it does. :D

The problem with adding odometer as in position based on distance and heading is you introduce sine and cosine which are nonlinear so then you have to start using an Extended Kalman Filter. That requires partial derivatives.

I haven't tried one yet but I think I can do it. I'm tempted. Maybe for 2015. 

Something is wrong with the implementation on Data Bus. More work required. 

Michael

--
You received this message because you are subscribed to the Google Groups "diyrovers" group.
To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+...@googlegroups.com.
To post to this group, send email to diyr...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/db8b54c4-1652-4dd9-86b1-22ac3cc53c92%40googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Ted Meyers]

Oh, sorry I missed that.  I thought that you were saying that the gps heading was only used for the initial starting position.   Makes more sense now.  

Actually it fuses gps heading and gyro. ;)

By the way I should've verified that it would work with changing heading before posting. Luckily for me it does. :D

Well, you could just use the heading from the odometers (instead of the GPS), or the compass heading for that matter.  But I imagine that since the odometer heading has the same sort of drift error as the gyro, it doesn't help as much as GPS or compass.

 The problem with adding odometer as in position based on distance and heading is you introduce sine and cosine which are nonlinear so then you have to start using an Extended Kalman Filter. That requires partial derivatives.

BTW, does GPS heading just use motion to compute heading, or is there a component of it based on satellite geometry?

Ted 

[Borna Emami]

Michael,

One thing to take into account for fusing GPS and Gyro is that the gyro reading is instantaneous, but the GPS reading is delayed up to a second. Even with the nice GPS receivers we have on 0x27's bot, I was seeing around 180ms of delay, which at the speeds and loop gains we were using, it becomes significant.

Because of this, I wrote and algorithm that would store the last 2 seconds of data from each sensor and constantly correlate them with each other to estimate the delay. Then every time a new gps reading is available it would get compared with the gyro reading from x ms ago to calculate the bias and also correct the current angle by a percentage of the difference.

This can also be modeled into a kalman filter if you know ahead of time what the delay is but the matrices would get very large. 0.5 seconds of delay with 100Hz update rate would make your matrix 50x50 which is not implementable in the standard way and some sparse processing trick would be needed.

another simple option which I recommend would be to run complementary low pass/ high pass filters between GPS and gyro. essentially make the gyro angle drift towards the GPS angle with a ~1 second time constant which works good enough to the point that you would not notice a difference in performance. I don't like this since it is a band aid type solution that does not really deal with the source of the issue, however it is a damn good one.

Unfortunately 0x27 will not make it to the event this year. I was looking forward to run the unicycle around the track but 2 of the team members and I are traveling and will be out of the country.

Regards,
Borna Emami

--

You received this message because you are subscribed to the Google Groups "diyrovers" group.
To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+...@googlegroups.com.
To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/96e8bdc9-9063-4872-bffc-1d5eb644847d%40googlegroups.com.

[Michael Shimniok]

Borna,

Sorry to hear you and your team won't make it this year. Hopefully next
year.

> One thing to take into account for fusing GPS and Gyro is that the
> gyro reading is instantaneous, but the GPS reading is delayed up to a
> second. Even with the nice GPS receivers we have on 0x27's bot, I was
> seeing around 180ms of delay, which at the speeds and loop gains we
> were using, it becomes significant.

Most definitely! I saw about 600ms on the Venus (2012) and now 400ms on
the uBlox6. I was seeing problems even at modest speeds.

> Because of this, I wrote and algorithm that would store the last 2
> seconds of data from each sensor and constantly correlate them with
> each other to estimate the delay.

I found the delay experimentally. I logged and analyzed a number of runs
and found the delay to be fixed.

> Then every time a new gps reading is available it would get compared
> with the gyro reading from x ms ago to calculate the bias and also
> correct the current angle by a percentage of the difference.

Cool. That's precisely what I did for 2012. It's neat to hear that the
first place team was using the same approach. :) Obviously it works
pretty well given our results. :D Here are the writeups I did in case
anyone is interested.

http://www.bot-thoughts.com/2012/05/avc-whats-wrong-with-data-bus.html
http://www.bot-thoughts.com/2012/07/avc-heading-and-position-estimation.html

> This can also be modeled into a kalman filter if you know ahead of
> time what the delay is but the matrices would get very large. 0.5
> seconds of delay with 100Hz update rate would make your matrix 50x50
> which is not implementable in the standard way and some sparse
> processing trick would be needed.

I was talking to Scott H. about doing this in the KF. I think handling
it programmatically is probably simplest.

Michael

[Michael Shimniok]

On 06/04/2014 10:31 PM, Ted Meyers wrote:

Well, you could just use the heading from the odometers (instead of the GPS), or the compass heading for that matter.  But I imagine that since the odometer heading has the same sort of drift error as the gyro, it doesn't help as much as GPS or compass.

One needs a drift-free source. With the approach I'm exploring, the Kalman filter is fed an assumed heading at the start (the bearing of the next waypoint from the starting point), which should generate a gyro bias estimate. That estimate should remain stable for a little while even in the absence of additional drift-free heading references. Which means that one could possibly run without GPS at all.

 The problem with adding odometer as in position based on distance and heading is you introduce sine and cosine which are nonlinear so then you have to start using an Extended Kalman Filter. That requires partial derivatives.

BTW, does GPS heading just use motion to compute heading, or is there a component of it based on satellite geometry?

I'm guessing it's computed with doppler shift, satellite geometry, or other magic. It's definitely not computing from the position estimate.  It's also filtered.  So far, three GPSs that I've used seem to report really good values for heading. Just delayed some amount from reality.

Michael

[Minuteman]

The gyro can provide an essentially drift-free source. We find the drift over 60 seconds to be less than 1/2 degree. We use an MPU6050, but sample the z axis at 1 kHz instead of using the IMU mode. It's very stable. For that reason, we have never bothered with GPS or any other source of direction sensing. 

We have considered other sources of gyro error such as chassis roll, but they don't seem to have much impact.  

[Eric Flam]

Curious about two things:

1. Did you intend to use dt somewhere?  Seems like it should be in A where 0.01 is.

2. Why are there two loops?  It seems you are saying that GPS data is coming in at a 1:5 ratio to gyro data, but I don't see that explained - is that correct?

This is great tho, one of the only simple to read kalman codes I've been able to find.  I can't figure out how it works to separate the estimate and bias from the measurement, but it does work so I'll just have to stare at it longer until I figure it out.

[Michael Shimniok]

Looks like this slipped through the cracks, sorry. 

I did try this on the actual robot... it didn't go so well. Then we started work on the Jeep and this fell by the wayside.

I'm hoping to get back to this. I've started working on Data Bus again after several years. I'm about ready to capture data again.

On Mon, Aug 6, 2018 at 2:39 PM Eric Flam <eflam...@gmail.com> wrote:

Curious about two things:

1. Did you intend to use dt somewhere?  Seems like it should be in A where 0.01 is.

I did intend to populate A with dt instead of a hardcoded number but this was a prototype algorithm so that didn't happen before I wrote the above :)

 

2. Why are there two loops?  It seems you are saying that GPS data is coming in at a 1:5 ratio to gyro data, but I don't see that explained - is that correct?

That's correct. The updater runs at 100Hz to poll the gyro and encoders, but the GPS (at the time, a Venus 638FLPx) provided updates at 10Hz (IIRC). The updates were sent by the GPS asynchronously. I believe I time synced the GPS data to the gyro data before running it through this prototype Octave algorithm.

 

This is great tho, one of the only simple to read kalman codes I've been able to find.  I can't figure out how it works to separate the estimate and bias from the measurement, but it does work so I'll just have to stare at it longer until I figure it out.

The bias is part of the state space model where ψ is the heading and ⍵ is the heading rate and β is the bias:

ψ0 = ψ1 + ⍵dt

⍵0 = ⍵1 + β

β0 = β1

(I think that's right)

Matrix A captures the state space equations and x the state variables ψ, ⍵, and β.

Michael