## 3 Methodology
### Trajectory Determination
#### 3.2.1 Process Model  
As in most strap-down navigators, the observations of linear acceleration \(a=\begin{bmatrix}a_{x}&a_{y}&a_{z}\end{bmatrix}\) and rotational velocity \(\omega=\begin{bmatrix}\omega_{p}&\omega_{q}&\omega_{r}\end{bmatrix}\) are made in the body frame by the IMU. The biases and noise within these observations are corrected for using the following models:  
\[\bar{a} =a-a_{b}-n_{a} \tag{3}\] \[\bar{\omega} =\omega-\omega_{b}-C_{n}^{b}\omega_{c}-n_{\omega} \tag{4}\]  
where \(\bar{a}\) and \(\bar{b}\) are the corrected observations and \(n_{a}\) and \(n_{\omega}\) are estimated noise terms within the measurements of acceleration (\(a\)) and angular rate (\(\omega\)). \(\omega_{c}\) is the rotational velocity of the Earth for a given longitude and latitude. The time varying noise terms \(a_{b}\) and \(\omega_{b}\) model both the bias and scale factor error terms of the IMU. These noise terms are modelled as a zero-mean Gaussian random variable with the variance set according to manufacturers specifications. A quaternion representation of orientation is used to avoid singularities which can occur in alternate representations of orientation, following [PERSON] [23], [PERSON] and [PERSON] [33], [PERSON] [35]. This results in a state vector with 16 elements as follows:  
\[x=\begin{bmatrix}p_{t}&v_{t}&e_{t}&a_{t}&\omega_{t}\end{bmatrix}^{T} \tag{5}\]where \(p_{t}=\begin{bmatrix}x&y&z\end{bmatrix}^{T}\) and \(v_{t}=\begin{bmatrix}v_{x}&v_{y}&v_{z}\end{bmatrix}^{T}\) are the position and velocity within a fixed mapping frame. \(e_{t}=\begin{bmatrix}q_{0}&q_{1}&q_{2}&q_{3}\end{bmatrix}^{T}\) is attitude quaternion representing the rotation between the body frame and the mapping frame. \(a_{t}\) and \(\omega_{t}\) are the three element accelerometer and gyroscope bias vectors. The mechanisation of the corrected IMU observations used as the kinematic process model to transform the measurements made in the body frame into changes in position and attitude in the mapping frame follows [PERSON] and [PERSON] [33]. The discrete time kinematic equations are given as follows:  
\[p_{t+1} =p_{t}+\dot{p_{t}}\cdot dt \tag{6}\] \[v_{t+1} =v_{t}+\dot{v_{t}}\cdot dt\] (7) \[e_{t+1} =exp(-\frac{1}{2}\tilde{\Omega}\cdot dt)e_{t}\] (8) \[a_{b_{t+1}} =a_{b_{t}}+r_{w_{t}}\cdot dt\] (9) \[\omega_{b_{t+1}} =\omega_{b_{t}}+w_{b_{t}}\cdot dt \tag{10}\]  
where \(r_{w_{t}}\) and \(w_{b_{t}}\) are zero mean Gaussian noise terms and the term \(exp(-\frac{1}{2}\tilde{\Omega}\cdot dt)\) is composed of a skew symmetric matrix representing the effective rotations in the body frame:  
\[\tilde{\Omega}=\begin{bmatrix}0&\bar{\omega}_{p}\cdot dt&\bar{\omega}_{q}\cdot dt &\bar{\omega}_{r}\cdot dt\\ -\bar{\omega}_{p}\cdot dt&0&-\bar{\omega}_{r}\cdot dt&\bar{\omega}_{q}\cdot dt \\ -\bar{\omega}_{q}\cdot dt&\bar{\omega}_{r}\cdot dt&0&-\bar{\omega}_{p}\cdot dt \\ -\bar{\omega}_{r}\cdot dt&\bar{\omega}_{q}\cdot dt&\bar{\omega}_{r}\cdot dt&0 \end{bmatrix} \tag{11}\]  
Based on the proofs provided by [PERSON] [36] and [PERSON] [37], the matrix exponent and the skew symmetric property can be used in forming a closed form solution as follows:  
\[exp(-\frac{1}{2}\tilde{\Omega}\cdot dt)=\left[I(cos(s)+j\cdot dt\lambda)- \frac{1}{2}\tilde{\Omega}\frac{sin(s)}{s}\right]e_{k} \tag{12}\]  
where \(s=\frac{1}{2}||\begin{bmatrix}\bar{\omega}_{p}\cdot dt&\bar{\omega}_{q}\cdot dt &\bar{\omega}_{r}\cdot dt\end{bmatrix}||\). The term \(j\cdot dt\lambda\) serves as a Lagrange multiplier to ensure the unity norm constraint of the quaternion orientation representation, given \(\lambda=1-|e_{k}|^{2}\) and \(j\) is the factor that determines the convergence speed of the numerical error.