1

I found a way to rotate a 3D vector using a given unit quaternion. Thanks to this answer.

Now, let's say I want to rotate a gravity vector: $\overrightarrow{g} = \begin{bmatrix} g_x\\ g_y\\ g_z\\ \end{bmatrix} = \begin{bmatrix} 0\\ 0\\ -9.81\\ \end{bmatrix}$, using a unit quaternion $ \mathbf q = [q_w, q_x, q_y, q_z] $ i.e. $ \lvert\lvert q\lvert\lvert^2 = q_w^2 + q_x^2 + q_y^2 + q_z^2 = 1 $

So, if I represent the rotated vector as $\overrightarrow{g'} = \begin{bmatrix} g'_x\\ g'_y\\ g'_z\\ \end{bmatrix}$

As $ g_x = 0 $ and $ g_y = 0 $, the simplified equations for the rotated vector looks like:

$$ g'_x = 2 g_z (q_x q_z + q_w q_y) $$

$$ g'_y = 2 g_z (q_y q_z - q_w q_x) $$

$$ g'_z = g_z (1 - 2 q_x^2 - 2 q_y^2) $$

where $ g_z = -9.81 $

Now, I need to calculate the Jacobian (as a part of my Extended Kalman Filter) of these equations w.r.t. $ q_w, q_x, q_y $ and $ q_z $:

$$ \frac{\partial g'_x}{\partial q_w} = 2 g_z q_y $$

$$ \frac{\partial g'_x}{\partial q_x} = 2 g_z q_z $$

$$ \frac{\partial g'_x}{\partial q_y} = 2 g_z q_w $$

$$ \frac{\partial g'_x}{\partial q_z} = 2 g_z q_x $$

$$ \frac{\partial g'_y}{\partial q_w} = - 2 g_z q_x $$

$$ \frac{\partial g'_y}{\partial q_x} = - 2 g_z q_w $$

$$ \frac{\partial g'_y}{\partial q_y} = 2 g_z q_z $$

$$ \frac{\partial g'_y}{\partial q_z} = 2 g_z q_y $$

But I'm confused about partial derivatis for $ g'_z $. Because in the equation of $ g'_z $,

  • if I keep the $ 1 $ in $ g'_z $ as it is then

$$ \frac{\partial g'_z}{\partial q_w} = \frac{\partial (g_z(1 - 2 q_x^2 - 2 q_y^2))}{\partial q_w} = 0 $$

  • if I replace the $ 1 $ in $ g'_z $ with $ q_w^2 + q_x^2 + q_y^2 + q_z^2 $ then

$$ \frac{\partial g'_z}{\partial q_w} = \frac{\partial (g_z(1 - 2 q_x^2 - 2 q_y^2))}{\partial q_w} = \frac{\partial (g_z(q_w^2 - q_x^2 - q_y^2 + q_z^2))}{\partial q_w} = 2 g_z q_w $$

  • if I keep the $ 1 $ in $ g'_z $ as it is but do something like

$$ \begin{align} \frac{\partial g'_z}{\partial q_w} &= \frac{\partial (g_z(1 - 2 q_x^2 - 2 q_y^2))}{\partial q_w} \\ &= \frac{\partial g_z}{\partial q_w} - 2 g_z \frac{\partial q_x^2}{\partial q_w} - 2 g_z \frac{\partial q_y^2}{\partial q_w}\\ &= \frac{\partial g_z}{\partial q_w} - 2 g_z \frac{\partial (1 - q_w^2 - q_y^2 - q_z^2)}{\partial q_w} - 2 g_z \frac{\partial (1 - q_w^2 - q_x^2 - q_z^2)}{\partial q_w} \\ &= 0 - 2 g_z (-2 q_w) - 2 g_z (-2 q_w) \\ &= 8 g_z q_w \\ \end{align} $$

So, which one is the correct way to take the partial derivative here?

I have the same confusion for $ \frac{\partial g'_z}{\partial q_x} $, $ \frac{\partial g'_z}{\partial q_y} $, and $ \frac{\partial g'_z}{\partial q_z} $.

If I keep the $ 1 $ in $ g'_z $ as it is then

$$ \frac{\partial g'_z}{\partial q_x} = -4 g_z q_x $$

$$ \frac{\partial g'_z}{\partial q_y} = -4 g_z q_y $$

$$ \frac{\partial g'_z}{\partial q_z} = 0 $$

But if I replace the $ 1 $ in $ g'_z $ then it kind of feels like a rabbit hole!

Milan
  • 117

2 Answers2

4

$ \newcommand\dd{\mathbf d} $It's safest to assume you can't take the derivative with respect to a constrained variable. You're better off using an unnormalized quaternion. Then the rotation is $g \mapsto g' = qgq^{-1}$ and the differential is $$ \dd g' = \dd(qgq^{-1}) = \dd q\,gq^{-1} + qg\,\dd(q^{-1}) = \dd q\,gq^{-1} - qgq^{-1}\dd q\,q^{-1} = (\dd q\,g - g'\dd q)q^{-1}. $$ where we derive the second-to-last equality by $$ qq^{-1} = 1 \implies \dd q\,q^{-1} + q\,\dd(q^{-1}) = 0 \implies \dd(q^{-1}) = -q^{-1}\,\dd q\,q^{-1}. $$ The Jacobian is the matrix of the differential: plugging in $1, i, j, k$ for $\dd q$, the resulting coefficients of $1, i, j, k$ are the columns of the Jacobian.

Handling a constrained variable essentially means computing a covariant derivative on some manifold; in this case I would do $(q,g) \in S^3\times\mathbb R^3$ where $S^3\subseteq\mathbb H$ is the unit 3-sphere. I would then represent the quaternion rotation as a vector field $(q,g) \mapsto 0\oplus qgq^*$.

This doesn't really buy you anything though, as what you'll end up with is the same differential $\dd g'$ above except $\dd q$ needs to be restricted to the tangent space of $q \in S^3$. In fact, you can notice that $\dd q = \alpha q$ for some scalar $\alpha$ when it is orthogonal to the tangent space and in this case $\dd g' = 0$ as it should be.

Nicholas Todoroff
  • 10,107
  • Isn't the rotation $g \mapsto g'$ defined as $qgq$ rather than $qgq^{-1}$? $q$ is the conjugate of quaternion $q$. – Milan Jul 19 '24 at 13:48
  • 3
    @Milan If $q$ is normalized then $q^* = q^{-1}$. In general you can rotate with an unnormalized quaternion using $qgq^{-1}$, which is what I'm proposing to do since then you are differentiating an unconstrained $q$ and won't run into any issues like you were. – Nicholas Todoroff Jul 19 '24 at 14:16
  • 1
    @Milan I added a bit of text about what happens when you constrain $q$. – Nicholas Todoroff Jul 19 '24 at 16:09
  • Oh shoot, how did I forget that $q^{-1}= \frac{q^*}{ \vert q\vert^2}$!! Thanks a lot for your answer. However, as I'm not that well-versed in quaternion, I still don't completely understand how will $(\dd q,g - g'\dd q)q^{-1}$ result in a 3x4 Jacobian matrix? Because even if I plug in $1, i, j, k$, it will still result in a quaternion, right?

    Do you have any recommendations for additional reading? Because I'm still confused!

     – Milan Jul 19 '24 at 20:27
  • In $(\dd q,g - g'\d d q)q^{-1}$, I can easily put the values for $g = [g_x, q_y, q_z]^T$, $g' = [g'_x, q'_y, q'_z]^T$, and for $q^{-1}$. However, what exactly should I do with $\ddq$? Just put $1, i, j, k$ and multiply them with $g$ (first half) or $g'$ (second half)? Even in that case, it will still result in a 4-dimensional vector. It will not result in the 3x4 Jacobian that I need. – Milan Jul 19 '24 at 21:53
  • 1
    @Milan Yes. The differential is the "best linear approximation" to the function $g \mapsto g' = qgq^{-1}$ in the sense that $g'(q+h) - g'(h) \approx \mathbf d g'(h)$. In my notation above, $\mathbf dq$ is this "displacement" $h$. The Jacobian $J$ is the matrix of the differential, so if you input e.g. $\mathbf dq = 1$ you get the first column: $$\mathbf g'(1) = J_{11} + J_{21}i + J_{31}j + J_{41}k.$$ Similarly, $\mathbf dg'(i), \mathbf dg'(j), \mathbf dg'(k)$ are the 2nd, 3rd, and 4th columns. – Nicholas Todoroff Jul 19 '24 at 22:28
  • Thanks again for further clarification. It makes more sense now. However, I have one more confusion: based on what you explained, once I put the values of $\overrightarrow{g}$ (i.e. $[0, 0, g_z]^T$) and equations for $\overrightarrow{g'}$ (the ones I mentioned in my question), I'll get a 4x4 Jacobian matrix. But here $g$ and $g'$ are 3D vectors and $q$ is 4D. So, for that, the Jacobian matrix should be 3x4, not 4x4, correct? – Milan Jul 22 '24 at 19:45
  • 1
    @Milan Yes, in other words the $4{\times}4$ Jacobian is guaranteed to have its first row be all zeroes. – Nicholas Todoroff Jul 22 '24 at 23:38
1

$ \def\o{{\tt1}} \def\lR#1{\Big(#1\Big)} \def\bR#1{\Big[#1\Big]} \def\LR#1{\left(#1\right)} \def\BR#1{\left[#1\right]} \def\q{\quad} \def\qq{\qquad} \def\qif{\q\iff\q} \def\qiq{\q\implies\q} \def\p{\partial} \def\grad#1#2{\frac{\p #1}{\p #2}} \def\m#1{\left[\begin{array}{r}#1\end{array}\right]} \def\c#1{\color{red}{#1}} \def\CLR#1{\c{\LR{#1}}} \def\gradLR#1#2{\LR{\grad{#1}{#2}}} $To add to Nicholas's excellent answer, note that the component-wise self-gradient of a quaternion is equal to the unit quaternion corresponding to that component, i.e. $$\eqalign{ \grad q{q_n} = &e_n \qq {\sf for}\; n\in\{w,x,y,z\} \\ &e_w = \m{\o,0,0,0},\q e_x = \m{0,\o,0,0} \\ &e_y\, = \m{0,0,\o,0},\q e_z = \m{0,0,0,\o} \\ }$$ So his differential expression can be readily translated into quaternion-valued gradients $$\eqalign{ \grad{g'}{q_n} &= \LR{e_ng-g'e_n}q^{-1} \\ }$$

greg
  • 40,033