# Jacobian Matrix of 6DOF Body (with IMU)

I am trying to derive the analytical Jacobian for a system that is essentially the equations of motion of a body (6 degrees of freedom) with gyro and accelerometer measurements. This is part of an Extended Kalman Filter.

The system state is given by: $\mathbf{x} = \left( \begin{array}{c} \mathbf{q}\\ \mathbf{b_\omega}\\ \mathbf{v}\\ \mathbf{b_a}\\ \mathbf{p}\\ \end{array} \right)$

where $q$ is the quaternion orientation of the body expressed in the global frame, $b_\omega$ and $b_a$ are the biases in the gyro and accelerometer respectively (expressed in the body frame) and $v$ and $p$ are the velocity and position of the body expressed in the global frame. All vectors are [3x1] except $q$ which is [4x1] in $[w,x,y,z]^\top$ format, and $R$ (below) which is [3x3].

The equations of motion $\frac{dx}{dt}=\dot{x}$ (t is time) are: $$\dot{\mathbf{q}} = \frac{1}{2}\mathbf{q} \otimes \left( \begin{array}{c} 0\\ \hat{\omega}\\ \end{array} \right) \\ \dot{\mathbf{b_\omega}} = 0 \\ \dot{\mathbf{v}} = R^\top (\hat{\mathbf{a}} + [\hat{\mathbf{\omega}}\times]R \mathbf{v})+ g \\ \dot{\mathbf{b_a}} = 0 \\ \dot{\mathbf{p}} = \mathbf{v}$$ Second-order terms are ignored. $\hat{a} = a - b_a$ and $\hat{\omega} = \omega - b_\omega$ are the corrected accelerometer and gyro biases ($a$ and $\omega$ are known) and are expressed in the body frame. $R$ is the rotation matrix (DCM) formed from $q$ and $g$ is the gravity vector $[0,0,9.81]^\top$. These equations have been validated against an aerospace engineering software library.

I need the jacobian $F = \frac{d\dot{x}}{dx}$ but I cannot find this jacobian in any texts (I do find the error-state jacobian eg this paper). I am struggling with doing this myself because I don't know how to handle the quaternion norm constraints. I also am concerned about the validity of a solution given through numerical differentiation.

Any help or explanation would be greatly appreciated. This is going towards an open-source robot localisation project.

• Is this system going to be observable? There are a lot of biases in that system state. Also, this whole mess can be avoided by moving to an unscented kalman filter. – holmeski Feb 14 '15 at 18:50
• Hi @holmeski, the system is observable - there are numerous papers (including observability analyses) that use this system however they do not explicitly state what the analytical jacobian is. I currently have this working using a forward-differencing numerical jacobian, but the analytic form would be nice. – Gouda Feb 15 '15 at 0:03
• Have you thought about using a Ukf? – holmeski Feb 15 '15 at 3:21