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.
No comments:
Post a Comment