Trying to find a relationship between angular velocity and time derivative of rotation vector

122 Views Asked by At

This question is related to enter link description here.

I am trying to simulate IMU measurement of angular velocity $\omega$ without approximation by using discrete steps.

The way I do it is I start from an analytical time varying rotation vector like $\phi(t) = [\sin(at + 1), \sin(bt + 2), \sin(ct + 3)]^T$, whose derivative is easily found to be $\dot{\phi}(t) = [a\cos(at + 1), b\cos(bt + 2), c\cos(ct + 3)]^T$.

Hence, using the Rodrigues formula we can convert $\phi$ to rotation matrix $R$:

$$ R(\phi) = I + \frac{\sin\theta}\theta \phi_\times + \frac{(1-\cos\theta)}{\theta^2}(\phi_\times)^2, $$ where $\theta = \|\phi\|$ and $(\cdot)_\times$ denotes the skew-symetric matrix of $(\cdot)$.

We also know that the angular velocity measured in body frame is related to the matrix derivative by

$$ \dot{R} = R\omega_\times \ \ \ \ (1) $$

Thus, I use Result 1 in this paper to find an alternative expression for $\dot{R}$, which is:

$$ \dot{R} = \frac{\partial R}{\partial t} = \frac{\partial R}{\partial \phi} \frac{\partial \phi}{\partial t} = \frac{\partial R}{\partial \phi} \dot{\phi} = -R\dot{\phi}_\times \frac{\phi\phi^T + (R^T-I)\phi_\times}{\|\phi\|^2} \ \ \ \ (2) $$

So combing (1) and (2) and dropping the common term $R$, we obtain the following

$$ \omega_\times = -\dot{\phi}_\times \frac{\phi\phi^T + (R^T-I)\phi_\times}{\|\phi\|^2} $$

Here is where the problem appears. On the LHS we have a skew symetric matrix, but on the RHS it is not the case when I plug in some numbers.

Where could I have been wrong? Thanks!