2
$\begingroup$

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.

$\endgroup$
2
$\begingroup$

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.

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

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

$\endgroup$
  • $\begingroup$ 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. $\endgroup$ – Peter K. Nov 9 '13 at 15:33
  • $\begingroup$ 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$. $\endgroup$ – Peter K. Nov 9 '13 at 15:36

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.