2
$\begingroup$

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.

$\endgroup$
2

3 Answers 3

3
$\begingroup$

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.

$\endgroup$
3
  • $\begingroup$ I see! Thank you @Wauzl $\endgroup$
    – Rufus
    Mar 29, 2018 at 11:32
  • 1
    $\begingroup$ Cool, I didn't know about "accepting" answers, thanks again. Unfortunately my rep is a little low for upvoting and downvoting, lucky for you tho! ;) $\endgroup$
    – Rufus
    Apr 3, 2018 at 14:58
  • $\begingroup$ Nice answer. A detailed derivation is given in: Lee, Byung-Uk (1991), "Differentiation with Quaternions, Appendix B" $\endgroup$ Nov 24, 2021 at 13:51
1
$\begingroup$

$ \require{enclose} \def\LR#1{\left(#1\right)} \def\rr#1{{#1}^{\small R}} \def\rrLR#1{\LR{{#1}}^{\small R}} \def\bbR#1{{\mathbb R}^{#1}} \def\p{\partial} \def\grad#1#2{\frac{\p #1}{\p #2}} \def\m#1{\left[\begin{array}{r}#1\end{array}\right]} \def\mc#1{\left[\begin{array}{c|c}#1\end{array}\right]} \def\mq#1{\left[\begin{array}{r|rrr}#1\end{array}\right]} \def\BEvec#1{\begin{Bmatrix}#1\end{Bmatrix}} \def\qiq{\quad\implies\quad} \def\qif{\quad\iff\quad} \def\c#1{\color{red}{#1}} \def\CLR#1{\c{\LR{#1}}} $An arbitrary quaternion can be represented as a real vector $(a)$ or matrix $(A)$ $$\eqalign{ a &= \BEvec{a_0\\a_1\\a_2\\a_3} \qif A = \mq{ a_0 & -a_1 & -a_2 & -a_3 \\ \hline a_1 & a_0 & -a_3 & a_2 \\ a_2 & a_3 & a_0 & -a_1 \\ a_3 & -a_2 & a_1 & a_0 } \\ }$$ This allows the quaternion-quaternion product to be replaced by a matrix-vector product.
The order of the product can be reversed using a secondary matrix representation which transposes the off-diagonal elements of the bottom left block, e.g. $$\eqalign{ \rr{A} = \mq{ a_0 & -a_1 & -a_2 & -a_3 \\ \hline a_1 & a_0 & a_3 & -a_2 \\ a_2 & -a_3 & a_0 & a_1 \\ a_3 & a_2 & -a_1 & a_0 } \\ }$$

$$\eqalign{ f &= a\circ b \qiq f = Ab \\ g &= b\circ a \qiq g = \rr{A}b \\ }$$ First, rewrite the rotation formula to remove the inverse term $$\eqalign{ h &= q\circ p\circ q^{-1} \qiq h\circ q &= q\circ p \\ }$$ then calculate the differential, convert to matrix form, and isolate the desired gradient $$\eqalign{ dh\circ q &+\; h\circ dq = dq\circ p \;+\; \enclose{horizontalstrike}{q\circ dp} \\ \rr{Q}dh &+\; H\,dq = \rr{P}dq \\ \rr{Q}dh &= \LR{\rr{P}-H}dq \\ \grad hq &= \LR{\rr{Q}}^{-1}\LR{\rr{P}-H} \\ &= \rrLR{Q^{-1}} \LR{\rr{P}-H} \\ }$$

Update

The above gradient does not respect the constraint $\|q\|={\tt1}$.

In order to satisfy the constraint, one must include a $\c{\rm nullspace\;projector}$ $$\eqalign{ \grad hq &= \LR{\rr{Q}}^{-1}\LR{\rr{P}-H}\CLR{I-qq^T} \\ }$$

$\endgroup$
0
$\begingroup$

Just take the partial derivatives of $h(q)$ with respect to $w$, $i$, $j$ and $k$. That gives you a 3x4 matrix which is the Jacobian.

$\endgroup$
1
  • $\begingroup$ Haha... "just" take the derivative! Did you forget that w is a function of I, just, and k for a normalized quaternion? $\endgroup$
    – Michael
    May 27, 2023 at 19:56

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .