Process Jacobian

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:

\[\begin{split}\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} }\end{split}\]

and

\[\vec{a}^{B} = \vec{a}_{meas}^{B} - \vec{a}_{bias}^{B}\]