\documentclass {article}

\usepackage{color}
\title {Rotation matrix as an exponential}

\newcommand\reals{\mathbf{R}}
\newcommand\rot{\mathbf{r}}
\newcommand\x{\mathbf{x}}
\newcommand\y{\mathbf{y}}
\newcommand\rcross[1]{[\rot_{#1}]_{\times}}
\newcommand\xcross{[\mathbf{x}]_{\times}}
\newcommand\ycross{[\mathbf{y}]_{\times}}
\newcommand\omegacross{[\omega]_{\times}}
\newcommand\rdotcross{\left[\dot{\rot}\right]_{\times}}
\newcommand\normr{\|\rot\|}
\newcommand\e{\mathbf{e}}
\newcommand\alphap{\alpha^{\prime}}
\newcommand\cross[1]{[#1]_{\times}}
\begin {document}
\maketitle

\section {Introduction and notation}

Let $\rot\in\reals^3$ be a vector. We denote by
\begin{itemize}
\item $$
\rcross{} = \left(\begin{array}{ccc}
0 & -\rot_3 & \rot_2 \\
\rot_3 & 0 & -\rot_1 \\
-\rot_2 & \rot_1 & 0
\end{array}\right)
$$
the antisymmetric matrix corresponding to the cross product by $\rot$,
\item $R = \exp (\rcross{})$ the rotation matrix corresponding to the rotation about
$\rot$.
\end{itemize}
According to Rodrigues formula, we have
\begin{equation}\label{eq:rodrigues}
R = \exp (\rcross{}) = I_3 + \frac{\sin \normr}{\normr}\rcross{} +
\frac{1 - \cos \normr}{\normr^2}\rcross{}^2
\end{equation}
We define
\begin{eqnarray}
\label{eq:alpha1}
\alpha_1 (x) &=& \frac{\sin x}{x}\\
\label{eq:alpha2}
\alpha_2 (x) &=& \frac{1 - \cos x}{x^2}\\
\label{eq:alpha3}
\alphap_1 (x) &=& \frac{x \cos x - \sin x}{x^2}\\
\label{eq:alpha4}
\alphap_2 (x) &=& \frac{x \sin x -2(1-\cos x)}{x^3}
\end{eqnarray}
With this notation, we can write
\begin{equation}\label{eq:rodrigues2}
R = I_3 + \alpha_1 (\normr) \rcross{} + \alpha_2 (\normr) \rcross{}^2
\end{equation}
\subsection {Powers of $\rcross{}$}

Let us compute the powers of $\rcross{}$:
\begin{eqnarray}
\rcross{}^2 &=& \left(\begin{array}{ccc}
-\rot_2^2-\rot_3^2 & \rot_2\rot_1 & \rot_3\rot_1 \\
\rot_1\rot_2 & -\rot_1^2-\rot_3^2 & \rot_3\rot_2 \\
\rot_1\rot_3 & \rot_2\rot_3 & -\rot_1^2-\rot_2^2
\end{array}\right) =
\rot\rot^T - \normr^2 I_3\\
\label{eq:rcross2}
\rcross{}^3 &=& \rcross{}\rot\rot^T - \normr^2 \rcross{} = - \normr^2 \rcross{}\\
\label{eq:rcross3}
\rcross{}^4 &=& - \normr^2 \rcross{}^2
\end{eqnarray}
Replacing in~(\ref{eq:rodrigues2}), we get
\begin{eqnarray}
R &=& I_3 + \alpha_1 (\normr) \rcross{} + \alpha_2 (\normr) \rot\rot^T - \alpha_2 (\normr) \normr^2 I_3\\
\label{eq:rodrigues3}
&=&I_3 + \alpha_1 (\normr) \rcross{} + \alpha_2 (\normr) \rot\rot^T + (\cos\normr-1) I_3
\end{eqnarray}

\section {Derivative}
We wish to find a relation between the derivative of $R$ as an antisymmetric
matrix and the derivative of vector $\rot$. We recall that
$$
\omegacross = \dot{R}R^T
$$
\begin{eqnarray*}
\normr = \sqrt{\rot^T\rot} && \frac{d}{dt}{\normr} = \frac{\rot^T\dot{\rot}}{\normr}
\end{eqnarray*}
Let us differentiate~(\ref{eq:rodrigues2})
$$
\dot{R} = \frac{\dot{\rot}^T\rot}{\normr} \alphap_1 (\normr) \rcross{} + \frac{\dot{\rot}^T\rot}{\normr}\alphap_2 (\normr) \rot\rot^T -\frac{\dot{\rot}^T\rot}{\normr} \sin\normr I_3 + \alpha_1 (\normr) \rdotcross + \alpha_2 (\normr) (\dot{\rot}\rot^T + \rot\dot{\rot}^T)
$$
To make notation lighter, we set $u = \frac{\rot^T\dot{\rot}}{\normr}$. Then
\begin{eqnarray*}
\omegacross &=& \dot{R}R^T\\
&=& \left(u \alphap_1 \rcross{} + u\alphap_2 \rot\rot^T -u \sin\normr I_3 + \alpha_1 \rdotcross + \alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T)\right)\\
&&\left(I_3 - \alpha_1 \rcross{} + \alpha_2 \rot\rot^T + (\cos\normr-1) I_3\right)\\
&=& u \alphap_1 \rcross{} - u \alphap_1\alpha_1 \rcross{}^2 + u \alphap_1\alpha_2 \rcross{}\rot\rot^T + u \alphap_1 (\cos\normr-1) \rcross{}\\
&& +u\alphap_2 \rot\rot^T - u\alpha_1\alphap_2\rot\rot^T\rcross{} + u\alphap_2\alpha_2\normr^2\rot\rot^T + u\alphap_2 (\cos\normr-1) \rot\rot^T\\
&& -u \sin\normr I_3 + u \sin\normr\alpha_1 \rcross{} - u \sin\normr\alpha_2 \rot\rot^T - u \sin\normr(\cos\normr-1) I_3\\
&& +\alpha_1 \rdotcross - \alpha_1^2 \rdotcross\rcross{} + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1(\cos\normr-1)\rdotcross\\
&&+\alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T) - \alpha_1 \alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T)\rcross{} + \alpha_2 \alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T)\rot\rot^T + (\cos\normr-1)\alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T)\\
\end{eqnarray*}
We notice that $\rcross{} \rot = 0$ and that $\rot^T\rcross{} = 0$. Thus by substituting $\rcross{}^2$ by $\rot\rot^T-\normr^2 I_3$,
\begin{eqnarray*}
\omegacross &=&
u \alphap_1 \rcross{} - u \alphap_1\alpha_1 (\rot\rot^T-\normr^2 I_3) + u \alphap_1 (\cos\normr-1) \rcross{}\\
&& + u\alphap_2 \rot\rot^T + u\alphap_2\alpha_2\normr^2\rot\rot^T + u\alphap_2 (\cos\normr-1) \rot\rot^T\\
&& -u \sin\normr I_3 + u \sin\normr\alpha_1 \rcross{} - u \sin\normr\alpha_2 \rot\rot^T - u \sin\normr(\cos\normr-1) I_3\\
&& +\alpha_1 \rdotcross - \alpha_1^2 \rdotcross\rcross{} + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1(\cos\normr-1)\rdotcross\\
&&+\alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T) - \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross{} +  \alpha_2^2\normr^2\dot{\rot}\rot^T + \alpha_2^2\rot\dot{\rot}^T\rot\rot^T + (\cos\normr-1)\alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T)
\end{eqnarray*}
We notice that $\rot\dot{\rot}^T\rot\rot^T=u\normr\rot\rot^T$
\begin{eqnarray*}
\omegacross &=&
- u \alphap_1\alpha_1 (\rot\rot^T-\normr^2 I_3) + u \alphap_1 \cos\normr \rcross{}\\
&& + u\alphap_2 \rot\rot^T + u\alphap_2\alpha_2\normr^2\rot\rot^T + u\alphap_2 (\cos\normr-1) \rot\rot^T\\
&& + u \sin\normr\alpha_1 \rcross{} - u \sin\normr\alpha_2 \rot\rot^T - u \sin\normr\cos\normr I_3\\
&& +\alpha_1 \rdotcross - \alpha_1^2 \rdotcross\rcross{} + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1(\cos\normr-1)\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross{} +  \alpha_2^2\normr^2\dot{\rot}\rot^T + \alpha_2^2u\normr\rot\rot^T + \cos\normr\alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T)
\end{eqnarray*}
$$
u\alphap_2\alpha_2\normr^2\rot\rot^T + u\alphap_2 (\cos\normr-1) \rot\rot^T =
u\alphap_2((1-\cos\normr)+(\cos\normr-1)) \rot\rot^T = 0
$$
\begin{eqnarray*}
\omegacross &=&
- u \alphap_1\alpha_1 (\rot\rot^T-\normr^2 I_3) + u \alphap_1 \cos\normr \rcross{} + u\alphap_2 \rot\rot^T\\
&& + u \sin\normr\alpha_1 \rcross{} - u \sin\normr\alpha_2 \rot\rot^T - u \sin\normr\cos\normr I_3\\
&& +\alpha_1 \rdotcross - \alpha_1^2 \rdotcross\rcross{} + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1(\cos\normr-1)\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross{} +  \alpha_2^2\normr^2\dot{\rot}\rot^T + \alpha_2^2u\normr\rot\rot^T + \cos\normr\alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T)\\
&=& - u \alphap_1\alpha_1 \rot\rot^T + u \alphap_1\alpha_1\normr^2 I_3 + u \alphap_1 \cos\normr \rcross{} + u\alphap_2 \rot\rot^T\\
&& + u \sin\normr\alpha_1 \rcross{} - u \sin\normr\alpha_2 \rot\rot^T - u \sin\normr\cos\normr I_3\\
&& +\alpha_1 \rdotcross - \alpha_1^2 \rdotcross\rcross{} + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1(\cos\normr-1)\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross{} +  \alpha_2^2\normr^2\dot{\rot}\rot^T + \alpha_2^2u\normr\rot\rot^T + \cos\normr\alpha_2\dot{\rot}\rot^T + \cos\normr\alpha_2\rot\dot{\rot}^T\\
&=& u \left(-\alphap_1\alpha_1 + \alphap_2 - \sin\normr\alpha_2 + \alpha_2^2\normr\right)\rot\rot^T \\
&&+ u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross{}\\
&&+ u\left(\alphap_1\alpha_1\normr^2 - \sin\normr\cos\normr\right) I_3\\
&& +\alpha_1 \rdotcross - \alpha_1^2 \rdotcross\rcross{} + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1(\cos\normr-1)\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross{} +  \alpha_2^2\normr^2\dot{\rot}\rot^T + \cos\normr\alpha_2\dot{\rot}\rot^T + \cos\normr\alpha_2\rot\dot{\rot}^T\\
\end{eqnarray*}
\begin{eqnarray*}
-\alphap_1\alpha_1 + \alphap_2 - \sin\normr\alpha_2 + \alpha_2^2\normr
&=&
-\frac{\normr \cos \normr - \sin \normr}{\normr^2}\frac{\sin \normr}{\normr} + \frac{\normr \sin \normr -2(1-\cos \normr)}{\normr^3}\\
&& - \sin\normr\frac{1 - \cos \normr}{\normr^2} + \left(\frac{1 - \cos \normr}{\normr^2}\right)^2\normr\\
&=&\frac{-\normr \cos \normr \sin \normr + \sin^2 \normr}{\normr^3}+\frac{\normr \sin \normr -2+2\cos \normr}{\normr^3}\\
&& +\frac{-\normr\sin\normr + \normr\sin\normr\cos \normr}{\normr^3}\\
&&+\frac{1 - 2\cos \normr + \cos^2 \normr}{\normr^3}\\
&=& 0
\end{eqnarray*}
\begin{eqnarray*}
\alphap_1\alpha_1\normr^2 - \sin\normr\cos\normr &=&
\frac{\normr \cos \normr - \sin \normr}{\normr^2}\frac{\sin \normr}{\normr}\normr^2- \sin\normr\cos\normr\\
&=&
\frac{\normr\cos\normr\sin\normr - \sin^2\normr}{\normr}- \sin\normr\cos\normr\\&=&-\frac{\sin^2\normr}{\normr}
\end{eqnarray*}

\begin{eqnarray*}
\omegacross &=&
+ u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross{} -u \frac{\sin^2\normr}{\normr} I_3\\
&& -\alpha_1^2 \rdotcross\rcross{} + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1\cos\normr\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross{} +  \alpha_2^2\normr^2\dot{\rot}\rot^T + \cos\normr\alpha_2\dot{\rot}\rot^T + \cos\normr\alpha_2\rot\dot{\rot}^T\\
\end{eqnarray*}
\begin{eqnarray*}
\alpha_2^2\normr^2\dot{\rot}\rot^T + \cos\normr\alpha_2\dot{\rot}\rot^T &=&
\alpha_2\left(\alpha_2\normr^2 + \cos\normr\right)\dot{\rot}\rot^T\\
\alpha_2\normr^2 + \cos\normr &=& \frac{1 - \cos \normr}{\normr^2}\normr^2 + \cos\normr = 1
\end{eqnarray*}
\begin{eqnarray*}
\omegacross &=&
+ u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross{} -u \frac{\sin^2\normr}{\normr} I_3\\
&& -\alpha_1^2 \rdotcross\rcross{} + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1\cos\normr\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross{} + \alpha_2\dot{\rot}\rot^T + \cos\normr\alpha_2\rot\dot{\rot}^T\\
\end{eqnarray*}
Using~(\ref{eq:xcrossycross}),
\begin{eqnarray*}
\omegacross &=&
u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross{} -u \frac{\sin^2\normr}{\normr} I_3\\
&& -\alpha_1^2 (\rot\dot{\rot}^T-\rot^T\dot{\rot} I_3) + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1\cos\normr\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross{} + \alpha_2\dot{\rot}\rot^T + \cos\normr\alpha_2\rot\dot{\rot}^T\\
&=& u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross{} -u \frac{\sin^2\normr}{\normr} I_3\\
&& -\alpha_1^2\rot\dot{\rot}^T+\alpha_1^2\rot^T\dot{\rot} I_3 + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1\cos\normr\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross{} + \alpha_2\dot{\rot}\rot^T + \cos\normr\alpha_2\rot\dot{\rot}^T\\
\end{eqnarray*}
\begin{eqnarray*}
-u \frac{\sin^2\normr}{\normr} I_3 + \alpha_1^2\rot^T\dot{\rot} I_3 &=&
-\frac{\rot^T\dot{\rot}}{\normr}\frac{\sin^2\normr}{\normr}+ \alpha_1^2\rot^T\dot{\rot} = 0
\end{eqnarray*}
\begin{eqnarray*}
\omegacross &=&
u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross{}\\
&& -\alpha_1^2\rot\dot{\rot}^T+ \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1\cos\normr\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross{} + \alpha_2\dot{\rot}\rot^T + \cos\normr\alpha_2\rot\dot{\rot}^T\\
\end{eqnarray*}
\begin{eqnarray*}
-\alpha_1^2\rot\dot{\rot}^T+ \cos\normr\alpha_2\rot\dot{\rot}^T &=&
\left(-\frac{\sin^2\normr}{\normr^2}+\cos\normr\frac{1-\cos\normr}{\normr^2}\right)\rot\dot{\rot}^T\\
&=&\frac{-\sin^2\normr-\cos^2\normr+\cos\normr}{\normr^2}\rot\dot{\rot}^T
=-\alpha_2\rot\dot{\rot}^T\\
\end{eqnarray*}
\begin{eqnarray*}
\omegacross &=&
u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross{}\\
&& + \alpha_2\left(\dot{\rot}\rot^T -\rot\dot{\rot}^T\right)+ \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1\cos\normr\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross{}\\
\end{eqnarray*}
Using~(\ref{eq:dotrrT-rdotrT}),
\begin{eqnarray*}
\omegacross &=&
u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross{} + \alpha_2\left[\rot\times\dot{\rot}\right]_{\times} + \alpha_1\cos\normr\rdotcross\\
&& + \alpha_1 \alpha_2 \left(\rdotcross\rot\rot^T - \rot\dot{\rot}^T\rcross{}\right)\\
\end{eqnarray*}
Finally, using~(\ref{eq:ouf}),
\begin{eqnarray*}
\omegacross &=&
u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross{} + \alpha_2\left[\rot\times\dot{\rot}\right]_{\times} + \alpha_1\cos\normr\rdotcross\\
&& + \alpha_1 \alpha_2 \left(\normr^2\rdotcross - \left[\rot\rot^T\dot{\rot}\right]_{\times}\right)\\
&=&u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross{} + \alpha_2\left[\rot\times\dot{\rot}\right]_{\times}\\
&& + \alpha_1\left(\cos\normr + \alpha_2\normr^2\right)\rdotcross - \alpha_1 \alpha_2\left[\rot\rot^T\dot{\rot}\right]_{\times}\\
&=&u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross{} + \alpha_2\left[\rot\times\dot{\rot}\right]_{\times} + \alpha_1\rdotcross - \alpha_1 \alpha_2\left[\rot\rot^T\dot{\rot}\right]_{\times}\\
&=&u\frac{\normr -\sin\normr\cos\normr}{\normr^2} \rcross{} + \alpha_2\left[\rot\times\dot{\rot}\right]_{\times} + \alpha_1\rdotcross - \alpha_1 \alpha_2\left[\rot\rot^T\dot{\rot}\right]_{\times}\\
\end{eqnarray*}
we get an expression of $\omega$ as a vector:
\begin{eqnarray*}
\omega &=&u\frac{\normr -\sin\normr\cos\normr}{\normr^2} \rot + \alpha_2\rot\times\dot{\rot} + \alpha_1\dot{\rot} - \alpha_1 \alpha_2\rot\rot^T\dot{\rot}\\
&=&\frac{\normr -\sin\normr\cos\normr}{\normr^3}(\rot^T\dot{\rot}) \rot + \alpha_2\rot\times\dot{\rot} + \alpha_1\dot{\rot} - \alpha_1 \alpha_2\rot\rot^T\dot{\rot}\\
&=&\frac{\normr -\sin\normr\cos\normr}{\normr^3}\rot\rot^T\dot{\rot} + \alpha_2\rot\times\dot{\rot} + \alpha_1\dot{\rot} - \alpha_1 \alpha_2\rot\rot^T\dot{\rot}\\
&=&\frac{\normr -\sin\normr}{\normr^3}\rot\rot^T\dot{\rot} + \alpha_2\rot\times\dot{\rot} + \alpha_1\dot{\rot}\\
\end{eqnarray*}
If we define the Jacobian matrix $J(\rot)$  such that
\begin{equation}\label{eq:jacobian}
\dot{\rot} = J(\rot)\omega
\end{equation}
we have
\begin{eqnarray*}
J^{-1} &=& \frac{\normr -\sin\normr}{\normr^3}\rot\rot^T + \alpha_2\rcross{} + \alpha_1 I_3\\
&=& \frac{\normr -\sin\normr}{\normr^3}\rot\rot^T + \frac{1 - \cos \normr}{\normr^2}\rcross{} + \frac{\sin \normr}{\normr} I_3\\
&=& \frac{\normr -\sin\normr}{\normr^3}(\rcross{}^2 + \normr^2 I_3) + \frac{1 - \cos \normr}{\normr^2}\rcross{} + \frac{\sin \normr}{\normr} I_3\\
&=& I_3  + \frac{1 - \cos \normr}{\normr^2}\rcross{} + \frac{\normr -\sin\normr}{\normr^3}\rcross{}^2\\
\end{eqnarray*}

\section{Computation of the Jacobian}

We need to inverse $J^{-1}$. For that, we make the assumption that $J$ is of the following form:
\begin{eqnarray*}
J &=& I_3 + \beta_1 \rcross{} + \beta_2 \rcross{}^2\\
JJ^{-1} &=& \left(I_3 + \beta_1 \rcross{} + \beta_2 \rcross{}^2\right)\left(I_3  + \frac{1 - \cos \normr}{\normr^2}\rcross{} + \frac{\normr -\sin\normr}{\normr^3}\rcross{}^2\right)\\
&=& I_3  + \frac{1 - \cos \normr}{\normr^2}\rcross{} + \frac{\normr -\sin\normr}{\normr^3}\rcross{}^2\\
&& +  \beta_1 \rcross{}  + \beta_1 \frac{1 - \cos \normr}{\normr^2}\rcross{}^2 + \beta_1 \frac{\normr -\sin\normr}{\normr^3}\rcross{}^3\\
&& +  \beta_2 \rcross{}^2  + \beta_2 \frac{1 - \cos \normr}{\normr^2}\rcross{}^3 + \beta_2 \frac{\normr -\sin\normr}{\normr^3}\rcross{}^4\\
&=& I_3 + \left(\frac{1 - \cos \normr}{\normr^2} + \beta_1\right)\rcross{} + \left(\frac{\normr -\sin\normr}{\normr^3} + \beta_1 \frac{1 - \cos \normr}{\normr^2} + \beta_2\right)\rcross{}^2\\
&& + \left(\beta_1 \frac{\normr -\sin\normr}{\normr^3} + \beta_2 \frac{1 - \cos \normr}{\normr^2}\right)(- \normr^2 \rcross{}) + \beta_2 \frac{\normr -\sin\normr}{\normr^3}(- \normr^2 \rcross{}^2)\\
&=& I_3 + \left(\frac{1 - \cos \normr}{\normr^2} + \beta_1\right)\rcross{} + \left(\frac{\normr -\sin\normr}{\normr^3} + \beta_1 \frac{1 - \cos \normr}{\normr^2} + \beta_2\right)\rcross{}^2\\
&& + \left(- \beta_1 \frac{\normr -\sin\normr}{\normr} - \beta_2 (1 - \cos \normr)\right)\rcross{} - \beta_2 \frac{\normr -\sin\normr}{\normr}\rcross{}^2\\
&=& I_3 + \left(\frac{1 - \cos \normr}{\normr^2} + \beta_1 \frac{\sin\normr}{\normr} - \beta_2 (1 - \cos \normr)\right)\rcross{} \\
&& + \left(\frac{\normr -\sin\normr}{\normr^3} + \beta_1 \frac{1 - \cos \normr}{\normr^2} + \beta_2 \frac{\sin\normr}{\normr}\right)\rcross{}^2 \\
\end{eqnarray*}
We get the following linear system
\begin{eqnarray*}
\normr\sin\normr \beta_1 - (1 - \cos \normr)\normr^2 \beta_2 &=& \cos \normr - 1 \\
\normr(1 - \cos \normr)\beta_1 + \normr^2\sin\normr \beta_2 &=& \sin\normr - \normr
\end{eqnarray*}
$$
\Delta = \normr^3\sin^2\normr + \normr^3 (1-\cos\normr)^2 = 2\normr^3(1 -\cos\normr)
$$
Hence
\begin{eqnarray*}
\beta_1 &=& \frac{1}{2\normr^3(1 -\cos\normr)}\left(\normr^2\sin\normr (\cos \normr - 1) + (1 - \cos \normr)\normr^2(\sin\normr - \normr)\right) \\
\beta_2 &=& \frac{1}{2\normr^3(1 -\cos\normr)}\left(-\normr(1 - \cos \normr)(\cos \normr - 1) + \normr\sin\normr(\sin\normr - \normr)\right)\\
\beta_1 &=& \frac{1}{2\normr^3(1 -\cos\normr)}\left(-(1 - \cos \normr)\normr^3\right) = -\frac{1}{2}\\
\beta_2 &=& \frac{1}{2\normr^2(1 -\cos\normr)}\left(-2\cos \normr + 2 - \normr\sin\normr\right) = \frac{1}{\normr^2} - \frac{\sin\normr}{2\normr(1-\cos\normr)}\\
\end{eqnarray*}
and finally
\begin{eqnarray}
\label{eq:J-1}
J &=& I_3 -\frac{1}{2}\rcross{} +  \left(\frac{1}{\normr^2} - \frac{\sin\normr}{2\normr(1-\cos\normr)}\right)\rcross{}^2\\
%% &=& \frac{\normr\sin\normr}{2(1-\cos\normr)} I_3 - \frac{1}{2}\rcross{} +  \left(\frac{1}{\normr^2} - \frac{\sin\normr}{2\normr(1-\cos\normr)}\right)\rot\rot^T\\
\label{eq:J-2}
&=& I_3 -\frac{1}{2}\rcross{} +  \left(\frac{2(1-\cos\normr) - \normr\sin\normr}{2\normr^2(1-\cos\normr)}\right)\rcross{}^2\\
\end{eqnarray}

\subsection {Jacobian with local representation of angular velocity}

We define $\Omega$ such that $\dot{R} = R \left[\Omega\right]_{\times}$. The relation
between $\omega$ and $\Omega$ is the following
$$
  \omega = R\Omega
$$
Thus
$$
\dot{\rot} = J (R) \omega = J (R) R \Omega
$$
In this section, we compute $J(R)R$ with respect to $\rot$. For that, we
multiply ({eq:J-1}) by (\ref{eq:rodrigues}):
\begin {eqnarray*}
J(R)R &=& (I_3 -\frac{1}{2}\rcross{} +  \left(\frac{1}{\normr^2} - \frac{\sin\normr}{2\normr(1-\cos\normr)}\right)\rcross{}^2)(I_3 + \frac{\sin \normr}{\normr}\rcross{} +
\frac{1 - \cos \normr}{\normr^2}\rcross{}^2)\\
&=& I_3 + \left(-\frac{1}{2} + \frac{\sin \normr}{\normr}\right)\rcross{} \\
&& + \left(-\frac{\sin \normr}{2\normr}+\frac{1}{\normr^2} - \frac{\sin\normr}{2\normr(1-\cos\normr)}+\frac{1 - \cos \normr}{\normr^2}\right)\rcross{}^2\\
&& + \left(-\frac{1 - \cos \normr}{2\normr^2} + (\frac{1}{\normr^2} - \frac{\sin\normr}{2\normr(1-\cos\normr)})\frac{\sin \normr}{\normr}\right)\rcross{}^3\\
&& + \left((\frac{1}{\normr^2} - \frac{\sin\normr}{2\normr(1-\cos\normr)})\frac{1 - \cos \normr}{\normr^2}\right)\rcross{}^4\\
&=& I_3 + \frac{2\sin\normr-\normr}{2\normr}\rcross{}\\
&& + \frac{-\normr\sin\normr(1-\cos\normr)+2(1-\cos\normr)-\normr\sin\normr+2(1-\cos\normr)^2}{2\normr^2(1-\cos\normr)}\rcross{}^2\\
&&+ \frac{-\normr(1-\cos\normr)^2 + 2(1-\cos\normr)\sin\normr - \normr\sin^2\normr}{2\normr^3(1-\cos\normr)} \rcross{}^3 \\
&&+ \frac{2(1-\cos\normr)^2-\normr\sin\normr(1-\cos\normr)}{2\normr^4(1-\cos\normr)} \rcross{}^4\\
&=& I_3 + \frac{2\sin\normr-\normr}{2\normr}\rcross{}\\
&& + \frac{-2\normr\sin\normr+\normr\sin\normr\cos\normr+4-6\cos\normr+2\cos^2\normr}{2\normr^2(1-\cos\normr)}\rcross{}^2\\
&&+ \frac{-\normr+2\normr\cos\normr-\normr\cos^2\normr + 2\sin\normr-2\cos\normr\sin\normr - \normr\sin^2\normr}{2\normr^3(1-\cos\normr)} \rcross{}^3 \\
&&+ \frac{2-4\cos\normr+2\cos^2\normr-\normr\sin\normr+\normr\sin\normr\cos\normr}{2\normr^4(1-\cos\normr)} \rcross{}^4
\end{eqnarray*}
Replacing $\rcross{}^3$ and $\rcross{}^4$ by their expressions (\ref{eq:rcross2}-\ref{eq:rcross3}), we get
\begin {eqnarray*}
J(R)R &=& I_3 + \frac{2\sin\normr-\normr}{2\normr}\rcross{}\\
&& + \frac{-2\normr\sin\normr+\normr\sin\normr\cos\normr+4-6\cos\normr+2\cos^2\normr}{2\normr^2(1-\cos\normr)}\rcross{}^2\\
&&- \frac{-\normr+2\normr\cos\normr-\normr\cos^2\normr + 2\sin\normr-2\cos\normr\sin\normr - \normr\sin^2\normr}{2\normr(1-\cos\normr)} \rcross{} \\
&&- \frac{2-4\cos\normr+2\cos^2\normr-\normr\sin\normr+\normr\sin\normr\cos\normr}{2\normr^2(1-\cos\normr)} \rcross{}^2\\
&=& I_3 + \frac{(2\sin\normr-\normr)(1-\cos\normr)}{2\normr(1-\cos\normr)}\rcross{}\\
&&- \frac{-\normr+2\normr\cos\normr-\normr\cos^2\normr + 2\sin\normr-2\cos\normr\sin\normr - \normr\sin^2\normr}{2\normr(1-\cos\normr)} \rcross{} \\
&& + \frac{-\normr\sin\normr+2-2\cos\normr}{2\normr^2(1-\cos\normr)}\rcross{}^2\\
&=& I_3 + \frac{2\sin\normr-\normr - 2\sin\normr\cos\normr+\normr\cos\normr}{2\normr(1-\cos\normr)}\rcross{}\\
&&- \frac{-\normr+2\normr\cos\normr-\normr\cos^2\normr + 2\sin\normr-2\cos\normr\sin\normr - \normr\sin^2\normr}{2\normr(1-\cos\normr)} \rcross{} \\
&& + \frac{-\normr\sin\normr+2-2\cos\normr}{2\normr^2(1-\cos\normr)}\rcross{}^2\\
&=& I_3 + \frac{-\normr\cos\normr + \normr}{2\normr(1-\cos\normr)}\rcross{} + \frac{-\normr\sin\normr+2-2\cos\normr}{2\normr^2(1-\cos\normr)}\rcross{}^2\\
&=& I_3 + \frac{1}{2}\rcross{} + \frac{2(1-\cos\normr)-\normr\sin\normr}{2\normr^2(1-\cos\normr)}\rcross{}^2
\end{eqnarray*}

\section{Logarithm}

From equations \ref{eq:rodrigues} and \ref{eq:rcross2}, we have:

\begin{eqnarray}
\label{eq:antisymmetric}
\frac{R - R^T}{2} &=& \frac{\sin \normr}{\normr}\rcross{} \\
\label{eq:symmetric}
\frac{R + R^T}{2} &=& I_3 + \frac{1 - \cos \normr}{\normr^2}\rcross{}^2 \\
\label{eq:trace}
tr(R) &=& 1 + 2 \cos \normr
\end{eqnarray}

Thus,
\begin{equation}
\normr = \arccos \left( \frac{tr(R) - 1}{2} \right)
\end{equation}

In order to derive $\rot$ from $R$, one can use Eq.\ \ref{eq:antisymmetric}.
However, around $\normr = 0$ and $\normr = \pi$, inverting $\frac{\sin\normr}{\normr}$ is numerically unstable (as the limit of the division of two terms tending to 0).
Around $0$, a limited development, proposed below, allows us to go beyond the numerical issue.
Around $\pi$, a work around is proposed below, using both Eq.\ \ref{eq:antisymmetric} and Eq.\ \ref{eq:symmetric}.
The resolution is thus divided in the three following cases.

\subsection{Around $\normr = 0$}

Equation \ref{eq:antisymmetric} gives, at the second order in $\normr$:

\begin{equation}
  \rcross{} = ( 1 - \frac{\normr^2}{3}) \frac{R - R^T}{2}
\end{equation}

\subsection{Around $\normr = \pi$}

Equation \ref{eq:symmetric} gives, denoting $\phi = \normr - \pi$:

\begin{eqnarray*}
\frac{R + R^T}{2} &=& I_3 + \frac{1 - \cos \normr}{\normr^2}\rcross{}^2 \\
                  &=& I_3 + \frac{1 + \cos \left( \phi \right) }{\normr^2} \left( \rot \rot^t - \normr^2 I_3 \right) \\
                  &=& - \cos (\phi) I_3 + \frac{1 + \cos \left( \phi \right) }{\normr^2} \rot \rot^t
\end{eqnarray*}

so
\begin{equation}
\rot \rot^t = \frac{\normr^2}{1 - \cos\phi} \left( \frac{R + R^T}{2} + \cos(\phi) I_3 \right)
\end{equation}

The diagonal elements of $\rot \rot^t$ are the square of the elements of $\rot$.
As $\frac{\sin \normr}{\normr} \geq 0 $, components of $\rcross{}$ have the sign of $\frac{R - R^T}{2}$.
$r$ is fully known.

This expression is numerical less precise than the next one, as the logarithm is computed as a square root of
a vector computed with machine precision.

\subsection{Nominal case}

\begin{equation}
\rcross{} = \frac{\normr}{\sin \normr} \frac{R - R^T}{2}
\end{equation}

\section{Appendix}

\begin{eqnarray}
\xcross \ycross &=&
\left(\begin{array}{ccc}
-x_3y_3 - x_2y_2 & x_2y_1 & x_3y_1\\
x_1y_2 & -x_3y_3-x_1y_1 & x_3y_2 \\
x_1y_3 & x_2y_3 & -x_1y_1 - x_2y_2
\end{array}\right)\\
\label{eq:xcrossycross}
&=& \y\x^T-\x^T\y I_3
\end{eqnarray}
\begin{eqnarray}
\dot{\rot}\rot^T-\rot\dot{\rot}^T &=&
\left(\begin{array}{ccc}
0 & \dot{\rot}_1\rot_2 - \dot{\rot}_2\rot_1 & \dot{\rot}_1\rot_3 - \dot{\rot}_3\rot_1\\
\dot{\rot}_2\rot_1 - \dot{\rot}_1\rot_2 & 0 & \dot{\rot}_2\rot_3 - \dot{\rot}_3\rot_2\\
\dot{\rot}_3\rot_1 - \dot{\rot}_1\rot_3 & \dot{\rot}_3\rot_2 - \dot{\rot}_2\rot_3 & 0\end{array}\right)\\
\label{eq:dotrrT-rdotrT}
&=& \left[\rot\times\dot{\rot}\right]_{\times}
\end{eqnarray}
{\tiny
\begin{eqnarray*}
&\rdotcross\rot\rot^T-\rot\dot{\rot}^T\rcross{}&\\
&=&\\
&\left(\begin{array}{ccc}
0&-(\rot_1^2+\rot_2^2)\dot{\rot}_3+\rot_2\rot_3\dot{\rot}_2+\rot_1\rot_3\dot{\rot}_1 & (\rot_1^2+\rot_3^2)\dot{\rot}_2-\rot_1\rot_2\dot{\rot}_1-\rot_2\rot_3\dot{\rot}_3\\
+(\rot_1^2+\rot_2^2)\dot{\rot}_3-\rot_2\rot_3\dot{\rot}_2-\rot_1\rot_3\dot{\rot}_1 & 0 & -(\rot_2^2+\rot_3^2)\dot{\rot}_1+\rot_1\rot_2\dot{\rot}_2+\rot_1\rot_3\dot{\rot}_3\\
-(\rot_1^2+\rot_3^2)\dot{\rot}_2+\rot_1\rot_2\dot{\rot}_1+\rot_2\rot_3\dot{\rot}_3 & (\rot_2^2+\rot_3^2)\dot{\rot}_1-\rot_1\rot_2\dot{\rot}_2-\rot_1\rot_3\dot{\rot}_3 & 0
\end{array}\right)&
\end{eqnarray*}
}
\begin{eqnarray*}
\rdotcross\rot\rot^T-\rot\dot{\rot}^T\rcross{}&=&
\left[\begin{array}{c}
(\rot_2^2+\rot_3^2)\dot{\rot}_1-\rot_1\rot_2\dot{\rot}_2-\rot_1\rot_3\dot{\rot}_3\\
-\rot_1\rot_2\dot{\rot}_1+(\rot_1^2+\rot_3^2)\dot{\rot}_2-\rot_2\rot_3\dot{\rot}_3\\
-\rot_1\rot_3\dot{\rot}_1-\rot_2\rot_3\dot{\rot}_2+(\rot_1^2+\rot_2^2)\dot{\rot}_3\end{array}\right]_{\times}
\end{eqnarray*}
\begin{eqnarray*}
\left(\begin{array}{c}
(\rot_2^2+\rot_3^2)\dot{\rot}_1-\rot_1\rot_2\dot{\rot}_2-\rot_1\rot_3\dot{\rot}_3\\
-\rot_1\rot_2\dot{\rot}_1+(\rot_1^2+\rot_3^2)\dot{\rot}_2-\rot_2\rot_3\dot{\rot}_3\\
-\rot_1\rot_3\dot{\rot}_1-\rot_2\rot_3\dot{\rot}_2+(\rot_1^2+\rot_2^2)\dot{\rot}_3\end{array}\right) &=&
\left(\begin{array}{ccc}
\rot_2^2+\rot_3^2&-\rot_1\rot_2&-\rot_1\rot_3\\
-\rot_1\rot_2&\rot_1^2+\rot_3^2&-\rot_2\rot_3\\
-\rot_1\rot_3&-\rot_2\rot_3&\rot_1^2+\rot_2^2\end{array}\right) \dot{\rot}\\
\left(\begin{array}{ccc}
\rot_2^2+\rot_3^2&-\rot_1\rot_2&-\rot_1\rot_3\\
-\rot_1\rot_2&\rot_1^2+\rot_3^2&-\rot_2\rot_3\\
-\rot_1\rot_3&-\rot_2\rot_3&\rot_1^2+\rot_2^2\end{array}\right) &=& \normr^2 I_3 - \rot\rot^T
\end{eqnarray*}
Hence
\begin{eqnarray}\label{eq:ouf}
\rdotcross\rot\rot^T-\rot\dot{\rot}^T\rcross{}&=&
\normr^2\rdotcross - \left[\rot\rot^T\dot{\rot}\right]_{\times}
\end{eqnarray}
\end {document}
