角速度描述
四元数微分
记 \(q(t)\) 为时刻 \(t\) 下的旋转姿态，\(q(t+\Delta t)=q(t)\otimes\left[\begin{matrix}\cos\frac{\omega \Delta t}2\\{\mathbf v}\sin\frac{\omega\Delta t}2\end{matrix}\right]\)
\[
\begin{aligned}
\frac{{\rm d}q(t)}{{\rm d}t}&=\lim_{\Delta t\to0}\frac{q(t+\Delta t)-q(t)}{\Delta t}\\
&=\lim_{\Delta t\to0}\frac{q(t)\otimes\left[\begin{matrix}\cos\frac{\omega \Delta t}2\\{\mathbf v}\sin\frac{\omega \Delta t}2\end{matrix}\right]-q(t)\otimes\left[\begin{matrix}1\\{\mathbf 0}\end{matrix}\right]}{\Delta t}\\
&=\lim_{\Delta t\to0}\frac{q(t)\otimes\left[\begin{matrix}\cos\frac{\omega \Delta t}2-1\\{\mathbf v}\sin\frac{\omega \Delta t}2\end{matrix}\right]}{\Delta t}\\
&=q(t)\otimes\lim_{\Delta t\to0}\frac{\left[\begin{matrix}\cos\frac{\omega \Delta t}2-1\\{\mathbf v}\sin\frac{\omega \Delta t}2\end{matrix}\right]}{\Delta t}\\
&=q(t)\otimes\left[\begin{matrix}0\\\frac12{\mathbf v}\omega\end{matrix}\right]\\
&=q(t)\otimes\left[\begin{matrix}0\\\frac12{\mathbf w}\end{matrix}\right]
\end{aligned}
\]
李群基础
记 \(R(t)\) 为随时间变化的旋转矩阵
指数映射
在李群 \(SO(3)\) 中任意 \(R\) 与李代数 \({\mathfrak so}(3)\) 中一个元素 \(\phi\) 对应
\[
R=\exp(\phi)=\sum_{n=0}^\infty\frac{\phi_\times^n}{n!}
\]
构造反对称矩阵 \(\phi_\times\)
\[
\phi_\times=\begin{pmatrix}0&-\phi_z&\phi_y\\\phi_z&0&-\phi_x\\-\phi_y&\phi_x&0\end{pmatrix}
\]
记 \(\hat\omega_\times=\frac{\phi_\times}{||\phi_\times||},\theta=||\phi_\times||\)
利用叉积的性质，有
\[
\begin{aligned}
\hat\omega_\times^3&=-\hat\omega_\times \\
\hat\omega_\times^4&=-\hat\omega_\times^2 \\
\hat\omega_\times^5&=\hat\omega_\times \\
\hat\omega_\times^6&=\hat\omega_\times^2 \\
\end{aligned}
\]
则
\[
\begin{aligned}
R&=\exp(\phi)\\
&=I+(\theta-\frac{\theta^3}{3!}+\frac{\theta^5}{5!}+\cdots)\hat\omega_\times+(\frac{\theta^2}{2!}-\frac{\theta^4}{4!}+\frac{\theta^6}{6!}-\cdots)\hat\omega_\times^2\\
&=I+\frac{\sin\theta}\theta\phi_\times+\frac{1-\cos\theta}{\theta^2}\phi_\times^2
\end{aligned}
\]
对数映射
\[
\phi=\log(R)
\]
\[
\begin{aligned}
{\rm tr}R&={\rm tr}I+\frac{\sin\theta}\theta{\rm tr}\phi_\times+\frac{1-\cos\theta}{\theta^2}{\rm tr}\phi_\times^2\\
&=3+0+\frac{1-\cos\theta}{\theta^2}(-2||\phi_\times||^2)
\end{aligned}
\]
\[
\cos\theta=\frac{{\rm tr}R-1}2
\]
\[
\theta=\arccos\frac{{\rm tr}R-1}2+2k\pi
\]
由李群性质，一个旋转矩阵可以被分为对称部分和反对称部分。当 \(\theta\neq0\) 且 \(\theta\to0\) 时
\[
R=\underbrace{\frac{\sin\theta}\theta\phi_\times}_{\frac12(R-R^T)}+\underbrace{I+\frac{1-\cos\theta}{\theta^2}\phi_\times^2}_{\frac12(R+R^T)}
\]
\[
\phi_\times=\frac{\theta(R-R^T)}{2\sin\theta}
\]
由旋转矩阵绕任意轴 \(\bf n\) 旋转 \(\theta\) 的表达式
\[
R=\begin{pmatrix}n_x^2(1-\cos\theta)+\cos\theta & n_xn_y(1-\cos\theta)-n_z\sin\theta & n_xn_z(1-\cos\theta)+n_y\sin\theta \\ n_yn_x(1-\cos\theta)+n_z\sin\theta & n_y^2(1-\cos\theta)+\cos\theta & n_yn_z(1-\cos\theta)-n_x\sin\theta \\ n_zn_x(1-\cos\theta)-n_y\sin\theta & n_zn_y(1-\cos\theta)+n_x\sin\theta & n_z^2(1-\cos\theta)+\cos\theta\end{pmatrix}
\]
可得
\[
\begin{aligned}
\phi_\times&=\frac{\theta}{2\sin\theta}\begin{pmatrix}0 & -2n_z\sin\theta & 2n_y\sin\theta \\ 2n_z\sin\theta & 0 & -2n_x\sin\theta \\ -2n_y\sin\theta & 2n_x\sin\theta & 0\end{pmatrix}\\ \\
&=\begin{pmatrix}0 & -n_z\theta & n_y\theta \\ n_z\theta & 0 & -n_x\theta \\ -n_y\theta & n_x\theta & 0\end{pmatrix}
\end{aligned}
\]
因此
\[
\begin{aligned}
\phi_x &= n_x\theta=\frac{\theta(r_{32}-r_{23})}{2\sin\theta} \\
\phi_y &= n_y\theta=\frac{\theta(r_{13}-r_{31})}{2\sin\theta} \\
\phi_z &= n_z\theta=\frac{\theta(r_{21}-r_{12})}{2\sin\theta} \\
\end{aligned}
\]
Baker-Campbell-Hausdorff公式
\[
\begin{aligned}
\log(\exp(\alpha)\exp(\beta))&=\sum_{n=1}^\infty\frac{(-1)^{n-1}}n\sum_{r_i+s_i>0,i\in[1,n]}\frac{(\sum_{i=1}^n(r_i+s_i))^{-1}}{\Pi_{i=1}^n(r_i!s_i!)}[A^{r_1}B^{s_1}A^{r_2}B^{s_2}\cdots A^{r_n}B^{s_n}]\\
&=A+B+\frac12[A,B]+\frac1{12}[A,[A,B]]-\frac1{12}[B,[A,B]]+\cdots
\end{aligned}
\]
对李括号
\[
[A,B]=AB-BA
\]
\(\log(\exp(\alpha)\exp(\beta))\) 的一阶近似为
\[
\log(AB)≈
\begin{cases}
J_l(\beta)^{-1}\alpha+\beta,&\alpha\to0\\
J_r(\alpha)^{-1}\beta+\alpha,&\beta\to0\\
\end{cases}
\]
旋转矩阵的微分
扰动模型
记 \(R(t)\) 为时刻 \(t\) 的旋转表示，\(R(t+\Delta t)=R(t)\exp(\omega\Delta t)\)
\[
\begin{aligned}
\frac{{\rm d}R(t)}{{\rm d}t}&=\lim_{\Delta t\to0}\frac{R(t+\Delta t)-R(t)}{\Delta t}\\
&=\lim_{\Delta t\to0}\frac{R(t)\exp(\omega\Delta t)-R(t)}{\Delta t}\\
&=\lim_{\Delta t\to0}\frac{R(t)[I+\frac{\sin(||\omega_\times\Delta t||)}{||\omega_\times\Delta t||}\omega_\times\Delta t+\frac{1-\cos(||\omega_\times\Delta t||)}{||\omega_\times\Delta t||^2}(\omega_\times\Delta t)^2]-R(t)}{\Delta t}\\
&=\lim_{\Delta t\to0}\frac{R(t)[\frac{\sin(||\omega_\times\Delta t||)}{||\omega_\times\Delta t||}\omega_\times\Delta t+\frac{1-\cos(||\omega_\times\Delta t||)}{||\omega_\times\Delta t||^2}(\omega_\times\Delta t)^2]}{\Delta t}\\
&=R(t)\omega_\times
\end{aligned}
\]
由于 \(RR^T=I\)
则
\[
\dot R(t)R(t)^T+R(t)\dot R(t)^T=0
\]
若利用 \(R^TR=I\)
则得到
\[
\dot R(t)^TR(t)+R(t)^T\dot R(t)=0
\]
于是
\[
R(t)^T\dot R(t)
=-\left[R(t)^T\dot R(t)\right]^T
\]
观察到 \(R(t)^T\dot R(t)\) 为反对称矩阵，因此总能找到 \(r(t)_\times=R(t)^T\dot R(t)\)
对等式两边同时左乘 \(R(t)\)
\[
R(t)R(t)^T\dot R(t)=\dot R(t)=R(t)[r(t)]_\times
\]
对右乘模型而言，\(r(t)_\times=\omega_\times\)
微分模型
记 \(R(t)\) 为时刻 \(t\) 的旋转表示，\(R(t+\Delta t)-R(t)=\exp(\phi+\omega\Delta t)-\exp(\phi)\)
\[
\begin{aligned}
\frac{{\rm d}R(t)}{{\rm d}t}&=\lim_{\Delta t\to0}\frac{R(t+\Delta t)-R(t)}{\Delta t}\\
&=\lim_{\Delta t\to0}\frac{\exp(\phi+\omega\Delta t)-\exp(\phi)}{\Delta t}\\
&=\lim_{\Delta t\to0}\frac{(\phi+\omega\Delta t)^\wedge-\phi^\wedge}{\Delta t}\\
&=\lim_{\Delta t\to0}\frac{\Delta t\cdot\omega^\wedge}{\Delta t}\\
&=\omega^\wedge
\end{aligned}
\]
旋转点的右乘雅可比
记存在空间点 \(p=[x,y,z]^T\)，经过 \(R\) 旋转后为 \(Rp\)
\[
\begin{aligned}
\frac{\partial Rp}{\partial\phi}&=\lim_{\phi\to0}\frac{R\exp(\phi)p-Rp}{\phi}\\
&=\lim_{\phi\to0}\frac{R\phi^\wedge p}{\phi}\\
&=\lim_{\phi\to0}\frac{-Rp^\wedge\phi}{\phi}\\
&=-Rp^\wedge
\end{aligned}
\]
\[
\begin{aligned}
\frac{\partial Rp}{\partial\phi}&=\lim_{\delta\phi\to0}\frac{\exp(\phi+\delta\phi)p-\exp(\phi)p}{\delta\phi}\\
&=\lim_{\delta\phi\to0}\frac{\exp(\phi)\left[\exp(J_r\delta\phi)-I\right] p}{\delta\phi}\\
&=\lim_{\delta\phi\to0}\frac{R(J_r\delta\phi)^\wedge p}{\delta\phi}\\
&=\lim_{\delta\phi\to0}\frac{-Rp^\wedge J_r\delta\phi}{\delta\phi}\\
&=-Rp^\wedge J_r
\end{aligned}
\]