Rate-gyro and compass: Kalman filter

Hi to all,

I have an underwater vehicle with an electronic compass and a rate-gyro.

You know, the compass has a good long-term accuracy but it's slow and 
noisy on short-term (there are several DC brushless motors in the nearby).
On the other side, the rate-gyro is more accurate on the short-term and 
for not-so-slow turns.

I want to mix up the two values in order to get a more accurate heading 
output. As far as I know I need a Kalman filter, I read something on the 
web.

Anyway, I can't figure out how to design it. May you help me with some 
code or any documents related?

Thanks
Marco Trapanese

On Wed, 27 May 2009 17:20:53 +0200, Marco Trapanese wrote:

> Hi to all,
> 
> I have an underwater vehicle with an electronic compass and a rate-gyro.
> 
> You know, the compass has a good long-term accuracy but it's slow and
> noisy on short-term (there are several DC brushless motors in the
> nearby). On the other side, the rate-gyro is more accurate on the
> short-term and for not-so-slow turns.
> 
> I want to mix up the two values in order to get a more accurate heading
> output. As far as I know I need a Kalman filter, I read something on the
> web.
> 
> Anyway, I can't figure out how to design it. May you help me with some
> code or any documents related?
> 
AFAIK you'll end up needing a 3-axis gyro.  Your vehicle isn't going to 
be running on rails, and you need all three axes to keep track of your 
one "cared about" axis as the vehicle rotates in all possible ways.

See this thread; search down to the word "bibliography" for my 
suggestions on this subject.

-- 
http://www.wescottdesign.com

Tim Wescott ha scritto:

> AFAIK you'll end up needing a 3-axis gyro.  Your vehicle isn't going to 
> be running on rails, and you need all three axes to keep track of your 
> one "cared about" axis as the vehicle rotates in all possible ways.


Hi Tim,

you helped me a lot in the past with your answers and your book!
You're right when you say I need a 3-axis gyro. And this will be 
mandatory on bigger vehicles (to say the truth, I will use a 6 DOF IMO) 
but on smaller ones I have to use only the z-axis gyro.

They don't run on rails, indeed, but the pitch and roll ranges are about 
±7° in normal conditions. Furthermore, I don't need a huge accuracy. At 
the moment I use only the compass output (with a PID) to do the auto 
heading and the response is quite poor.

I tried to use only the rate-gyro, integrated to output an heading value 
and this improved the overall performance.

For this first step, I just need to mix up the compass with the gyro 
output. Next upgrade will consider a more complex sensor and control loop.


> See this thread; search down to the word "bibliography" for my 
> suggestions on this subject.


Did you mean "see this link" - your website?

Marco

On Thu, 28 May 2009 09:52:25 +0200, Marco Trapanese wrote:

> Tim Wescott ha scritto:
> 
>> AFAIK you'll end up needing a 3-axis gyro.  Your vehicle isn't going to
>> be running on rails, and you need all three axes to keep track of your
>> one "cared about" axis as the vehicle rotates in all possible ways.
> 
> 
> Hi Tim,
> 
> you helped me a lot in the past with your answers and your book! You're
> right when you say I need a 3-axis gyro. And this will be mandatory on
> bigger vehicles (to say the truth, I will use a 6 DOF IMO) but on
> smaller ones I have to use only the z-axis gyro.
> 
> They don't run on rails, indeed, but the pitch and roll ranges are about
> ±7° in normal conditions. Furthermore, I don't need a huge accuracy. At
> the moment I use only the compass output (with a PID) to do the auto
> heading and the response is quite poor.
> 
> I tried to use only the rate-gyro, integrated to output an heading value
> and this improved the overall performance.
> 
> For this first step, I just need to mix up the compass with the gyro
> output. Next upgrade will consider a more complex sensor and control
> loop.

With two closely related inputs like this, if you don't need the 
acquisition capabilities of a Kalman, you can approximate what you want 
with plain old filtering and intuition.  You won't get an optimum filter, 
but then you wouldn't get that with the Kalman exercise unless you got 
all the noise variances and other error terms very well modeled.

First, consider a pair of inputs that are just noisy versions of the same 
thing:

ra = x + na,
rb = x + nb

If you filter these right then you'll get back your signal plus noise:

x_est = H * ra + (1 - H) * rb.

So, for example, if H is a 1st-order lowpass filter with DC gain of 1, 
then 1 - H will be a first-order highpass filter with AC gain of 1.

But what you have is approximated by

ra = x + na     (your compass)
rb = dx/dt + nb (your gyro)

So make a third signal:

rc = rb / s     (I'm playing fast and loose with notation)

then

rc = x + nb / s

Now you know that the noise on ra is severe but good at DC, and you know 
the noise on rc is infinite at DC.  So choose that 1st-order lowpass 
filter again, and use

x_est = H * ra + (1 - H) * rc.

That should get you close.

>> See this thread; search down to the word "bibliography" for my
>> suggestions on this subject.
> 
> 
> Did you mean "see this link" - your website?

No, I meant this: http://www.dsprelated.com/showmessage/111654/1.php.  I 
just left it out...



-- 
http://www.wescottdesign.com

Tim Wescott ha scritto:

> First, consider a pair of inputs that are just noisy versions of the same 
> thing:
> 
> ra = x + na,
> rb = x + nb
> 
> If you filter these right then you'll get back your signal plus noise:
> 
> x_est = H * ra + (1 - H) * rb.
> 
> So, for example, if H is a 1st-order lowpass filter with DC gain of 1, 
> then 1 - H will be a first-order highpass filter with AC gain of 1.
> 
> But what you have is approximated by
> 
> ra = x + na     (your compass)
> rb = dx/dt + nb (your gyro)


Ok.


> So make a third signal:
> 
> rc = rb / s     (I'm playing fast and loose with notation)


You're integrating the gyro, aren't you?


> then
> 
> rc = x + nb / s


Yep. But as far as I see rc != ra, because the compass output is 
referred to the North but the integrated gyro has no reference.


> Now you know that the noise on ra is severe but good at DC, and you know 
> the noise on rc is infinite at DC.  So choose that 1st-order lowpass 
> filter again, and use
> 
> x_est = H * ra + (1 - H) * rc.
> 
> That should get you close.


Ok, I got it.
My only doubt is related to the reference of the gyro. I can't 
understand how to handle that...


> No, I meant this: http://www.dsprelated.com/showmessage/111654/1.php.  I 
> just left it out...


Very interesting. Thank you.

Marco

On Thu, 28 May 2009 21:16:59 +0200, Marco Trapanese wrote:

> Tim Wescott ha scritto:
> 
>> First, consider a pair of inputs that are just noisy versions of the
>> same thing:
>> 
>> ra = x + na,
>> rb = x + nb
>> 
>> If you filter these right then you'll get back your signal plus noise:
>> 
>> x_est = H * ra + (1 - H) * rb.
>> 
>> So, for example, if H is a 1st-order lowpass filter with DC gain of 1,
>> then 1 - H will be a first-order highpass filter with AC gain of 1.
>> 
>> But what you have is approximated by
>> 
>> ra = x + na     (your compass)
>> rb = dx/dt + nb (your gyro)
> 
> 
> Ok.
> 
> 
>> So make a third signal:
>> 
>> rc = rb / s     (I'm playing fast and loose with notation)
> 
> 
> You're integrating the gyro, aren't you?
> 
> 
>> then
>> 
>> rc = x + nb / s
> 
> 
> Yep. But as far as I see rc != ra, because the compass output is
> referred to the North but the integrated gyro has no reference.
> 
> 
>> Now you know that the noise on ra is severe but good at DC, and you
>> know the noise on rc is infinite at DC.  So choose that 1st-order
>> lowpass filter again, and use
>> 
>> x_est = H * ra + (1 - H) * rc.
>> 
>> That should get you close.
> 
> 
> Ok, I got it.
> My only doubt is related to the reference of the gyro. I can't
> understand how to handle that...

If you choose H to be a unity-gain DC filter, i.e. H = w0/(s + w0), then 
1 - H will have zero gain at DC.  This means that 1-H will have an 's' 
term in it's numerator, which will cancel with the integrator in rc.

So rc = w_g/s + nb/s,

but 1 - H = s / (s + w0)

so (1 - H) rc = (wg + nb) / (s + w0).

This is a low-pass filter.  Your gyro's 'reference' is zero (assuming 
zero gyro drift); the average compass heading comes from the compass.

Note that if you want this to work around the full circle you'll have to 
mind your p's and q's in the filter implementation such that the filter 
correctly handles the transition from -180 to +180 seamlessly.

-- 
http://www.wescottdesign.com

Tim Wescott ha scritto:

> This is a low-pass filter.  Your gyro's 'reference' is zero (assuming 
> zero gyro drift); the average compass heading comes from the compass.


Great!
Now I understand!


> Note that if you want this to work around the full circle you'll have to 
> mind your p's and q's in the filter implementation such that the filter 
> correctly handles the transition from -180 to +180 seamlessly.


Yes, I got it.

Thanks a lot!
Marco

Tim Wescott ha scritto:

> Note that if you want this to work around the full circle you'll have to 
> mind your p's and q's in the filter implementation such that the filter 
> correctly handles the transition from -180 to +180 seamlessly.


Tim,

I'm playing with some filters to see what they do :)
Currently, I'm trying a biquad IIR but I can't easily handle the 360° -> 
0° transition and vice versa.

The straight case is quite simple:

if (HMR3300.hdg > 270 && ROVDATA.hdg < 90)
    ROVDATA.hdg = (int) iir_update(&iir_low_pass, HMR3300.hdg + 360)
else if (HMR3300.hdg < 90 && ROVDATA.hdg > 270)
    ROVDATA.hdg = (int) iir_update(&iir_low_pass, HMR3300.hdg - 360)

if (ROVDATA.hdg >= 360) ROVDATA.hdg -= 360;
if (ROVDATA.hdg < 0   ) ROVDATA.hgd += 360;

where HMR3300.hdg is the input signal and ROVDATA.hdg the current filter 
value.

But what about the samples stored in the delay line?

Conditioning all the samples seems to be a bit harder. And if the filter 
is longer? Argh!

I can't see a simple way. Someone suggested me to convert the angle to 
cartesian coordinates (sin, cos) filter them and then come back with atan.

There is a more convenient way to do this?

Marco

In article <KI2dnRRwTPRANoPXnZ2dnUVZ_qZi4p2d@web-ster.com>,
Tim Wescott  <tim@seemywebsite.com> wrote:
>On Thu, 28 May 2009 09:52:25 +0200, Marco Trapanese wrote:
>> For this first step, I just need to mix up the compass with the gyro
>> output. Next upgrade will consider a more complex sensor and control
>> loop.
>
>With two closely related inputs like this, if you don't need the 
>acquisition capabilities of a Kalman, you can approximate what you want 
>with plain old filtering and intuition. [....]
>If you filter these right then you'll get back your signal plus noise:
>x_est = H * ra + (1 - H) * rb.

Ignoring the intuition ;), this is what's also referred to as a complementary
filter, right?

-- 
   Wim Lewis <wiml@hhhh.org>, Seattle, WA, USA. PGP keyID 27F772C1
  "We learn from history that we do not learn from history." -Hegel

On Fri, 05 Jun 2009 08:18:36 +0200, Marco Trapanese wrote:

> Tim Wescott ha scritto:
> 
>> Note that if you want this to work around the full circle you'll have
>> to mind your p's and q's in the filter implementation such that the
>> filter correctly handles the transition from -180 to +180 seamlessly.
> 
> 
> Tim,
> 
> I'm playing with some filters to see what they do :) Currently, I'm
> trying a biquad IIR but I can't easily handle the 360° -> 0° transition
> and vice versa.
> 
> The straight case is quite simple:
> 
> if (HMR3300.hdg > 270 && ROVDATA.hdg < 90)
>     ROVDATA.hdg = (int) iir_update(&iir_low_pass, HMR3300.hdg + 360)
> else if (HMR3300.hdg < 90 && ROVDATA.hdg > 270)
>     ROVDATA.hdg = (int) iir_update(&iir_low_pass, HMR3300.hdg - 360)
> 
> if (ROVDATA.hdg >= 360) ROVDATA.hdg -= 360; if (ROVDATA.hdg < 0   )
> ROVDATA.hgd += 360;
> 
> where HMR3300.hdg is the input signal and ROVDATA.hdg the current filter
> value.
> 
> But what about the samples stored in the delay line?
> 
> Conditioning all the samples seems to be a bit harder. And if the filter
> is longer? Argh!
> 
> I can't see a simple way. Someone suggested me to convert the angle to
> cartesian coordinates (sin, cos) filter them and then come back with
> atan.
> 
> There is a more convenient way to do this?

Convenient, simple -- and damned subtle and hard to wrap your brain 
around:

Use the fact that integers wrap to your advantage.  It's the only time 
you'll be able to, so revel in it.

If you're programming in C, on a machine that uses 2's compliment 
arithmetic, then INT_MAX will equal 2^(N-1) - 1, where N is the word 
width in bits.  INT_MIN will equal exactly -2^(N-1).  So map your angles 
into integers such that INT_MIN represents -180 degrees.  Then -- as if 
by magic -- you'll find that +179 degrees + 1 degree = -180 degrees.

Now _carefully_ arrange your filter:

y_t = y_t + (x_t - y_t) / factor.

This is a first-order low-pass with a pole at 1 - 1/factor.  What's cool 
about it is that (on all of the machines that I've encountered) the x_t - 
y_t term wraps, so it always finds the shortest path between the state 
and the target.

y_t       x_t        x_t - y_t
0x6000    0xa000     0x4000
0x7000    0xa000     0x3000
0x7c00    0xa000     0x2400
0x8500    0xa000     0x1b00
0x8bc0    0xa000     0x1440

Try it,  You'll like it.  Hopefully this will get you started -- I'm sure 
this can be extended to 2nd-order filters, but you'll have to use a more 
state-space approach rather than looking up bilinear filters in a cook 
book.

-- 
http://www.wescottdesign.com