| tocdepth: | 3 |
|---|
.. sectionauthor:: Joseph S Motyka <jmotyka at aceinna.com>
As the system is nonlinear, the vector \vec{f} cannot be used to propagate the covariance matrix, P. Instead the Process Jacobian, F, (a linearized version of the state-transition vector) is computed at each time step (based on the current system states) to propagate P forward in time:
F_{k-1} = \left.{ {\partial{\vec{f}}} \over {\partial{\vec{x}}} }\right|_{\vec{x}_{k-1},\vec{u}_{k-1}}
This requires taking the derivative of each state-equation with respect to each state. Each row of the Jacobian corresponds to a specific state-equation; each column of the matrix corresponds to a specific system state. Performing this operation results in:
F = I_{16} + {
\begin{bmatrix} {
\begin{array}{ccccc}
{0_{3}} &
{I_{3}} &
{0_{3 \times 4}} &
{0_{3}} &
{0_{3}}
\cr
{0_{3}} &
{0_{3}} &
{\partial{v}\partial{q}} &
{0_{3}} &
{-{^{N}{R}^{B}}}
\cr
{0_{4 \times 3}} &
{0_{4 \times 3}} &
{{{1} \over {2}} \cdot \Omega} &
{-{{1} \over {2}} \cdot \Xi} &
{0_{4 \times 3}}
\cr
{0_{3}} &
{0_{3}} &
{0_{3 \times 4}} &
{0_{3}} &
{0_{3}}
\cr
{0_{3}} &
{0_{3}} &
{0_{3 \times 4}} &
{0_{3}} &
{0_{3}}
\end{array}
} \end{bmatrix}
} \cdot {dt}
The one new term in the matrix, {\partial{v}\partial{q}} is defined as:
{\partial{v}\partial{q}} \equiv {
2 \cdot \overline{Q}_{F} \cdot { \begin{bmatrix} {
\begin{array}{cc}
{0} &
{\begin{pmatrix} { {\vec{a}^{B}} } \end{pmatrix} ^{T}}
\cr
{\vec{a}^{B}} &
{-\begin{bmatrix} { {\vec{a}^{B}} \times } \end{bmatrix}}
\end{array}
} \end{bmatrix}
}
}
where \overline{Q}_{F} is:
\overline{Q}_{F} &= {
\begin{bmatrix} {
\begin{array}{cccc}
{q_{1}} &
{q_{0}} &
{-q_{3}} &
{q_{2}}
\cr
{q_{2}} &
{q_{3}} &
{q_{0}} &
{-q_{1}}
\cr
{q_{3}} &
{-q_{2}} &
{q_{1}} &
{q_{0}}
\end{array}
} \end{bmatrix}
} \\
{\hspace{5mm}} \\
&= {
\begin{bmatrix} {
{\vec{q}_{v}} \hspace{5mm} {q_0 \cdot I_{3} + \begin{bmatrix} { {\vec{q}_{v}} \times } \end{bmatrix}}
} \end{bmatrix}
}
and
\vec{a}^{B} = \vec{a}_{meas}^{B} - \vec{a}_{bias}^{B}