Dual estimation issues using Kalman filters

Hi!

I intent to play around with rotation rate and accelerometer sensors
for a hobby project. The goal is to have a good estimate of where
up/down is in the device coordinate system and to separate the
gravitational part from the motion part of the accelerometer
measurements.

The first approach would simply be to integrate the rotation rates to
keep track of a rotation that maps device coordinates to some other
reference frame in which the gravitational component of the
accelerometer measurements should be roughly constant over time
(without too much drift). Then, in this other reference frame I'd
simply apply a lowpass and a highpass filter to separate the
gravitational component from the motion-based parts of the measure
accelerations. And this seems to work great given calibrated sensors.

Though, it would be really nice to avoid a complicated calibration
procedure. I was thinking of using a Kalman filter for doing both
online: state and calibration parameter estimation. For this I've
implemented a square root version of the Unscented Kalman filter. The
state vector I've come up with is this:

  - two parameters for a normalized 3D vector in device coordinates
    pointing to the sky (so that I know what accelerometer
    measurement to expect w.r.t. gravity)
  - rotation rate vector in device coordinates
  - velocity vector in device coordinates
  - acceleration vector in device coordinates (just linear
    acceleration without the gravitational component)

So, 11 dimensions. The first two values are based on a stereographic
projection of the sky vector relative to some reference sky vector.
The UKF framework makes it easy to use "local parameterizations" of
manifolds such as the unit sphere so that I can easily compress a
normalized 3D vector into two coefficients without any singularity
issues. The rest of the state should be self-explanatory. The process
noise would affect how the rotation rate and acceleration develops
over time.

Then, I added the 12 calibration parameters to the state for the
dual estimation:

  - 3 accelerometer biases         (initialized with 0)
  - 3 accelerometer sensitivities  (initialized with 1)
  - 3 rotation rate biases         (initialized with 0)
  - 3 rotation rate sensitivities  (initialized with 1)

As for measurements: In addition to the rotation rate and
accelerometer measurements I "faked" velocity measurements. The
assumption is that the device's velocity will fluctuate around zero
with some standard deviation. This is basically what allows me to
avoid the "sky vector" from drifting away.

Given this, I simulated the whole thing with the virtual device not
moving/rotating at all and with Gaussian noise of stddev 0.03 m/s^2
for the accelerometer and stddev 0.0018 rad/s for the rotation rate
sensors.

To my surprize after about 5000 iterations at an update rate of 200 Hz,
the rotation rate sensor sensitivity estimates were quite low (around
0.5 instead of the 1.0 that I actually simulated). And if I think
about it: It makes some sense. The device wasn't moving at all so we
can't really determine the exact rotation rate sensitivity from the
data. So, what the Kalman filter ended up doing was setting the
rotation rate sensitivities to a lower value to attenuate the noise, I
guess. The real rotation rate was zero but the measured rotation rates
fluctuated around zero due to the noise. And by lowering the rotation
rate sensitivies the estimated rotation rates were closer to the real
ones, because this only attenuated the noise. But this shouldn't
really happen. If the device doesn't move enough to estimate the
sensors' sensitivities, the filter shouldn't really be updating those
and lowering their estimated uncertainty. It there a way to make dual
estimation more robust in such cases? Should I somehow disable updates
to the calibration parameters and their estimated uncertainties unless
I detect a movement which is suitable for estimating the calibration parameters? If so: How? Is this a known issue in this scenario?
Any suggestions?

Thanks for reading!
SG

On Wed, 31 Aug 2016 07:48:11 -0700, SG wrote:

> Hi!
> 
> I intent to play around with rotation rate and accelerometer sensors for
> a hobby project. The goal is to have a good estimate of where up/down is
> in the device coordinate system and to separate the gravitational part
> from the motion part of the accelerometer measurements.
> 
> The first approach would simply be to integrate the rotation rates to
> keep track of a rotation that maps device coordinates to some other
> reference frame in which the gravitational component of the
> accelerometer measurements should be roughly constant over time (without
> too much drift). Then, in this other reference frame I'd simply apply a
> lowpass and a highpass filter to separate the gravitational component
> from the motion-based parts of the measure accelerations. And this seems
> to work great given calibrated sensors.
> 
> Though, it would be really nice to avoid a complicated calibration
> procedure. I was thinking of using a Kalman filter for doing both
> online: state and calibration parameter estimation. For this I've
> implemented a square root version of the Unscented Kalman filter. The
> state vector I've come up with is this:
> 
>   - two parameters for a normalized 3D vector in device coordinates
>     pointing to the sky (so that I know what accelerometer measurement
>     to expect w.r.t. gravity)
>   - rotation rate vector in device coordinates - velocity vector in
>   device coordinates - acceleration vector in device coordinates (just
>   linear
>     acceleration without the gravitational component)
> 
> So, 11 dimensions. The first two values are based on a stereographic
> projection of the sky vector relative to some reference sky vector.
> The UKF framework makes it easy to use "local parameterizations" of
> manifolds such as the unit sphere so that I can easily compress a
> normalized 3D vector into two coefficients without any singularity
> issues. The rest of the state should be self-explanatory. The process
> noise would affect how the rotation rate and acceleration develops over
> time.
> 
> Then, I added the 12 calibration parameters to the state for the dual
> estimation:
> 
>   - 3 accelerometer biases         (initialized with 0)
>   - 3 accelerometer sensitivities  (initialized with 1)
>   - 3 rotation rate biases         (initialized with 0)
>   - 3 rotation rate sensitivities  (initialized with 1)
> 
> As for measurements: In addition to the rotation rate and accelerometer
> measurements I "faked" velocity measurements. The assumption is that the
> device's velocity will fluctuate around zero with some standard
> deviation. This is basically what allows me to avoid the "sky vector"
> from drifting away.
> 
> Given this, I simulated the whole thing with the virtual device not
> moving/rotating at all and with Gaussian noise of stddev 0.03 m/s^2 for
> the accelerometer and stddev 0.0018 rad/s for the rotation rate sensors.
> 
> To my surprize after about 5000 iterations at an update rate of 200 Hz,
> the rotation rate sensor sensitivity estimates were quite low (around
> 0.5 instead of the 1.0 that I actually simulated). And if I think about
> it: It makes some sense. The device wasn't moving at all so we can't
> really determine the exact rotation rate sensitivity from the data. So,
> what the Kalman filter ended up doing was setting the rotation rate
> sensitivities to a lower value to attenuate the noise, I guess. The real
> rotation rate was zero but the measured rotation rates fluctuated around
> zero due to the noise. And by lowering the rotation rate sensitivies the
> estimated rotation rates were closer to the real ones, because this only
> attenuated the noise. But this shouldn't really happen. If the device
> doesn't move enough to estimate the sensors' sensitivities, the filter
> shouldn't really be updating those and lowering their estimated
> uncertainty. It there a way to make dual estimation more robust in such
> cases? Should I somehow disable updates to the calibration parameters
> and their estimated uncertainties unless I detect a movement which is
> suitable for estimating the calibration parameters? If so: How? Is this
> a known issue in this scenario?
> Any suggestions?
> 
> Thanks for reading!
> SG

First, if you have a Kalman filter that's estimating a bunch of 
integrating processes, and you starve it of data, then it'll drift off to 
weird states.

Second, if you design such a Kalman filter with an inaccurate model and 
then give it less than strong excitation the model inaccuracies will 
overwhelm what the data's telling it and it'll drift off into weird 
states.

Third, any time I need to know more about this sort of thing than what I 
just said, I have to hit the books and simulations.  It ain't easy stuff.

I suspect that you're asking too much of the filter.

-- 

Tim Wescott
Wescott Design Services
http://www.wescottdesign.com

I'm looking for work -- see my website!

Hi Tim,

thanks for responding! BTW: I like your PID controller videos!

Tim Wescott wrote:
> First, if you have a Kalman filter that's estimating a bunch of 
> integrating processes, and you starve it of data, then it'll drift off to 
> weird states.
> [...]
> I suspect that you're asking too much of the filter.

I suspect so, too. In the state estimation setting (11 dimensions)
with fixed calibration parameters, it seems to work well. The
integrated velocity won't drift away due to the fake velocity
measurements I added (measuring zero with an assumed stddev of 0.3m/s)
and the "sky vector" won't drift away due to the gravitational
component in the accelerometer measurements that has to be there since
the device will hardly move. But it seems I shouldn't try to estimate
the sensors' sensitivities this way. I'll probably limit the
calibration parameters I want to estimate to the rotation rate sensor
biases only and see what happens...

> Second, if you design such a Kalman filter with an inaccurate model and 
> then give it less than strong excitation the model inaccuracies will 
> overwhelm what the data's telling it and it'll drift off into weird 
> states.

It was a simulation where the filter's model matches the simulation.

And apart from that I think it's a pretty good model. I even went to
the trouble of using a 2nd order approximation of the integral of
linearly changing rotation rates over the time step instead of simply
assuming a constant mean rotation rate. But I don't think it matters
much. It just makes a difference if there is a "strong rotation
acceleration" component perpendicular to a high rotation rate.

Cheers!
SG

On Wed, 31 Aug 2016 07:48:11 -0700, SG wrote:

> Hi!
> 
> I intent to play around with rotation rate and accelerometer sensors for
> a hobby project. The goal is to have a good estimate of where up/down is
> in the device coordinate system and to separate the gravitational part
> from the motion part of the accelerometer measurements.
> 
> The first approach would simply be to integrate the rotation rates to
> keep track of a rotation that maps device coordinates to some other
> reference frame in which the gravitational component of the
> accelerometer measurements should be roughly constant over time (without
> too much drift). Then, in this other reference frame I'd simply apply a
> lowpass and a highpass filter to separate the gravitational component
> from the motion-based parts of the measure accelerations. And this seems
> to work great given calibrated sensors.
> 
> Though, it would be really nice to avoid a complicated calibration
> procedure. I was thinking of using a Kalman filter for doing both
> online: state and calibration parameter estimation. For this I've
> implemented a square root version of the Unscented Kalman filter. The
> state vector I've come up with is this:
> 
>   - two parameters for a normalized 3D vector in device coordinates
>     pointing to the sky (so that I know what accelerometer measurement
>     to expect w.r.t. gravity)
>   - rotation rate vector in device coordinates - velocity vector in
>   device coordinates - acceleration vector in device coordinates (just
>   linear
>     acceleration without the gravitational component)
> 
> So, 11 dimensions. The first two values are based on a stereographic
> projection of the sky vector relative to some reference sky vector.
> The UKF framework makes it easy to use "local parameterizations" of
> manifolds such as the unit sphere so that I can easily compress a
> normalized 3D vector into two coefficients without any singularity
> issues. The rest of the state should be self-explanatory. The process
> noise would affect how the rotation rate and acceleration develops over
> time.
> 
> Then, I added the 12 calibration parameters to the state for the dual
> estimation:
> 
>   - 3 accelerometer biases         (initialized with 0)
>   - 3 accelerometer sensitivities  (initialized with 1)
>   - 3 rotation rate biases         (initialized with 0)
>   - 3 rotation rate sensitivities  (initialized with 1)
> 
> As for measurements: In addition to the rotation rate and accelerometer
> measurements I "faked" velocity measurements. The assumption is that the
> device's velocity will fluctuate around zero with some standard
> deviation. This is basically what allows me to avoid the "sky vector"
> from drifting away.
> 
> Given this, I simulated the whole thing with the virtual device not
> moving/rotating at all and with Gaussian noise of stddev 0.03 m/s^2 for
> the accelerometer and stddev 0.0018 rad/s for the rotation rate sensors.
> 
> To my surprize after about 5000 iterations at an update rate of 200 Hz,
> the rotation rate sensor sensitivity estimates were quite low (around
> 0.5 instead of the 1.0 that I actually simulated). And if I think about
> it: It makes some sense. The device wasn't moving at all so we can't
> really determine the exact rotation rate sensitivity from the data. So,
> what the Kalman filter ended up doing was setting the rotation rate
> sensitivities to a lower value to attenuate the noise, I guess. The real
> rotation rate was zero but the measured rotation rates fluctuated around
> zero due to the noise. And by lowering the rotation rate sensitivies the
> estimated rotation rates were closer to the real ones, because this only
> attenuated the noise. But this shouldn't really happen. If the device
> doesn't move enough to estimate the sensors' sensitivities, the filter
> shouldn't really be updating those and lowering their estimated
> uncertainty. It there a way to make dual estimation more robust in such
> cases? Should I somehow disable updates to the calibration parameters
> and their estimated uncertainties unless I detect a movement which is
> suitable for estimating the calibration parameters? If so: How? Is this
> a known issue in this scenario?
> Any suggestions?
> 
> Thanks for reading!
> SG

I had a chance to think about this over lunch, and I think I have some 
more detail for you to chew on.

My view of how a Kalman filter works comes partially from Wikipedia, 
partially from Dan Simon's "Optimal State Estimation", and partially from 
lots of hard thinking.

Consider an n-state Kalman filter that works with a system that has just 
one measurable output.  The correlation matrix of the Kalman filter 
(usually P in the literature) defines a ball in n-space.  Initially that 
ball is (usually) Really Big in all directions.  Any one measurement 
gives information about a single direction in the state space.  
Initially, this measurement lets you squash that n-dimensional ball into 
a disk that's thin in one direction but thick in the other n-1.

Now, you iterate the Kalman.  In general (thanks to the properties of the 
state transition matrix, usually F or A in the literature), the ball 
defined by P rotates, but the direction of the measurement does not.  So 
you can increase the certainty of P along the same direction as before -- 
but P has rotated, so _some_ of the certainty that was gained in the last 
iteration is preserved, at the same time that a lot of certainty is added 
in that one magic direction.

Now, keep doing that -- the ball defined by P keeps rotating, it keeps 
getting squished in just one direction, and it keeps retaining 
information as it rotates.  Eventually, it's been squished every which 
way, and it's small.

That's for a "good" Kalman filter.  But _your_ Kalman filter is based on 
a platform that's not rotating.  Without rotating your platform, the 
uncertainty in the gyro gain and the uncertainty in the gyro bias will 
always and ever maintain the same angle from the direction that the 
uncertainty ball gets corrected, so you will never, ever, be able to 
correct both.

You may find that if you re-do your simulation for a platform that's 
always being tilted with respect to level, even if it's only by a bit, 
that your state estimates suddenly get a lot better.  If so, you'll also 
find that if you're going to do that, you're going to have a requirement 
on the device that it always tilts a little bit, or you're just going to 
have to be happy with states that are never quite right when the platform 
doesn't tilt.

-- 

Tim Wescott
Wescott Design Services
http://www.wescottdesign.com

I'm looking for work -- see my website!