| tocdepth: | 3 |
|---|
.. sectionauthor:: Joseph S Motyka <jmotyka at aceinna.com>
The velocity propagation equation is based on the following first-order model:
\vec{v}_{k} = \vec{v}_{k-1} + \dot{\vec{v}}_{k-1} \cdot dt
\dot{\vec{v}}_{k-1} is an estimate of system acceleration (linear-acceleration corrected for gravity) and is formed from the accelerometer signal with estimated accelerometer-bias and gravity removed.
\vec{a}_{motion,k-1} = \vec{a}_{meas,k-1} - \vec{a}_{bias,k-1} - \vec{a}_{grav}
Substituting this expression (along with the noise term) into the velocity propagation equation, and explicitly stating the frames in which the readings are made, leads to:
\vec{v}_{k}^N = \vec{v}_{k-1}^N + \begin{pmatrix} {
\vec{a}_{motion,k-1}^N - {^{N}{R}_{k-1}^{B}} \cdot \vec{a}_{noise}^{B}
} \end{pmatrix} \cdot {dt}
where
\vec{a}_{motion,k-1}^N = {^{N}{R}_{k-1}^{B}} \cdot \begin{pmatrix} {
\vec{a}_{meas,k-1}^B - \hat{a}_{bias,k-1}^B
} \end{pmatrix} - \vec{a}_{grav}^{N}
The velocity process-noise vector resulting from accelerometer noise is:
\vec{w}_{v,k-1}^{N} = -{^{N}{R}_{k-1}^{B}} \cdot \vec{a}_{noise}^{B} \cdot {dt}
leading to the final formulation for the velocity state-transition model:
\vec{v}_{k}^N = \vec{v}_{k-1}^N + \vec{a}_{motion,k-1}^N \cdot dt + \vec{w}_{v,k-1}^{N}
The velocity process noise vector is used to compute the elements of the process covariance matrix (Q) related to the velocity estimate, as follows:
\Sigma_{v} = {\vec{w}_{v,k-1}} \cdot {\vec{w}_{v,k-1}}^{T}
By making the assumption that all axes have the same noise characteristics ({\sigma_{a}}^{2}) and manipulating the expression, the result can be simplified to the following:
\Sigma_{v} = { \begin{pmatrix} {
\sigma_{a} \cdot dt
} \end{pmatrix} }^{2} \cdot I_3