Kalman Filter with an Accelerometer
in [DSP]
|
Prev: Today night the physicists-criminals from CERN accelerated protonsto the record energy 3.5 TeV per beam.
Next: CERN. If I had an access to the start-button of a rocket with nuclear war-head I would push it and I would direct it on Geneva.
From: Positron on 20 Mar 2010 20:19 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!
From: Michael Plante on 20 Mar 2010 23:44 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
From: Michael Plante on 20 Mar 2010 23:51 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
From: Positron on 21 Mar 2010 00:42 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!
From: Michael Plante on 21 Mar 2010 01:20
>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 |