公式推導 設一個 $q(t)$ 為一個隨時間 $t$ 變動的單位四元數函數，並且在時間 $t$ 以絕對坐標系表示的瞬時角速度為 $\omega(t)$，可簡記為 $\omega$ 因此在極短的時間片段 $\Delta t$ ，物體進行一個沿單位向量$\hat\omega$ 旋轉角度$\Delta \theta = ||\omega|| \Delta t$ 的旋轉運動 並且可以以單位四元數將運動描述為 $$ \begin{align} \Delta q &= cos\frac{\Delta \theta}{2} + \hat \omega sin \frac{\Delta \theta}{2} \\ &= cos\frac{||\omega||\Delta t}{2} + \hat \omega sin \frac{||\omega||\Delta t}{2} \end{align} $$ 於是在時間 $t + \Delta t$ 的單位四元數函數可表示為我們可以取得 q(t)) $$ q(t + \Delta t) = \Delta q\text { }q(t) $$ 我們可以取得 $q(t)$ 的瞬時變化量

Kalman Filter Start from Recursive Bayes Filter Basic Assumptions Observation and state transition matrices are linear: $$ \left\{ \begin{matrix} x_t &=& A_tx_{t-1}+B_tu_t + \omega_t\\ z_t &=& C_tx_t+\nu_t \end{matrix} \right. $$ All of the noises are Gaussian distributed: $$ \omega_t \sim \mathcal N(0, R_t)\\ \nu_t \sim \mathcal N(0, Q_t) $$ Time Update (Prediction) Noted that: The belief $bel(x_{t-1})$ obeys Normal distribution $\mathcal N(x_{t-1};\mu_{t-1}, \Sigma_{t-1})$ The motion model $p(x_t|x_{t-1}, u_t)$ obeys Normal distribution $\mathcal N(x_t;A_tx_{t-1}+B_tu_t, R_t)$ The prediction step can be represented as:

Recursive Bayes Filter Estimate the state $x$ of a system given observations $z$ and controls $u$ : Bayes Filter $$ \begin{align*} bel(x_t) &= p(x_t|z_{1:t}, u_{1:t})= \frac{p(z_t|x_t,z_{1:t-1},u_{1:t})p(x_t|z_{1:t-1},u_{1:t})}{p(z_t|z_{1:t-1},u_{1:t})}&\text {(Bayes' rule)} \\ &=\eta p(z_t|x_t,z_{1:t-1},u_{1:t})p(x_t|z_{1:t-1},u_{1:t}) \\ &=\eta p(z_t|x_t)p(x_t|z_{1:t-1},u_{1:t}) &\text { (Markov assumption)} \\&=\eta p(z_t|x_t) \int p(x_t|x_{t-1},z_{1:t-1},u_{1:t})p(x_{t-1}|z_{1:t-1}, u_{1:t})dx_{t-1} &\text { (Law of total probablility)} \\&=\eta p(z_t|x_t) \int p(x_t|x_{t-1},u_{t})p(x_{t-1}|z_{1:t-1}, u_{1:t})dx_{t-1} &\text { (Markov assumption)} \\&=\eta p(z_t|x_t) \int p(x_t|x_{t-1},u_{t})p(x_{t-1}|z_{1:t-1}, u_{1:t-1})dx_{t-1} &\text { (Independence assumption)} \end{align*} $$ So that Bayes filter can be written as two step process:

從羅德里格旋轉公式開始 向量 $\vec x’$ 為向量 $\vec x$ 以單位向量 $\vec n$ 為旋轉軸旋轉角度 $\psi$ 所得到的向量 rodrigue's rotation 如圖可得 $$ \begin{align} \vec{v_2} &= \cos \psi \vec{v_1} - \sin \psi \vec{v_3} \\ \vec{v_1} &= \vec{x} - (\vec{n} \cdot \vec{x})\vec{n} \\ \vec{v_3} &= \vec{v_1} \times \vec{n} \\ &= (\vec{x} - (\vec{n} \cdot \vec{x})\vec{n}) \times \vec{n} \\ &= (\vec x \times \vec n) - (\vec n \cdot \vec x) \mathop {\cancel{{\vec n \times \vec n}}}\limits_0 \\ &= \vec x \times \vec n \end{align} $$ 最後可將 $\vec x’$ 表示為