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.
