I want to generate a rotation and the corresponding angular velocity in 3-d space in order to test a Kalman filter, but converting and applying these angular velocities as quaternions gives me wrong results compared to just applying an angular velocity to an angular rotation.
Can someone tell me what I am doing wrong ?
Here is my approach:
Let's assume we have
$f(t) = \sin(t)$
$dx f(t) = \cos(t)$
I have a rotation and an angular velocity for the 3-axes
$\phi(t) = [x\;y\;z]; \;\omega(t) = [x\;y\;z]$.
The rotation is given given by
$\phi(t) = [f(t)\;f(t+1)\;f(t+2)]$,
and the angular velocity by
$\omega(t) = [dx f(t)\;dx f(t+1)\;dx f(t+2)]$.
For the measurement step we have:
$\phi(t) = \phi(t)$
$q(t) = \text{RotationVectorToQuat}(\phi(t)) $
For each prediction step the new rotation is given by:
$dt = t - t_{-1}$
$\phi(t) = \phi (t-1) * \omega(t) * dt$
$q(t) = q(t-1) \otimes \text{RotationVectorToQuat}(\omega(t)*dt) $
RotationVectorToQuat is given by
$\text{angle} = \text{norm}(\phi)$
$\text{halfAngle} = \text{angle}/2$
$q = [\cos(\text{halfAngle})\; (\phi/\text{angle} * \sin(\text{halfAngle}))]$
Here is the resulting plot, as you can see the quaternion result converted back to a rotation vector drifts during the prediction.
You can checkout my matlab demo here: https://drive.proton.me/urls/GVZHW21W78#XGKYTgCbc84f
This is... not right. You cannot just turn $\omega(t)\,dt$ into a rotation quaternion and expect to get the correct update to $q$.
The quaternion $q$ defines the motion of a particle with position vector $r$ (represented as an imaginary quaternion) via $$ r(t) = q(t)r(0)\bar q(t) $$ where $\bar q$ is the conjugate and we've assumed that $q$ is normalized. Here we are using juxtaposition for quaternion multiplication. The angular velocity is defined by $$ \dot r(t) = r(t)\times\omega(t) $$ with $\dot r$ the time derivative and $\times$ corresponding to the vector cross product, though it can be written algebraically using the quaternion product as $$ A\times B = \frac12(AB - BA). $$ What you'll find by differentiating the first equation with $r$ and $q$ (and by some other shennanigans) is $$ \dot q(t) = \frac12\omega(t)q(t). $$ That is to say that the update equation for $q$ must be $$ \frac{q(t) - q(t - dt)}{dt} = \frac12\omega(t)q(t) \implies \left(1-\frac12\omega(t)\,dt\right)q(t) = q(t-dt) $$$$ \implies q(t) = \left(1-\frac12\omega(t)\,dt\right)^{-1}q(t-dt). $$ Since $\omega$ is pure imaginary we can write $$ q(t) = \frac{1+\frac12\omega(t)\,dt}{1 + \frac14|\omega(t)|^2}q(t-dt), $$ though it would be best to just compute $$ \left(1 + \frac12\omega(t)\,dt\right)q(t-dt) $$ and then normalize. Remember that these are quaternion products. Note also that $\phi$ is unecessary except maybe to specify the inital orientation.
A possible reference is section 3.4.1 of Geometric Algebra for Physicists by Chris Doran and Anthony Lasenby, though this is a book employing geometric algebra and not just quaternions. This particular topic is expressible using just quaternions though, and so I've adapted the content there slightly.