I am trying to implement an extended Kalman filter which takes a vector as a sensor measurement. To model this I need to rotate the vector to the satellite reference frame using quaternion rotation. For the filter I need to find the Jacobian of my measurement function.
I would like to calculate the Jacobian of some function h which performs a passive quaternion rotation, where q is my quaternion and p is some vector:
$h(q) = q p q^{-1}$
I'd like to find:
$H = \frac{\partial h(q)}{\partial q}$
I'm using unit quaternions in the form $q = [w, \vec{v}]$
Many thanks.
Best Answer
You can write the quaternion multiplication $\circ$ as a matrix-vector product: $$ q\circ p = Q(q)\cdot p,$$ where $$ Q(q) = \begin{bmatrix} q_0 & -q_1 & -q_2 & -q_3 \\ q_1 & q_0 & -q_3 & q_2 \\ q_2 & q_3 & q_0 & -q_1 \\ q_3 & -q_2 & q_1 & q_0 \end{bmatrix} \quad\text{for}\quad q = \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix}. $$ Likewise, there is a matrix that fulfills $q\circ p = \hat Q(p)\cdot q$ given by $$ \hat Q(p) = \begin{bmatrix} p_0 & -p_1 & -p_2 & -p_3 \\ p_1 & p_0 & p_3 & -p_2 \\ p_2 & -p_3 & p_0 & p_1 \\ p_3 & p_2 & -p_1 & p_0 \end{bmatrix} \quad\text{for}\quad p = \begin{bmatrix} p_0 \\ p_1 \\ p_2 \\ p_3 \end{bmatrix}. $$ It is easy to see that for a unit quaternion $q$ it is $$q^{-1} = \underbrace{\operatorname{diag}(1,-1,-1,-1)}_{=:I^*}\cdot q.$$
Now we use the product rule and the above and get \begin{align*} \frac{\partial h(q)}{\partial q} &= \frac{\partial}{\partial q}(q^*\circ p\circ q^{-1})\big|_{q^*=q} + \frac{\partial}{\partial q}(q\circ p\circ (q^*)^{-1})\big|_{q^*=q} \\ &= Q(q^*\circ p)\cdot I^*\big|_{q^*=q} + \hat Q(p\circ (q^*)^{-1})\big|_{q^*=q} \\ &= Q(q\circ p)\cdot I^* + \hat Q(p\circ q^{-1}) \end{align*}
Notice that, if $h$ is a rotation by a unit quaternion the first row of this matrix will vanish, because the scalar (or real) part of $h(q)$ vanishes, as well.