# 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.

• Its rather odd, if $q$ is in the state, why is that $\dot q$ is not? In this case, it is simply impossible to have a derivative of $\dot q$ w.r.t. $q$-they are independent in 2nd order dynamics! Remember that the state space is always a tangent bundle. And also $v$ is in inertial frame, but $\dot q$ is in body frame. Why? – Troy Woo Feb 9 '15 at 11:20
• @TroyWoo To put it simply, $\dot{q}$ is not in the state because it is not of direct interest. I'm sorry I don't know RE the tangent bundle. All the vectors in the state are in the inertial (global) frame except for the two bias terms $b_a$ and $b_\omega$; the body->inertial transformations are made where appropriate. – Gouda Feb 9 '15 at 11:24
• To put it simply, $d\dot q/dq=0$. Why does the rate of rotation has anything to do with rotation itself? On a second thought, if your definition of state can be different from a mathematical one simply because you want it that way, then this question should be an off-topic and should be addressed to robotics rather than here. – Troy Woo Feb 9 '15 at 11:29
• @TroyWoo (Note I corrected the $\dot{q}$ equation) The rate of change of the orientation is related to the orientation because of the equation given for $\dot{x}$, that comes from the (corrected) observation of the body-frame angular velocity $\hat{\omega}$. As far as I am aware the state I am giving is simply a reduced full-state. Thanks for your time. – Gouda Feb 9 '15 at 12:09
• You didn't get it...it doesn't matter if the expression for $\dot q$ contains $q$ or not...physically the derivative $d\dot q/dq$ (rigoroulsy speaking we should assign $q$ $\dot q$ 3-d coordinates) should always be zero. – Troy Woo Feb 9 '15 at 12:39