Kalman Filter with Accelerometer with DC Offset

Question

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$?

Thanks for your help.

Answer 1

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.

Answer 2

@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.

Appreciate your help.