# Kalman Filter with Accelerometer with DC Offset

Goal: For a particle moving uniaxially, to estimate position ($d$) and velocity ($v$) from noisy acceleration ($a$) and very noisy position (GPS) measurements using a Kalman filter.

Catch: The accelerometer has a DC offset, i.e. its zero is not zero but some number $\Omega$ such that the "true" acceleration $a^{true}$ is obtained from the measured acceleration coming out of the sensor $a^{obs}$ as: $$a^{true}=a^{obs}-\Omega$$ My system states and transitions, note I've added the accelerometer bias or DC offset as a state are:

$$\hat{x}_{k+1}=\left[\begin{array}{cccc} d_{k+1} \\ v_{k+1} \\ a_{k+1}^{true} \\ \Omega \end{array}\right]=\left[\begin{array}{cccc} 1 & dt & dt^2/2 & 0 \\ 0 & 1 & dt & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right]\cdot\left[\begin{array}{cccc} d_k \\ v_k \\ a_k^{true} \\ \Omega \end{array}\right]$$

And then the measurements are (notice I've used $a^{obs}$ instead of $a^{true}$): $$z_k=\left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & -1 \\ 0 & 0 & 0 & 0 \end{array}\right]\cdot\left[\begin{array}{cccc} d_k \\ 0 \\ a_k^{obs} \\ 0 \end{array}\right]$$

Question 1: Does this look right and/or feasible and,

Question 2: How do I determine the process noise and measurement noise matrices $Q$ and $R$?

Q1: Without actually implementing it and seeing that it works, your formulation looks pretty good... except that I do not understand your final equation: $$z_k=\left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & -1 \\ 0 & 0 & 0 & 0 \end{array}\right]\cdot\left[\begin{array}{cccc} d_k \\ 0 \\ a_k^{obs} \\ 0 \end{array}\right]$$ That doesn't make sense because a) the output matrix is too big and b) $a_k^{obs}$ does not form part of the state, $a_k^{true}$ does. It should look more like: $$z_k=\left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & +1 \\ \end{array}\right]\cdot\left[\begin{array}{cccc} d_k \\ v_k \\ a_k^{true} \\ \Omega \end{array}\right] = \left[\begin{array}{cccc} d_k\\ a_k^{true} + \Omega \end{array}\right]$$ Notice the change in sign to get the expression for the acceleration measurement to come out correctly.

Q2: What noise is driving your system? Is it just the acceleration?

Your state update equation should look more like:

$$\hat{x}_{k+1}=\left[\begin{array}{cccc} 1 & dt & dt^2/2 & 0 \\ 0 & 1 & dt & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right]\cdot\left[\begin{array}{cccc} d_k \\ v_k \\ a_k^{true} \\ \Omega \end{array}\right] + Gw_k$$ where $G$ determines which part of the state is driven by the noise process $w_k$. The dimension of $w_k$ (is it a scalar or a vector?) will determine whether, if more than one part of the state is being driven, whether those drivers are different for each part.

If the driver is just acceleration, then choose: $$G = \left[\begin{array}{cccc} 0 \\ 0 \\ 1 \\ 0 \end{array}\right]$$ assuming that the offset in acceleration is not changing.

Then the question of what the covariance of $Q$ is boils down to: how quickly does your acceleration change? That will really depend on your precise system. In this case, $w_k$ is just a scalar.

As to the covariance of $R$, that will depend on how different your measurements are form reality. I'd need more information to make a stab at that.

• I replied below because I ran out of space here. – brotmandel Nov 8 '13 at 18:43

@Peter K

I'm replying here because I ran out of space in the comments:

Yeah that change you propose to the measurement equations makes perfect sense to me, thanks for that.

Now, as for the addition of $Gw_k$ to the state update I have some comments.

I was under the impression this is added if you assume there are unmodeled dynamics, i.e. your physics are imperfect. In my case I think the only unmodeled dynamic is that the offset will actually change very slowly, so perhaps I could add that in there with a very small variance to account for slow changes?

I don't see the need to add it for any of the other states. Let me tell you about my problem: I have an accelerometer and a GPS (permanent sites) and I'm looking at ground motion (shaking) during earthquakes, so I know the noise characteristics of each sensor very well from analyzing quiescent periods, i.e. when there is no shaking. So I could just add that into $R$ no?

The Kalman filter is cool because each sensor alone only records in a limited frequency band and the combination of the two produces very broadband recordings of shaking.

• The $Gw_k$ is the driving force for your state, other than the state update matrix. Does anything in your state change, other than through the state update? If so, then it needs a driving (process) noise. – Peter K. Nov 9 '13 at 15:33
• The noise characteristics of each sensor tell you about the measurement noise, not the process noise. Really, your output equation should be: $$z_k=\left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & +1 \\ \end{array}\right]\cdot\left[\begin{array}{cccc} d_k \\ v_k \\ a_k^{true} \\ \Omega \end{array}\right] + J v_k$$ and your sensor characterization will tell you about the covariance of $v_k$. – Peter K. Nov 9 '13 at 15:36