8

I've implemented a Quaternion Kalman filter and i have the choice between multiple way to integrate angular velocities. The goal is to predict futur orientation $q^{n+1}$ from current orientation $q^{n}$ and angular velocity $\vec{r}$. During the time step $\Delta_t$ separating $q^{n+1}$ from $q^{n}$, the angular velocity is said to be constant.

The first method transform angular velocity $\vec{r}=[r_x \ r_y \ r_z]$ into a quaternion $q_r$ and multiply the result with the current orientation quaternion $q^n$ :

$$ q_r =(a,\vec{v})\\ a = \cos{(\frac{|\vec{r}|\Delta_t}{2})} \\ \vec{v}=\sin{(\frac{|\vec{r}|\Delta_t}{2})}\frac{\vec{r}}{|\vec{r}|}\\ q^{n+1}=q^{n}q_r $$

The second method is based on the quaternion derivative formula : $$ q_r =(0,\vec{r})\\ q^{n+1}=q^n+ \Delta_t(\frac{1}{2}q^nq_r) $$

What are the fundamental differences between the two approach? What are their properties ?

I understand the second one is a simple Euler integration, a crude first order approximation assuming a fixed $q$ and $\vec{r}$ during integration. At the end, we can possibly have $|q^{n+1}|$ different from unity so a normalization can be necessary. This kind of approach, based on the derivative, can easily be generalized to higher orders.

On the other hand, i don't know what the first approach really is. It doesn't require any normalization. In which aspect is it an approximation ? If this integration method a numeric approximation, what's its order ? Could a RK4 approximation be better ?

Here is a similar question asked on a programming thread. No clear answer was given.

Here is a patent relative to the first method : http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20080004113.pdf

Best,

Community
  • 1
jcolafrancesco
  • 153

2 Answers2

2

Method 1: As was pointed out the first method uses the angular velocity to calculate a difference-orientation $q_r$, which accumulated as a result of the contant angular velocity. This method is an Euler-approximation of the integral of the angular velocity! $$\Delta \alpha = |\vec{r}| \Delta_t$$ is a first order approximation of this difference angle! In the next step this angle is transformed into the quaternion $q_r$ and then this difference rotaion is applied to the current rotation.

The benefit is a correct integration for constant velocities. The drawback is a computational effor by using trigonometric functions.

Method 2: Here, the calculations are ideal up until the derivative of the quaternion $$\dot{q} = \frac{1}{2} q_r \, q $$ which has no simplifications in it. Now at this (later) point you apply the approximations that 1) the angular velocity is constant and 2) that you can integrate this quaternion derivative by a first order method. You can easily use a higher order method for integrating this last equation, yes! RK4 is a valid choice.

Conclusion: Both methods include a first order integration. However, the first one yields correct rotations for constant velocities and is affecting the norm of the quaternion less than the latter approach. However, using a higher order method I would prefer the second approach, since it is computationally more efficient. This is the main point of method 2! You can implement this on really cheap hardware with high speed even with fixed point arithmetics!

mike
  • 654
  • Isn't better to refer 1) as "exp map" derivative ? (that is just identity)? – minorlogic Jul 05 '24 at 11:06
  • "better" only makes sense with a goal functional in mind. I think it is a true statement. I've tried to state it in terms of time and widely known numerics, rather than their differential geometric names. – mike Aug 30 '24 at 11:57
  • "Better" in terms of generalization. – minorlogic Sep 07 '24 at 16:07
1

I can't say much about general approach, but I think I can explain what the first method is.

When you calculate $q_r$, what you're actually calculating is the rotation undergone by the object with that angular velocity. |r⃗ |Δt is the angle rotated through (since r is degrees/sec or radians/sec). Then you are using that angle to make a quaternion in the standard way ($q = <\cos(\theta/2), \sin(\theta/2)*\vec{v}$ > with $\vec{v}$ being a unit vector).

When you multiply $q$ by $q_r$, you are applying the rotation $q_r$ to the body that used to have the orientation $q$ (since multiplying by a quaternion applies that rotation). If your angular velocity is constant, then the result should be exact (because $q_r$ will be exactly the rotation undergone). if your angular velocity isn't constant, then it is an approximation.

I'm not sure what is best to use, sorry.

sources : http://www.cprogramming.com/tutorial/3d/quaternions.html, http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/

michael bagherpour
  • 11
  • Since this question was asked 4 months ago, you could have just waited and given a comprehensive answer – Shailesh Aug 07 '16 at 18:14