Quaternion - calibration sensor

62 Views Asked by At

I have attached my sensor to a box. What I am interested in is the rotation of the box with respect to the world. In my case, to avoid the gimball lock, and beacause the sensor already provides it, I am using a quaternion.

The relation between the orientation I am interested in, and the output of the sensor is the following:

$$q_{sensor/world} = q_{sensor/box} \cdot q_{box/world}$$

I know that my sensor is not perfectly aligned, hence the $$q_{sensor/box}$$ term is not a unit vector, but an unknown. If I want to compute the orientation of the box with respect to the world, what is the best approach to do it ?

1

There are 1 best solutions below

1
On BEST ANSWER

You're going to have to do some calibration. Setup an experiment in such a way that you know exactly what the $box/world$ orientation is and calculate $q_{box/world}$ from this; combine this with the corresponding $q_{sensor/world}$ reading to get $q_{sensor/box}=q_{sensor/world}q_{box/world}^{-1}$.

If we represent a vector $v$ as an imaginary quaterion then its rotation via another quaternion $q$ is $v \mapsto qvq^{-1}$. If we replace $q$ with $-q$ we get the same rotation. When you take two measurements $q_1, q_2$ of $q_{sensor/box}$ and want to average them, then it might end up that this average is nearly zero because we ended up with $q_1 \approx -q_2$. The simplest way to combat this is to check a component of $q_1$ and $q_2$ and replace e.g. $q_2$ with $-q_2$ if the sign of this component differs in $q_1, q_2$. If the component is you check is zero or close to zero, then you should choose a different component.

Now a note on averaging. You could take your measurements $q_1, q_2,\dotsc q_k$, form $q = q_1 + q_2 + \dotsb q_k$, and then maybe normalize this to $q/|q|$ where of course $|q| = \sqrt{q\bar q}$. It is better practice however to average their logarithms since this keeps everything in "rotation space" (though you may still prefer the naieve averaging because the calculation is simpler). What I mean is that every rotation quaternion $q$ can be written $$ q = e^{\theta v/2} $$ where $v$ is a purely imaginary unit quaternion representing the axis of rotation and $\theta$ is the angle of rotation about this axis. So if $q_i = e^{\theta_i v_i/2}$, it better to form their "average" as $$ q = \exp\frac1k\sum_{i=1}\frac{\theta_i v_i}2. $$ We can do this as follows: for simplicity, normalize the signs of $q_1,\dotsc,q_k$ as described above. Then since Euler's formula applies $$ q_i = e^{\theta_iv_i/2} = \cos\theta_i/2 + v_i\sin\theta_i/2 $$ and if $q_i = q_{iw} + q_{ix}i + q_{iy}j + q_{iz}k$ then that means $$ \theta_i/2 = \arctan\frac{\sqrt{q_{ix}^2 + q_{iy}^2 + q_{iz}^2}}{q_{iw}},\quad v_i = \frac{q_{ix}i + q_{iy}j + q_{iz}k}{\sqrt{q_{ix}^2 + q_{iy}^2 + q_{iz}^2}}. $$ If preferred, we can also write this in terms of just $q_i$: $$ \theta_i/2 = \arctan\frac{|q_i - \bar q_i|}{q_i + \bar q_i},\quad v_i = \frac{q_i - \bar q_i}{|q_i - \bar q_i|}. $$ Now let $V$ be the average of these quantities: $$ V = \frac1k\sum_{i=1}^k\frac{\theta_i}2v_i. $$ Then the "average" quaternion $q$ is $$ q = \cos|V| + \frac V{|V|}\sin|V| $$