참고자료 : Quaternion kinematics for the error-state Kalman filter

---

1. 쿼터니언의 정의 및 성질

쿼터니언은 $a+bi$의 형태로 나타내는 복소수를 확장한 것으로 볼 수 있다.
$q = q_w + q_xi + q_yj + q_zk \in H$
$where \space \{q_w, q_x, q_y, q_z\} \in R \space and \space \{i,j,k\} \space \space is \space imaginary \space unit \space numbers$

쿼터니언의 유용한 점 중 하나는 norm = 1인 unit quaternion을 이용해 3D 공간 상의 회전을 기술할 수 있다는 것이다. 이는 unit complex number을 이용해 2D rotation을 기술하는 것을 확장한 것으로 보면 편하다.

unit quaternion : $q_u=e^{(u_xi+u_yj+u_zk)\theta/2}$
point rotation : $x'=q_u\otimes x \otimes q_u^*$

general quaternion : $q=q_w + q_v = \begin{bmatrix}q_w\\q_v\end{bmatrix}=\begin{bmatrix}q_w\\q_x\\q_y\\q_z\end{bmatrix}$

real quaternion : $q_w=\begin{bmatrix}q_w\\0\end{bmatrix}$

pure quaternion : $q_v=\begin{bmatrix}0\\q_v\end{bmatrix}$

quaternion sum : $p+q=\begin{bmatrix}p_w\pm q_w\\p_v \pm q_v\end{bmatrix}$

quaternion product $p\otimes q=\begin{bmatrix}p_wq_w-p_v^Tq_v\\p_wq_v+q_wp_v+p_v\times q_v\end{bmatrix}$

quaternion product를 행렬 연산으로 대체하기 위해 left/right quaternion product matrix를 정의할 수 있다.
$q_1\otimes q_2=[q_1]_Lq_2=[q_2]_Rq_1$

$[q]_L=q_wI_4+\begin{bmatrix}0_3&-q_v^T\\q_v&[q_v]_\times\end{bmatrix}$

$[q]_R=q_wI_4+\begin{bmatrix}0_3&-q_v^T\\q_v&-[q_v]_\times\end{bmatrix}$

skew operator : $[w]_\times=\begin{bmatrix}0&-w_z&w_y\\w_z&0&-w_x\\-w_y&w_x&0\end{bmatrix}, \quad [a]_\times b=a\times b$

identity quaternion : $q_1=1=\begin{bmatrix}1\\0_v\end{bmatrix}$

conjugate of quaternion : $q^*=q_w-q_v=\begin{bmatrix}q_w\\-q_v\end{bmatrix}$

norm of quaternion : $||q||=\sqrt{q\otimes q^*}=\sqrt{q_w^2+q_x^2+q_y^2+q_z^2}$

inverse of quaternion : $q^{-1}=q^*/||q||^2,\quad q\otimes q^*=q^* \otimes q=q_1$

unit quaternion : $||q||=1, \space q^{-1}=q^*,\space q=\begin{bmatrix}cos\theta\\u*sin\theta\end{bmatrix}$ (u 는 unit vector)

exponential of pure quaternion
pure quaternion $v=v_xi+v_yj+v_zk$
pure quaternion의 exponential은 unit quaternion이다.
$e^v=e^{u\theta}=\begin{bmatrix}cos\theta\\u*sin\theta\end{bmatrix}$

exponential of general quaternion
$e^q=e^{q_w}e^{q_v}=e^{q_w}e^{u\theta}$
$e^q=e^q_w\begin{bmatrix}cos||q_v||\\\frac{q_v}{||q_v||}sin||q_v||\end{bmatrix}$

logarithm of unit quaternion
$log\space q=log(cos\theta+u\space sin\theta)=log(e^{u\theta})=u\space \theta$
unit quaternion의 logarithm은 pure quaternion이다.
여기서 unit quaternion q로부터 angle-axis를 얻는 과정은 아래와 같다.
axis : $u=q_v\||q_v||$
angle : $\theta=arctan(||q_v||,q_w)$

logarithm of general quaternion
$log\space q=log||q||+u\theta=\begin{bmatrix}log||q||\\u\theta\end{bmatrix}$

unit quaternion의 거듭제곱 : $q^t=exp(t\space u\space \theta)=\begin{bmatrix}cos(t\theta)\\u\space sin(t\theta)\end{bmatrix}$

2. rotation과의 상호 관계

vector $x$를 axis $u$에 대해 $\phi$만큼 회전하는 경우.
$x$를 $u$와 평행한 부분과 수직한 부분으로 나눠 생각할 수 있다.
$x=x_{\parallel}+x_{\bot}$
$x_{\parallel}=uu^Tx,\quad x_{\bot}=x-uu^Tx$

$x'=x_{\parallel}+x_{\bot}cos\phi+(u\times x)sin\phi$

exponential map of rotation matrix
$R=Exp(\phi)=exp([\phi]_{\times})$

여기서 vector $\phi \in R^3$는 rotation vector 또는 angle-axis vector로 불리며, angle $\phi \in R$와 axis $u$로 분해될 수 있다.

Rodrigues rotation formula
$R=e^{[\phi]_{\times}}=e^{\phi[u]_{\times}}=I_3cos\phi+[u]_{\times}sin\phi+uu^T(1-cos\phi)$

logarithmic map of rotation matrix
$log(R)=\phi[u]_{\times}$
with $\phi=arccos(\frac{tr(R)-1}{2}),\quad u=\frac{(R-R^T)^{\vee}}{2sin\phi}$

$Log(R)=u\phi$

rotation using rotation matrix
$x'=Rx=Exp(u\phi)\space x=x_{\parallel}+(u\times x)sin\phi+x_{\bot}cos\phi$

exponential map
pure quaternion $V \in H_p$에 대해 pure quaternion에서 unit quaternion으로의 매핑 exp는 아래와 같다.
$exp(V)=e^V$

rotation vector $\phi \in R^3$에 대해 $R^3$로부터 unit quaternion으로의 매핑 Exp는 아래와 같다.
$Exp(\phi)=exp(\phi/2)=e^{\phi/2}$

이로부터 rotation벡터에 대응되는 unit quaternion을 얻을 수 있다.
$q=Exp(\phi u)=e^{\phi u/2}=\begin{bmatrix}cos(\phi/2)\\u\space sin(\phi/2)\end{bmatrix}$

logarithmic map
$log(q)=u\theta=u\phi/2$

$Log(q)=u\phi$

unit quaternion으로부터 angle-axis 값을 아래와 같이 얻을 수 있다.

$\phi=2arctan(||q_v||, q_w)$
$u=q_v/||q_v||$

rotation using quaternion

$r(x)=q\otimes x\otimes q^*=x_{\parallel}+(u\times x)sin\phi+x_{\bot}cos\phi$

2.5. rotation matrix와 quaternion

$\phi,x \in R^3, \quad q=Exp(\phi), \quad R=Exp(\phi)$
then $q\otimes x \otimes q^*=Rx$

$R=(q_w^2-q_v^Tq_v)I_3+2q_vq_v^T+2q_w[q_v]_\times$

2.6. rotation composition

$q_{AC}=q_{AB}\otimes q_{BC}, \quad R_{AC}=R_{AB}R_{BC}$

3. 쿼터니언 컨벤션

쿼터니언을 표현하는 2가지 컨벤션이 존재한다.

Hamilton type : $q=(q_w,q_v)$
JPL type : $q=(q_v,q_w)$

C++ Eigen library의 경우 Hamilton type(w, x, y, z 순서)을 사용한다.
하지만 주의할 점은 실제 data가 저장되는 순서는 JPL type을 따른다는 것이다.
따라서 array data를 이용해 Quaternion의 생성자를 호출하는 경우 JPL type의 순서로 저장이 된다.

또한 q.coeffs() 메서드를 사용해서 값을 받아왔을 때도 JPL type(x, y, z, w 순서)으로 값이 저장되어있다.

여기서는 Hamilton 타입을 사용한다.

Active vs Passive
Active interpretation : vector를 회전.
$x'=q_{active}\otimes x \otimes q_{active}^*$
$x'=R_{active}x$

Passive interpretation : vector는 그대로이고, 좌표계가 회전. frame transformation.
$x_B=q_{passive} \otimes x_{A} \otimes q_{passive}^*$
$x_{B}=R_{passive}x_{A}$

$A, B$는 각각의 Cartesian reference frame이고, $x_{A},x_{B}$는 vector $x$를 각 좌표계에서 표현한 것.

아래와 같은 관계를 가진다.
$q_{active}=q_{passive}^*$
$R_{active}=R_{passive}^T$

4. Perturbation / Derivative / Integral

4.1. additive, subtractive operators in SO3

$q_S=q_R \oplus \theta=q_R \otimes Exp(\theta)$
$R_S=R_R \oplus \theta=R_R Exp(\theta)$

$\theta=q_S \ominus q_R=Log(q_R^* \otimes q_S)$
$\theta=R_S \ominus R_R=Log(R_R^TR_S)$

4.3. Jacobians of the rotation

Jacobian respect to vector

$\frac{\partial(q\otimes a \otimes q^*)}{\partial a}=\frac{\partial (Ra)}{\partial a}=R$

Jacobian respect to quaternion

$\frac{\partial(q \otimes a \otimes q^*)}{\partial q}=2[wa+v\times a | v^TaI_3+va^T-av^T-w[a]_\times] \in R^{3\times 4}$

Right Jacobian of SO(3)

rotation vector $\theta \in R^3$이 $\delta \theta$만큼 변화할 때,
SO(3)의 탄젠트 공간에서 R의 변화를 rotation vector $\delta \phi \in R^3$를 이용해 표현하면 아래와 같다.

$Exp(\theta) \otimes \delta \phi=Exp(\theta+\delta \theta)$

$\delta \phi =Log(Exp(\theta)^{-1} \circ Exp(\theta+\delta \theta))=Exp(\theta+\delta \theta) \ominus Exp(\theta)$

Right Jacobian : $J_r=\frac{\partial \delta \phi}{\partial \delta \theta}=\frac{\partial Exp(\theta)}{\partial \theta}$

Right Jacobian의 closed form은 아래와 같다.

Right Jacobian은 아래와 같은 특성을 가진다.
$Exp(\theta+\delta \theta) \approx Exp(\theta)Exp(J_r(\theta)\delta \theta)$
$Exp(\theta)Exp(\delta \theta) \approx Exp(\theta+J_r^{-1}(\theta)\delta \theta)$
$Log(Exp(\theta)Exp(\delta \theta)) \approx \theta + J_r^{-1}(\theta)\delta \theta$

Jacobian respect to rotation vector
$\frac{\partial (q \otimes a \otimes q^*)}{\partial \delta \theta}=\frac{\partial (Ra)}{\partial \delta \theta}=-R{\theta}[a]_\times J_r(\theta)$

4.4 Perturbation / Uncertainty / Noise

local perturbation
$\tilde{q}=q \otimes \Delta q_{L}$
$\tilde{R}=R \Delta R_L$

local perturbation을 rotation vector form으로 표현하면,

$\tilde{q_L}=q_L \otimes Exp(\Delta\phi_L)$
$\tilde{R_L}=R_L Exp(\Delta \phi_L)$

$\Delta \phi_L=Log(q_L^* \otimes \tilde{q_L})=Log(R_L^T\tilde{R}_L)$

perturbation angle이 작은 경우 perturbation의 quaternion / rotation matrix form은 아래와 같이 근사할 수 있다.

$\Delta q_L \approx \begin{bmatrix} 1 \\ \frac{1}{2}\Delta\phi_L \end{bmatrix}$
$\Delta R_L \approx I + [\Delta\phi_L]_\times$

global perturbation

$\tilde{q}_G=Exp(\Delta\phi_G)\otimes q_G$
$\tilde{R}_G=Exp(\Delta\phi_G)R_G$

$\Delta\phi_G=Log(\tilde{q}_G\otimes q_G^*)=Log(\tilde{R}_GR^T_G)$

4.5. Time derivatives

$w_L(t)=\frac{d\phi_L(t)}{dt}$

$\dot{q}=\lim\limits_{\Delta t \to 0}\frac{q(t+\Delta t)-q(t)}{\Delta t}=\frac{1}{2}q \otimes \begin{bmatrix}0 \\ w_L\end{bmatrix}$

$\dot{q}=\frac{1}{2}[w_L]_R*q=\frac{1}{2}q\otimes w_L$

$\dot{R}=R[w_L]_\times$

global perturbation을 사용하면

$\dot{q}=\frac{1}{2}w_G\otimes q$

$\dot{R}=[w_G]_\times R$

4.6. Time integral of rotation rate(angular velocity)

angular velocity 가 변하지 않는다고 가정하면
$q_{n+1}=q_n \otimes q\{w_n \Delta t\}$