# Math Derivation of Strapdown Inertial Navigation ###### tags: `2022 SDC course` :::info :bulb: This note will go through the math derivation of integrating IMU's angular velocity measurements. ::: --- > The orientation, or attitude, of an INS relative to the global frame of reference is tracked by **‘integrating’** the angular velocity signal $\omega_b(t)=(\omega_{bx}(t),\omega_{by}(t),\omega_{bz}(t))^T$ obtained from the system’s rate-gyroscopes. [original paper](https://www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.pdf) --- :::warning :rotating_light: **CALCULUS INVOLVED** ::: --- ## Prior Knowledge - Rotation Matrices :::info :bulb: Rotation matrix in 3D can be decomposed into rotations about x, y, and z axes. There are many ways to decompose the rotation: https://en.wikipedia.org/wiki/Euler_angles ::: --- $R_x = \begin{bmatrix} 1 & 0 & 0\\ 0 & \cos \phi & \sin \phi \\ 0 & -\sin \phi & \cos \phi \end{bmatrix}$ $R_y=\begin{bmatrix} \cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta \end{bmatrix}$ $R_z=\begin{bmatrix} \cos \psi & \sin \psi & 0\\ -\sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end{bmatrix}$ $R=R_x(\phi) R_y(\theta) R_z(\psi)$ --- :::warning :zap: It is important to note that the order in which the rotations are applied is important. :::  --- ## Notation - Rotation matrix: C (from body frame to global frame) - Direction vector in global frame: $v_g$ - Direction vector in body frame: $v_b$ $$v_g = C v_b$$ --- - $C(t)$ represents the rotation at time **t** - $C'(t)$ is the rate of change of $C$ at time $t$ $$ C'(t)=\lim_{\delta t \to 0} \frac{C(t+\delta t)-C(t)}{\delta t} $$ - $A(t)$ is the rotation change from $C(t)$ to $C(t+\delta t)$ $$ C(t+\delta t)=C(t)A(t) $$ $A(t)=R_x(\delta \phi)R_y(\delta \theta)R_z(\delta \psi)$ --- :::warning :zap: using small angle approximation $\cos \theta \approx 1,\sin \theta \approx \theta$ when $\theta$ is small. And multiply of two angles can be ignored. ::: $A(t)= I+ \delta \Psi$ where $\delta \Psi=\begin{bmatrix} 0 & -\delta \psi & \delta \theta \\ \delta \psi & 0 & -\delta \psi \\ -\delta \theta & \delta \phi & 0 \end{bmatrix}$ --- By substitution: $$C'(t)=\lim_{\delta t\to0} \frac{C(t)(I+\delta \Psi)-C(t)}{\delta t}=C(t)\lim_{\delta t \to 0}\frac{\delta \Psi}{\delta t}$$ $$ \lim_{\delta t \to 0}\frac{\delta \Psi}{\delta t} = \Omega(t) = \begin{bmatrix} 0 & -\omega_{bz}(t) & \omega_{by}(t) \\ \omega_{bz}(t) & 0 & -\omega_{bx}(t) \\ -\omega_{by}(t) & \omega_{bx}(t) & 0 \end{bmatrix} $$ --- Eventually, $C'(t)=C(t)\Omega(t)$ Solve the 1st order homogeneous *differential equation*: $$ \int^{t+\delta t}_t\frac{1}{C(t)} dC(t)=\int^{t+\delta t}_t \Omega(t)dt \\ C(t+\delta t)=C(t) \cdot \exp(\int^{t+\delta t}_t \Omega(t)dt) $$ --- Using the **rectangle rule** of calculus: $$ \int^{t+\delta t}_t \Omega(t)dt = \begin{bmatrix} 0 & -\omega_{bz}\delta t & \omega_{by}\delta t \\ \omega_{bz}\delta t & 0 & -\omega_{bx}\delta t \\ -\omega_{by}\delta t & \omega_{bx}\delta t & 0 \end{bmatrix} $$ --- Here denote $\int^{t+\delta t}_t \Omega(t)dt = B$, and use **Taylor expansion**. Rewrite: $$ C(t+\delta t)=C(t) \cdot \exp(B) \\ = C(t)(I+B+\frac{B^2}{2!}+\frac{B^3}{3!}+\frac{B^4}{4!}+... ) \\ = C(t)(I+B+\frac{B^2}{2!}-\frac{\sigma^2 B}{3!}-\frac{\sigma^2 B^2}{4!}+... ), \sigma=|\omega_b \delta t| $$ --- $$ = C(t)(I+(1-\frac{\sigma^2}{3!}+\frac{\sigma^4}{5!}+...)B+ \\ (\frac{1}{2!}-\frac{\sigma^2}{4!}+\frac{\sigma^4}{6!}+...)B^2 ) \\ = C(t)(I+\frac{\sin \sigma}{\sigma}B+\frac{1-\cos \sigma}{\sigma^2}B^2) $$