Kalman Filter with an Accelerometer

Hi, I wanted to estimate the position of something using data collected
from an accelerometer. Now, I know this is a noisy process, so I wanted to
use some filtering technique to get accurate position and velocity from
just accelerometer readings. I was told that the Kalman Filter would do
just the thing. 

I first decided to design and test a Kalman filter in Matlab and test it by
making acceleration "data" (with added noise by a randn command). I would
compare the Kalman estimated position to the actual position and compare it
to a position estimate found by simply double integrating the noisy
acceleration. However, when I implemented what I thought would be very
simple into Matlab, the results did not seem to suggest any success. My
Kalman estimated position was not very good and was just as bad as a doubly
integrated acceleration. (The double integration is done by just doing a
Riemann sum over my acceleration data twice). 

I'm using the same variables as those given in the explanation on
Wikipedia's site http://en.wikipedia.org/wiki/Kalman_Filter

First, I define a time vector of t = [0:.01:10] to be the time points in
which I sample my acceleration. I then define an acceleration vector with
something like acc = sin(t). I then make a noisy acceleration (which
simulates accelerometer readings) by doing noisyAcc = acc + randn(1,1001).
I want this to be the measurement data that I will be inputting into the
Kalman Filter.

So, I wanted to make sure I understood what my state matrices should be. My
state variable, denoted x, is a 3 by 1 vector of position, velocity, and
acceleration. I will be sampling every dt=.01 seconds (this is just because
I define my time vector in matlab to be 0:.01:10). So, the state should
evolve kinematically by the matrix F = [1 dt dt^2/d ; 0 1 dt; 0 0 1]. So,
x_k = F* x_(k-1). I'm assuming no process noise for now. However, this
doesn't seem to take into account the fact that the object is accelerating
by its own drive. So, I figured there should be some input to change the
acceleration. So, I added a B matrix given by B = [dt^2/2; dt; 1] which
should account for the change in acceleration from the k-1 to the k stage.

So, my evolution would then be

x_k = F*x_(k-1) + B*(acc(k) - acc(k-1))

Where acc(k) is the k-th measured acceleration data point. But, these
acceleration vectors are noisy, so I'm not sure this is the right thing to
put here. I've also tried this without the B* term and my Kalman program
similarly performs poorly.

Next, my measurement matrix would be H = [0; 0; 1], as I'm measuring only
acceleration. From here, I define everything exactly as wikipedia suggests
with its equations to predict, calculate innovation, and update. For
covariance updates, I use P = F*P*F' and I have an R = sigma^2 where
sigma^2 is the standard deviation of the noise signal I input (this is
calculated before the program is run). However, my Kalman estimated
positions do not seem to do very well, and are often worse than if I just
used a simple numerical integration.  Any suggestions or hints on what I
might have done wrong? I'm guessing I've made some mistake with my state
evolution. 

Thanks!

Positron wrote:
>Hi, I wanted to estimate the position of something using data collected
>from an accelerometer. 

Is your orientation constant?  How many axes of accelerometers do you have?
 Have you accounted for gravity and the various pseudoforces?  I'll assume
this is a simulation problem only, and that you've taken trivial solutions
to these, but you'll eventually have to worry about these things in
practice.  (I answer that way because you've said almost nothing about your
plant or sensors, so I assume you haven't chosen one.)


>Now, I know this is a noisy process, so I wanted to
>use some filtering technique to get accurate position and velocity from
>just accelerometer readings. I was told that the Kalman Filter would do
>just the thing. 

Accelerometers alone won't do you much good.  You need something redundant,
such as a decent plant model, other sensors, or, ideally, both.


>I first decided to design and test a Kalman filter in Matlab and test it
by
>making acceleration "data" (with added noise by a randn command). I would
>compare the Kalman estimated position to the actual position and compare
it
>to a position estimate found by simply double integrating the noisy
>acceleration. However, when I implemented what I thought would be very
>simple into Matlab, the results did not seem to suggest any success. My
>Kalman estimated position was not very good and was just as bad as a
doubly
>integrated acceleration. (The double integration is done by just doing a
>Riemann sum over my acceleration data twice). 
>
>I'm using the same variables as those given in the explanation on
>Wikipedia's site http://en.wikipedia.org/wiki/Kalman_Filter
>
>First, I define a time vector of t = [0:.01:10] to be the time points in
>which I sample my acceleration. I then define an acceleration vector with
>something like acc = sin(t). I then make a noisy acceleration (which
>simulates accelerometer readings) by doing noisyAcc = acc +
randn(1,1001).
>I want this to be the measurement data that I will be inputting into the
>Kalman Filter.
>
>So, I wanted to make sure I understood what my state matrices should be.
My
>state variable, denoted x, is a 3 by 1 vector of position, velocity, and
>acceleration. 

I'm not sure if you can avoid making acceleration a state, but you might
try.


>I'm assuming no process noise for now. However, this
>doesn't seem to take into account the fact that the object is
accelerating
>by its own drive. 

When you move under your own control, you may add additional process noise
in proportion to how much you think you're moving (assuming your plant has
a tendency to move less when no control is applied, due to friction and
such), since your movement mechanisms are presumably less certain than
sitting still.


>So, I figured there should be some input to change the
>acceleration. So, I added a B matrix given by B = [dt^2/2; dt; 1] which
>should account for the change in acceleration from the k-1 to the k
stage.
>
>So, my evolution would then be
>
>x_k = F*x_(k-1) + B*(acc(k) - acc(k-1))

You're differentiating an already-noisy signal (randn is ~ white gaussian;
its derivative is even worse).


>Where acc(k) is the k-th measured acceleration data point. But, these
>acceleration vectors are noisy, so I'm not sure this is the right thing
to
>put here. I've also tried this without the B* term and my Kalman program
>similarly performs poorly.

B is usually related to a control input, not a measurement.  You pick "B"
according to how your motors, rockets, etc drive your platform in theory. 
If you're using the same measurements both for "B" and "H", I can't see
that being helpful.  Unless I'm missing something, this is likely to be the
best area to work on next.

Michael

Positron wrote:
>I'm assuming no process noise for now. 

I meant to also point out that if you do that, your covariance will just
decay towards zero.  Process noise and sensor noise are supposed to oppose
each other.  As your filter becomes more confident of its estimate (as it
will if the process noise is too low), it starts to essentially reject
measurements.

Michael

Hi! Thanks for the assistance so far.

Yes, for now I am trying to do just a simple 1-D model with just a single
accelerometer. 

You suggest avoiding using acceleration as a state. I have seen this in
other examples where people have used the Kalman Filter for similar
practices. I could redefine my state vector to just be position and
velocity, and make my evolution matrix F = [1 dt; 0 1]. However, how would
I add in the fact that the body (assume it's a car or something that can
accelerate on its own) is accelerating? Is that just added in the
measurement stage?

Therefore, I would have 

x_k = F*x_(k-1)  But, this doesn't seem to be including the addition of the
acceleration at each state (which is measured by the accelerometer).


If I remove acceleration as a state, how would I model the measurement? In
my measurement state, I had y = z - H*x_k where z is the noisy acceleration
measurement and H is the measurement matrix which was picking the
acceleration out of my state vector. That is, H = [0; 0; 1]. If I remove
acceleration as a state, how would I do this part? Acceleration wouldn't be
in my state vector anymore, so what would H be?

Am I in error thinking that I can use the Kalman Filter to accurately
predict position when the car is accelerating on its own? Does it only work
for situations where the acceleration is random white-Gaussian noise?

Thanks for the help so far!

>Yes, for now I am trying to do just a simple 1-D model with just a single
>accelerometer. 

You still need to know how your plant moves.  Inertia, forces, etc.


>
>You suggest avoiding using acceleration as a state. I have seen this in
>other examples where people have used the Kalman Filter for similar
>practices. I could redefine my state vector to just be position and
>velocity, and make my evolution matrix F = [1 dt; 0 1]. However, how
would
>I add in the fact that the body (assume it's a car or something that can
>accelerate on its own) is accelerating? Is that just added in the
>measurement stage?

That is really the smallest concern.  The flip side of my point was that if
you can't do it, don't worry about it.


>If I remove acceleration as a state, how would I model the measurement? 

If your plant model were more complicated, it *may* make sense to take the
portion of your F and B matrices giving the continuous-time velocity update
(i.e., dv/dt) and use that.  But, at the same time, it may be worse.  Like
I said, it's less important and you shouldn't focus on it yet until the
filter at least appears to work some.  The gain is not differentiating your
accelerometer measurements, but the suggestion is not without its problems.
 Wait on this.


>Am I in error thinking that I can use the Kalman Filter to accurately
>predict position when the car is accelerating on its own? 

On its own?  What does that mean?  Newton's first law:

"An object at rest tends to stay at rest and an object in motion tends to
stay in motion with the same speed and in the same direction unless acted
upon by an unbalanced force."

(copied from a random google search)




>Does it only work
>for situations where the acceleration is random white-Gaussian noise?

The state should not be noise to begin with.  Have a look back at what I
said about "B" in my first post.

Now the noise *ought* to be white gaussian noise, but even then the filter
is known to be somewhat robust in many situations.

Michael

Sorry I've been rather unclear up to this moment! Thanks for your patience
and willingness to help. I'm not an engineering student (just a curious
math major!). When I said "it moves on its own" what I meant is a scenario
like this: Imagine a car with a driver in it that can drive however he or
she wishes. The car has an accelerometer attached to it. The goal is to
determine purely from the accelerometer readings how far the car has
traveled.

So, the car moves simply using Newton's laws and kinematics. That's why I
chose F =[1 dt dt^2/2 ; 0 1 dt; 0 0 1]. But, this ignores the actual
acceleration that the driver seems to be able to choose. That's why I
figured we would take measurements of acceleration. However, when the state
evolves from x_k to x_(k+1) I need to factor in the new acceleration at the
time step k+1.

"You still need to know how your plant moves.  Inertia, forces, etc."

I'm unfamiliar with the word plant. Does that just refer to the body in
question with the accelerometer attached to it? Or does this just mean the
actual model itself? 

With respect to forces, the car has a driver, so accelerations occur at the
whim of the driver. For my simple model, I'm assuming there are no other
outside forces (although, later I will assume a bumpy road scenario so
there will be some random Gaussian process noise). 

Does that make a bit more sense?

Thanks again for the assistance!

Positron wrote:

>Sorry I've been rather unclear up to this moment! Thanks for your
patience
>and willingness to help. I'm not an engineering student (just a curious

Curiosity is good, but you might also do some searching, too.


>math major!). When I said "it moves on its own" what I meant is a
scenario
>like this: Imagine a car with a driver in it that can drive however he or
>she wishes. 

So you're trying to work without measuring what the driver is doing, but,
rather, the effects of what the driver is doing.  Can you add a sensor to
measure the positions of the pedals (or something closely related)?  That's
really the kind of thing that goes in the "B" and "u" terms.  At the risk
of overgeneralizing, the more information you give the filter, the better,
and only giving it one piece of information is not a recipe for success, as
you've found.  I'd still expect some drift even then, but it'll be less,
and you can cut it down if you can assume the car tends to stop on its own
(somewhat level ground).

If you're not able to connect to the car, you might at least postulate GPS,
computer vision, odometry, or some other source of data.  Is this a purely
theoretical exercise, or are you actually going to try to implement it at
some point?  If it's just a theoretical exercise, you may want to try
something a bit different (let me know, and I can give you some ideas to
play around with to build intuition).

Can you afford a book?


>The car has an accelerometer attached to it. The goal is to
>determine purely from the accelerometer readings how far the car has
>traveled.

The way you've posed the problem, it probably just won't work.  The Kalman
Filter is not magical.  You may be able to improve your results slightly,
but nothing better than applying a digital FIR or IIR filter to the data
and doing your double integration.  The nice thing about the Kalman Filter
is its ability to combine information.



>
>So, the car moves simply using Newton's laws and kinematics. That's why I
>chose F =[1 dt dt^2/2 ; 0 1 dt; 0 0 1]. But, this ignores the actual
>acceleration that the driver seems to be able to choose. 

Yes, and the acceleration is usually a function of the system state.  In
this case, it is not.  The fact that your filter has no idea what the
driver chose is technically a violation of an assumption in the derivation
of the filter, but that in and of itself isn't necessarily fatal in
practice.  It usually means that you add a lot of process noise to try to
cover up what the driver is doing.  I wouldn't necessarily expect the
covariance to be a useful measure of how the filter is doing in that case,
though you can certainly build intuition through simulation.

Again, I don't think the Kalman Filter is really gaining you anything the
way you're using it.



>That's why I
>figured we would take measurements of acceleration. However, when the
state
>evolves from x_k to x_(k+1) I need to factor in the new acceleration at
the
>time step k+1.
>
>"You still need to know how your plant moves.  Inertia, forces, etc."
>
>I'm unfamiliar with the word plant. Does that just refer to the body in
>question with the accelerometer attached to it? Or does this just mean
the
>actual model itself? 

In this case, at a minimum, it refers to the car.  You can also make it
refer to the driver and the road, but that choice is not arbitrary, and
depends on what you're doing.  "plant" is one of the first terms you hit in
controls theory, so I assumed that you'd have run into it in your reading
on wikipedia and elsewhere.  You apply a control input (u) to a plant (the
system and/or a model of a system), and you measure outputs through sensors
(y).  There are internal states that are not usually directly observable,
which we model as "x".  The choice of "x" is usually not unique, but
physical states like position, etc. are acceptable in this case.  Sometimes
the sensor data is fed through a transformation (usually with memory), back
into the control input; this is basic feedback control of the plant, such
as in a cruise control (which uses a measurement of speed, not
acceleration).



>
>With respect to forces, the car has a driver, so accelerations occur at
the
>whim of the driver. For my simple model, I'm assuming there are no other
>outside forces (although, later I will assume a bumpy road scenario so
>there will be some random Gaussian process noise). 

A bumpy road presents sensor difficulties, but ultimately has a [nearly?]
zero-mean effect on the states.  A grade in the road may violate
assumptions about the plant:  does your car come to a stop if you coast
long enough?



>Does that make a bit more sense?

Yes, although I think you'll wind up frustrated if you follow this path
without changing the problem statement any.  Either in simulation or in
practice, this is not really going to be pleasant without some changes.  

If you integrate white noise, you get a random walk, and if you integrate
that, you get a random ramp.  Have you seen graphs of realizations of those
processes, or of their statistics?  Try doing your double cumsum() you
mentioned over 10000 of those randn() realizations, and plotting either all
the traces, or, if MATLAB is being retarded about memory, just median, Q1,
Q3 (quantile(), IIRC).  The results are all over the place, no?

And that's assuming no bias on the accelerometers.  By measuring a quantity
more closely related to what you want to estimate (such as position), even
if your measurements are rare, you can try to keep those drifts in check.

Michael

Positron wrote:
> Hi, I wanted to estimate the position of something using data collected
> from an accelerometer. Now, I know this is a noisy process, so I wanted to
> use some filtering technique to get accurate position and velocity from
> just accelerometer readings. I was told that the Kalman Filter would do
> just the thing. 

It won't, at least not for real-world problems with real-world 
accelerometers.  Even if you have a three-axis accelerometer, to do this 
in the real world would require that you also have a three-axis gyro, 
and that your gyros and accelerometers are inertial grade.

Even then, the problem statement lacks any redundant data, so the Kalman 
filter will degenerate into a simple multiple-integration problem -- and 
you'll be completely wrong unless your initial position and orientation 
estimates are dead on.

> I first decided to design and test a Kalman filter in Matlab and test it by
> making acceleration "data" (with added noise by a randn command). I would
> compare the Kalman estimated position to the actual position and compare it
> to a position estimate found by simply double integrating the noisy
> acceleration. However, when I implemented what I thought would be very
> simple into Matlab, the results did not seem to suggest any success. My
> Kalman estimated position was not very good and was just as bad as a doubly
> integrated acceleration. (The double integration is done by just doing a
> Riemann sum over my acceleration data twice). 

That's because if an accelerometer is your only input, then 
doubly-integrating it's output _is_ the optimal solution.

> I'm using the same variables as those given in the explanation on
> Wikipedia's site http://en.wikipedia.org/wiki/Kalman_Filter
> 
> First, I define a time vector of t = [0:.01:10] to be the time points in
> which I sample my acceleration. I then define an acceleration vector with
> something like acc = sin(t). I then make a noisy acceleration (which
> simulates accelerometer readings) by doing noisyAcc = acc + randn(1,1001).
> I want this to be the measurement data that I will be inputting into the
> Kalman Filter.

Fair enough.

> So, I wanted to make sure I understood what my state matrices should be. My
> state variable, denoted x, is a 3 by 1 vector of position, velocity, and
> acceleration. I will be sampling every dt=.01 seconds (this is just because
> I define my time vector in matlab to be 0:.01:10). So, the state should
> evolve kinematically by the matrix F = [1 dt dt^2/d ; 0 1 dt; 0 0 1]. So,
> x_k = F* x_(k-1). I'm assuming no process noise for now. However, this
> doesn't seem to take into account the fact that the object is accelerating
> by its own drive. So, I figured there should be some input to change the
> acceleration. So, I added a B matrix given by B = [dt^2/2; dt; 1] which
> should account for the change in acceleration from the k-1 to the k stage.
> 
> So, my evolution would then be
> 
> x_k = F*x_(k-1) + B*(acc(k) - acc(k-1))
> 
> Where acc(k) is the k-th measured acceleration data point. But, these
> acceleration vectors are noisy, so I'm not sure this is the right thing to
> put here. I've also tried this without the B* term and my Kalman program
> similarly performs poorly.

Unless the accelerometer has significant dynamics (which it won't for 
your case, I think), you don't want to model the accelerometer state. 
Just take the accelerometer output as an input to a system.

> Next, my measurement matrix would be H = [0; 0; 1], as I'm measuring only
> acceleration. From here, I define everything exactly as wikipedia suggests
> with its equations to predict, calculate innovation, and update. For
> covariance updates, I use P = F*P*F' and I have an R = sigma^2 where
> sigma^2 is the standard deviation of the noise signal I input (this is
> calculated before the program is run). However, my Kalman estimated
> positions do not seem to do very well, and are often worse than if I just
> used a simple numerical integration.  Any suggestions or hints on what I
> might have done wrong? I'm guessing I've made some mistake with my state
> evolution. 

With your two state system you will find that H = [0; 0], and it will 
become clear why the exercise of constructing a Kalman filter isn't 
sensible.  Even with your system, if you go through the update matrices, 
I think you'll find that updating the acceleration state doesn't do 
anything significant to the velocity and positions states.

Kalman filters make sense when you have redundant information (like 
acceleration plus a noisy position reading, or acceleration plus an 
occasional position reading), or when you are doing something like 
measuring position and attempting to estimate acceleration from it. 
Usually it is not immediately clear how to use the information you have 
to get an optimal -- or even sensible -- answer.  This is what the 
Kalman filter provides.  What it _doesn't_ do is cough up a magic filter 
that will extract data where there is no good data is to be found.

I very much suspect that your _construction_ of a Kalman filter is fine, 
but your _choice_ of a Kalman filter is misinformed, because you don't 
have the redundant information you need in this case.

Make a problem statement that gives you a reasonably good position 
measurement once every second or so, or gives you a really noisy 
position measurement continuously, and you'll find a lot of benefit to a 
Kalman filter.

-- 
Tim Wescott
Control system and signal processing consulting
www.wescottdesign.com

Michael Plante wrote:
> Positron wrote:
>> I'm assuming no process noise for now. 
> 
> I meant to also point out that if you do that, your covariance will just
> decay towards zero.  Process noise and sensor noise are supposed to oppose
> each other.  As your filter becomes more confident of its estimate (as it
> will if the process noise is too low), it starts to essentially reject
> measurements.

(Note:  This is getting poured onto a USENET newsgroup, where retaining 
context is a very nice thing.)

For the construction that he stated -- three states, one of which is the 
accelerometer state -- the accelerometer noise should be process noise. 
  Then the velocity and position uncertainty will show up correctly.

For the construction that I suggested (and that I think Michael did, 
too), with just velocity and position as states, and acceleration as an 
input, the output gain is always zero, the Kalman filter never really 
updates, and the velocity and position uncertainty will also evolve as 
expected.

-- 
Tim Wescott
Control system and signal processing consulting
www.wescottdesign.com

Positron wrote:

   ...

> With respect to forces, the car has a driver, so accelerations occur at the
> whim of the driver. For my simple model, I'm assuming there are no other
> outside forces (although, later I will assume a bumpy road scenario so
> there will be some random Gaussian process noise). 
> 
> Does that make a bit more sense?

It makes clear what you are leaving out. What will the accelerometer 
outputs be when the car is parked facing up hill? How many axes are you 
measuring? If fewer than three, what assumptions do you make.

Be aware that the accelerometer outputs will have enough offset to 
saturate any integrators you may use in a discouragingly short time.

Jerry
-- 
Discovery consists of seeing what everybody has seen, and thinking what
nobody has thought.    .. Albert Szent-Gyorgi
�����������������������������������������������������������������������