I have IMU angular velocity measurements and trying to constrain the rotational part of a B-Spline trajectory by the measurements as follows(Eq 259 of Diebel's paper https://www.astro.rug.nl/software/kapteyn/_downloads/attitude.pdf).

$$e = \textbf{R}(\textbf{r})\omega- W(\textbf{r})G(\textbf{r})\dot{\textbf{r}}$$

Where $\textbf{r}$ is the rotation vector and $W(\textbf{r})G(\textbf{r})\dot{\textbf{r}}$ a standard rotation vector to angular velocity conversion. $\textbf{R}$ is local to the world rotation matrix to convert local angular velocity to the world one. And the rotation vector is a function of four nearby control points $\textbf{c}$. The problem is how to calculate analytic Jacobian of above. The first term $\textbf{R}\omega$ is easy and will be look like $[[\textbf{R}\omega]_{\times} f_1 \ \ [\textbf{R}\omega]_{\times} f_2 ...]$, where $f_n$ are a spline weight function and it is 3 by 12 matrix. And applying the product rule the second term becomes similar to

$$\frac{\partial W(\textbf{r})}{\partial \textbf{r}}G(\textbf{r})\dot{\textbf{r}}+W(\textbf{r})\frac{\partial G(\textbf{r})}{\partial \textbf{r}}\dot{\textbf{r}}+W(\textbf{r})G(\textbf{r})\frac{\partial \dot{\textbf{r}}}{\partial \textbf{r}}$$

Of course, it is wrong. I realized that when I was trying to evaluate the mid term as

$$W(\textbf{r})\frac{\partial G(\textbf{r})}{\partial \textbf{r}}\dot{\textbf{r}}=W(\textbf{r})\frac{\partial G(\textbf{r})}{\partial \textbf{r}}\frac{\partial \textbf{r}}{\partial \textbf{c}}\dot{\textbf{r}}$$

The matrix dimension does not match.

I guess I am doing wrong with matrix to vector derivative. I have been checking some of online materials for the vector to vector or vector to matrix product rule but it has not been successful.

Can anyone give me a hint?


I realized that there is a direct conversion method for the rotation vector to angular velocity problem. Their jacobian is easier than the jacobian of the above. The implementation is here: https://github.com/ethz-asl/kalibr


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.