In order to get meaningful information out of the sensors regarding the IMU’s position with respect to a static reference point, we have to set up a static coordinate system. In this article, we are using the NED, or North-East-Down, coordinate system. This can work by initializing two reference vectors that are static as two axes, and using the cross-product to determine the third axis. In this implementation, we are using gravity as our Downward Vector, $V_D$, and the North-Magnetic field as our North Vector, $V_N$. In order to maintain axis-orthogonality, we set the values of these vectors as follows:
$$ V_D = \begin{bmatrix} 0 \\ 0 \\ -1 \end{bmatrix} \qquad V_N = \begin{bmatrix} V_\mathrm{x} \\ V_\mathrm{y} \\ 0 \\ \end{bmatrix} $$
In order to ascertain the position of the IMU with regard to this coordinate system, we must use Rotation matrices to and from the Body-Frame and the NED-Frame.
The Rotation matrix from the Body-Frame (Sensor) to the NED-Frame (Reference) is shown below:
$$ R_b^n = \begin{bmatrix} c\theta c\psi & s\phi s\theta c\psi - c\phi c\psi & c\phi s\theta c\psi + s\phi s\psi \\ c\theta s\psi & s\phi s\theta s\psi + c\phi c\psi & c\phi s\theta s\psi - s\phi s\psi \\ -s\theta & s\phi c\theta & c\phi c\theta \\ \end{bmatrix} = \begin{bmatrix} (q_0^2 + q_1^2 - q_2^2 - q_3^2) & 2(q_1 q_2 - q_0 q_3) & 2(q_1 q_3 + q_0 q_2) \\ 2(q_1 q_2 + q_0 q_3) & (q_0^2 - q_1^2 + q_2^2 - q_3^2) & 2(q_2 q_3 - q_0 q_1) \\ 2(q_1 q_3 - q_0 q_2) & 2(q_2 q_3 + q_0 q_1) & (q_0^2 - q_1^2 - q_2^2 + q_3^2) \\ \end{bmatrix} $$
Or equivalently, a more computationally efficient matrix may be implemented:
$$ R_b^n = \begin{bmatrix} 1 - 2(q_2^2 + q_3^2) & 2(q_1 q_2 - q_0 q_3) & 2(q_1 q_3 + q_0 q_2) \\ 2(q_1 q_2 + q_0 q_3) & 1 - 2(q_1^2 + q_3^2) & 2(q_2 q_3 - q_0 q_1) \\ 2(q_1 q_3 - q_0 q_2) & 2(q_2 q_3 + q_0 q_1) & 1 - 2(q_1^2 + q_2^2) \\ \end{bmatrix} $$
Furthermore, the Rotation matrix from the NED-Frame (Reference) to the Body-Frame (Sensor) is shown below. This is also the transpose of the previous Rotation Matrix, $R_b^n$.
$$ R_n^b = (R_b^n)^{T} = \begin{bmatrix} c\theta c\psi & c\theta s\psi & -s\theta \\ s\phi s\theta c\psi - c\phi c\psi & s\phi s\theta s\psi + c\phi c\psi & s\phi c\theta \\ c\phi s\theta c\psi + s\phi s\psi & c\phi s\theta s\psi - s\phi s\psi & c\phi c\theta \\ \end{bmatrix} = \begin{bmatrix} (q_0^2 + q_1^2 - q_2^2 - q_3^2) & 2(q_1 q_2 + q_0 q_3) & 2(q_1 q_3 - q_0 q_2) \\ 2(q_1 q_2 - q_0 q_3) & (q_0^2 - q_1^2 + q_2^2 - q_3^2) & 2(q_2 q_3 + q_0 q_1) \\ 2(q_1 q_3 + q_0 q_2) & 2(q_2 q_3 - q_0 q_1) & (q_0^2 - q_1^2 - q_2^2 + q_3^2) \\ \end{bmatrix} $$
Or equivalently, the more computationally efficient matrix may be implemented.
$$ R_n^b = \begin{bmatrix} 1 - 2(q_2^2 + q_3^2) & 2(q_1 q_2 + q_0 q_3) & 2(q_1 q_3 - q_0 q_2) \\ 2(q_1 q_2 - q_0 q_3) & 1 - 2(q_1^2 + q_3^2) & 2(q_2 q_3 + q_0 q_1) \\ 2(q_1 q_3 + q_0 q_2) & 2(q_2 q_3 - q_0 q_1) & 1 - 2(q_1^2 + q_2^2) \\ \end{bmatrix} $$
The computation of the Quaternions in the Extended Kalman Filter are performed in the global NED-Frame, whereas the sensor readings are extracted in the local Body-Frame. Therefore, we will have to use these rotation matrices to compare and extract useful information from our algorithm.