Implementation

System Inputs

The inputs into the EKF system include:

$$ \begin{aligned} &\text{1. Gyroscope - $G_{x}$, $G_{y}$, $G_{z}$} \\ &\text{2. Accelerometer - $A_x$, $A_y$, $A_z$} \\ &\text{3. Magnetometer - $M_x$, $M_y$, $M_z$} \\ \end{aligned} $$

The Gyroscope readings are converted from $\frac{deg}{sec}$ into $\frac{rad}{sec}$, this conversion must be applied to the calibrated bias/offset of each axis as well. We can then assign the Gyroscope values to the $u_\mathrm{k-1}$ vector.

The Accelerometer measurements are recorded from the sensor then Calibrated using the Algorithm found in Section 1 - Accelerometer. Afterwards, the values are normalized in order to obtain purely directional components. A similar procedure is applied to the Magnetometer readings after applying its Calibration Algorithm found in Section 1 - Magnetometer, however normalization is performed after other processing of the data.

The roll and pitch of the IMU are calculated using the following equations:

$$ \phi_{A} = atan2 \left( \frac{A_{y}}{A_{z}} \right) \qquad \theta_{A} = \arcsin \left( \frac{A_x}{\sqrt{A_{x}^{2} + A_{y}^{2} + A_{z}^{2}}} \right) $$

In order to obtain the euler angles of Roll ($\phi$), Tilt ($\theta$), and Yaw ($\psi$), we must understand how the accelerometer and gyroscope affect their calculations. For example, when tilting the Grove IMU 10DOF v2.0 board upward, with the accelerometer’s +x-axis pointing away from me, the gyroscope’s y-axis outputs a negative value. Furthermore, when rolling the IMU board counter-clockwise while pointed in the same direction as before, the gyroscope’s x-axis outputs a negative value. Therefore, these must be taken into consideration when applying any filters or performing other calculations that include changes in angles. A simple complimentary filter implemented to stabilize the roll and pitch are as follows:

$$ \phi = 0.65*\left( \phi - G_x dt \right) + 0.35*\phi_\mathrm{A_x} $$

$$ \theta = 0.95*\left( \theta - G_y dt \right) + 0.05*\theta_\mathrm{A} $$

Complimentary angles are easy to implement and produce reasonable results. However, for more robust systems, the complimentary filter is not good enough, therefore we’re implementing an Extended Kalman Filter.

The key to a robust orientation system is to establish a static reference frame, which can be done according to many different AHRS (Attitde and Heading Reference Systems) coordinate systems, such as:

1. World Geodetic System Frame (WGS)
2. Earth-Centered-Earth-Fixed Frame(ECEF)
3. North-East-Down Frame (NED)
4. North-East-Up Frame (NEU)

In our implementation, we will be using the North-East-Down (NED) Frame system. Therefore, we are able to measure two of the three key reference axis of the NED system, specifically North(N) and (D)Down, using our Accelerometer(Gravity-Down) and Magnetometer(Magnetic Field-North). In order to get meaningful information from the Accelerometer and Magnetometer, we must understand how the Gravity and North Magnetic field reference vectors are related. To begin, the Accelerometer records measurement in the local Body-Frame. However, we know that gravity is a constant force acting in the downwards direction of the Global Refernce Frame. Therefore, we can use this fact to simplify generating Rotation Matrices, $R^n_b$, for the sensor from the Body-Frame(Local) to the NED-Frame(Global) by using the following $V_D^{ref}$ vector.

This same concept of rotating the sensor measurements to the reference vector can be applied to the Magnetometer’s reading of the North Magnetic Field. Recall, that due to Axis-Orthogonality constraints, the Magnetic Reference value cannot have a z-axis component, therefore we can zero-out the z-value. Once a Northward reference vector, $V_N^{ref}$, has been determined, a rotation matrix can be applied to the sensor’s Body-Frame to generate the sensors location in the NED-Frame, in order to satisfy the z-axis zeroing restraint, we zero out the z-axis while the sensor reading is rotated to the NED-Frame. Next, we normalize the vector while in the NED-Frame to obtain purely directional measurements, and rotate the readings back to the Body-Frame.

$$ V^{ref}_D = g \begin{bmatrix} \phantom{-}0 \\ \phantom{-}0 \\ -1 \\ \end{bmatrix} \qquad \qquad V^{ref}_N = \begin{bmatrix} M_x \\ M_y \\ 0 \\ \end{bmatrix} $$

Quaternion Initialization

In order to minimize the amount of time for convergence to a solution, and to avoid the system from diverging, we can initialize the Quaternion by using an averaging scheme as shown below. We first initialize the state quaternion, $x^+_\mathrm{0}$ by iteratively calculating the average over $N$ samples using the Euler Angle to Quaternion conversion formula from Section 4 - Euler-Quaternion Conversion.

$$ x^+_0 = E[x_0] = \frac{1}{N}\sum_{i=0}^{N}x_i $$

$$ x_k = \begin{bmatrix} \boldsymbol{q} \\[0.5em] b_x \\[0.5em] b_y \\[0.5em] b_z \end{bmatrix} = \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \\ b_x \\ b_y \\ b_z \end{bmatrix} \qquad \boldsymbol{q} = \begin{bmatrix} q_0 \\[0.5em] q_1 \\[0.5em] q_2 \\[0.5em] q_3 \\[0.5em] \end{bmatrix} = \begin{bmatrix} \cos{\frac{\phi}{2}} \cos{\frac{\theta}{2}} \cos{\frac{\psi}{2}} + \sin{\frac{\phi}{2}} \sin{\frac{\theta}{2}} \sin{\frac{\psi}{2}} \\[0.5em] \sin{\frac{\phi}{2}} \cos{\frac{\theta}{2}} \cos{\frac{\psi}{2}} - \cos{\frac{\phi}{2}} \sin{\frac{\theta}{2}} \sin{\frac{\psi}{2}} \\[0.5em] \cos{\frac{\phi}{2}} \sin{\frac{\theta}{2}} \cos{\frac{\psi}{2}} + \sin{\frac{\phi}{2}} \cos{\frac{\theta}{2}} \sin{\frac{\psi}{2}} \\[0.5em] \cos{\frac{\phi}{2}} \cos{\frac{\theta}{2}} \sin{\frac{\psi}{2}} - \sin{\frac{\phi}{2}} \sin{\frac{\theta}{2}} \cos{\frac{\psi}{2}} \\[0.5em] \end{bmatrix} $$

Where $\phi$, $\theta$, $\psi$, are derived from the Accelerometer, Magnetometer and Gyroscope measurements, found in Section 2. - Euler Angles. We set the initial values of $b_x$, $b_y$, $b_z$, to 0.0001, therefore we won’t have any issues with zero being used in any mathematical operations. Next, we take one more measurement from the IMU sensor, and use it’s quaternion calculations in the variable $x_0$. Now, we are able to compute the initial value of $P^+_0$.

$$ \boldsymbol{P}^+_0 = E \left[ (x_0 - x^+_0)(x_0 - x^+_0)^T \right] $$

Next, we set the Process Noise Covariance $Q_{k-1}$ and Measurement Noise Covariance $R_k$ matrices as shown below, where $I$ represents the Identity matrix.

$$ \boldsymbol{Q}_{k-1} = 0.005*\boldsymbol{I}_{7x7} \qquad \boldsymbol{R}_{k} = 0.005*\boldsymbol{I}_{6x6} $$

The initial values of $\boldsymbol{A}_{k-1}$ and $\boldsymbol{B}_{k-1}$ are initialized using state estimate, $x^+_0$.

$$ A_{0} = \begin{bmatrix} 1 & 0 & 0 & 0 & -\frac{T}{2}(-q_1) & -\frac{T}{2}(-q_2) & -\frac{T}{2}(-q_3) \\ 0 & 1 & 0 & 0 & -\frac{T}{2}(\phantom{-}q_0) & -\frac{T}{2}(-q_3) & -\frac{T}{2}(\phantom{-}q_2) \\ 0 & 0 & 1 & 0 & -\frac{T}{2}(\phantom{-}q_3) & -\frac{T}{2}(\phantom{-}q_0) & -\frac{T}{2}(-q_1) \\ 0 & 0 & 0 & 1 & -\frac{T}{2}(-q_2) & -\frac{T}{2}(\phantom{-}q_1) & -\frac{T}{2}(\phantom{-}q_0) \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix}_{q = x^{+}_{0}} \qquad B_{0} = \frac{T}{2} \begin{bmatrix} -q_1 & -q_2 & -q_3 \\ \phantom{-}q_0 & -q_3 & \phantom{-}q_2 \\ \phantom{-}q_3 & \phantom{-}q_0 & -q_1 \\ -q_2 & \phantom{-}q_1 & \phantom{-}q_0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix}_{q = x^{+}_0} $$

Prediction Step

After initialization, we can take a step into the EKF. In the steps following initialization, $x^+_{k-1}$, is used to update $\boldsymbol{A}$ and $\boldsymbol{B}$. Furthermore, $P^-_k$ is updated using the calculated $\boldsymbol{A}$ matrix. $$ x_k^- = \boldsymbol{A}_{k-1} x^+_{k-1} + \boldsymbol{B}_{k-1}u_{k-1} $$

$$ \boldsymbol{P}_k^- = \boldsymbol{A}_{k-1} \boldsymbol{P}^+_{k-1} \boldsymbol{A}^T_{k-1} + \boldsymbol{Q}_{k-1} $$

Update Step

In order to determine the Kalman Gain, $K_k$, we must first determine the Jacobian Matrix, $H_{k}$. As found in Section 5 - Explanation, the jacobians for both $H_a$ and $H_m$ are found by substituting the reference vectors into the Generalized Jacobian and solving, one-by-one. Afterwards, $H_{k}$ can be found by concatenating the two matrices.

$$ H_\mathrm{k} = \begin{bmatrix} H_a & 0_\mathrm{3x3} \\ H_m & 0_\mathrm{3x3} \\ \end{bmatrix}_{q = x^-_k} $$

$$ H_a = -2 * \begin{bmatrix} -q_2 & \phantom{-}q_3 & -q_0 &\phantom{-}q_1 \\ \phantom{-}q_1 & \phantom{-}q_0 & \phantom{-}q_3 & \phantom{-}q_2 \\ \phantom{-}q_0 & -q_1 & -q2 & \phantom{-}q_3 \\ \end{bmatrix}_{q = x^-_k} $$

$$ H_m = 2 * \begin{bmatrix} \begin{bmatrix} \phantom{-}q_0 & \phantom{-}q_3 & -q_2 \end{bmatrix} V^{ref}_N & \begin{bmatrix} \phantom{-}q_1 & \phantom{-}q_2 & \phantom{-}q_3 \end{bmatrix} V^{ref}_N & \begin{bmatrix} -q_2 & \phantom{-}q_1 & -q_0 \end{bmatrix} V^{ref}_N & \begin{bmatrix} -q_3 & \phantom{-}q_0 & \phantom{-}q_1 \end{bmatrix} V^{ref}_N \\[0.5em] \begin{bmatrix} -q_3 & \phantom{-}q_0 & \phantom{-}q_1 \end{bmatrix} V^{ref}_N & \begin{bmatrix} \phantom{-}q_2 & -q_1 & \phantom{-}q_0 \end{bmatrix} V^{ref}_N & \begin{bmatrix} \phantom{-}q_1 & \phantom{-}q_2 & \phantom{-}q_3 \end{bmatrix} V^{ref}_N & \begin{bmatrix} -q_0 & -q_3 & \phantom{-}q_2 \end{bmatrix} V^{ref}_N \\[0.5em] \begin{bmatrix} \phantom{-}q_2 & -q_1 & \phantom{-}q_0 \end{bmatrix} V^{ref}_N & \begin{bmatrix} \phantom{-}q_3 & -q_0 & -q_1 \end{bmatrix} V^\mathrm{ref}_N & \begin{bmatrix} \phantom{-}q_0 & \phantom{-}q_3 & -q_2 \end{bmatrix} V^{ref}_N & \begin{bmatrix} \phantom{-}q_1 & \phantom{-}q_2 & \phantom{-}q_3 \end{bmatrix} V^{ref}_N \\[0.5em] \end{bmatrix}_{q = x^-_k} $$

Now we calculate the Kalman gain using the calculated Predicted Covariance matrix, $P^-_\mathrm{k}$.

$$ \boldsymbol{K}_k = \boldsymbol{P}^-_{k} \boldsymbol{H}^T_{k} \left( \boldsymbol{H}_{k} \boldsymbol{P}^-_k \boldsymbol{H}^T_{k} + \boldsymbol{R}_k \right)^{-1} $$

After determining the Kalman Gain, we need to perform some Coordinate orientation manipulations on our sensor predictions, $d^-_\mathrm{k}$. In order to determine the predicted value of our measurements, $d^-_{k}$, we need to use our predicted quaternion x values, $x^-_k$, as shown below. Due to the quaternion values being computed in the NED-Frame (Global), for orientation Estimations in an absolute context, we must therefore rotate the predicted quaternion values from the NED-Frame to the Body-Frame.

$$ d^-_{k} = \begin{bmatrix} \lVert d^-_a \rVert \\[0.5em] \lVert d^-_m \rVert \\ \end{bmatrix} \qquad d^-_{a} = R^b_n (x^-_k) * V^{ref}_D \qquad d^-_{m} = R^b_n (x^-_k) * V^{ref}_N $$

$$ \begin{aligned} d^-_{a} &= \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) \\[0.5em] 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) \\[0.5em] 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}_{q=x^-_k} \begin{bmatrix} \phantom{-}0 \\[0.5em] \phantom{-}0 \\[0.5em] -1 \\ \end{bmatrix} = - \begin{bmatrix} 2(q_1 q_3 - q_0 q_2) \\[0.5em] 2(q_2 q_3 + q_0 q_1) \\[0.5em] 1 - 2(q_1^2 - q_2^2) \\ \end{bmatrix}_{q=x^-_k} \\ d^-_{m} &= \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) \\[0.5em] 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) \\[0.5em] 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}_{q=x^-_k} \begin{bmatrix} M_{ref_x} \\[0.5em] M_{ref_y} \\[0.5em] 0 \\ \end{bmatrix} \end{aligned} $$

After the $d^-_a$ and $d^-_m$ matrices are solved for, we normalize the 3 components in order to obtain purely directional values. $$ {\lVert d^-_a \rVert} = \begin{bmatrix} \frac{d^{-}_{a_x}}{\lVert d^-_a \rVert} \\[0.5em] \frac{d^{-}_{a_y}}{\lVert d^-_a \rVert} \\[0.5em] \frac{d^{-}_{a_z}}{\lVert d^-_a \rVert} \end{bmatrix} \qquad {\lVert d^-_m \rVert} = \begin{bmatrix} \frac{d^{-}_{m_x}}{\lVert d^-_a \rVert} \\[0.5em] \frac{d^{-}_{m_y}}{\lVert d^-_a \rVert} \\[0.5em] \frac{d^{-}_{m_z}}{\lVert d^-_a \rVert} \end{bmatrix} $$

Next, to be able to relate the actual observed measurement values, $z_k$, to the predicted values of $d^-_k$, we must perform computations on the the magnetometer component $z_m$.

$$ z_{k} = \begin{bmatrix} \lVert z_a \rVert \\ \lVert R*z_{m_i} \rVert \\ \end{bmatrix} = \begin{bmatrix} \lVert z_a \rVert \\ \lVert z_{m_f} \rVert \end{bmatrix} $$

To relate the magnetometer values of $z_m$ to the predicted magnetometer values $d^-_m$, we must first rotate $z_m$ from the local Body-Frame to the Global NED-Frame, using $x^-_k$ for the rotation quaternion.

$$ z_{t} = \begin{bmatrix} z_{t_x} \\ z_{t_y} \\ z_{t_z} \end{bmatrix} = R_{b}^{n} z_{m_i} = \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}_{q=x^-_k} \begin{bmatrix} z_{m_x} \\ z_{m_y} \\ z_{m_z} \end{bmatrix} $$

Where $z_t$ is the temporary value of $z_m$.

Next, we set the z-axis component to zero, $z_{t_z} = 0$, to match the reference vector $V^{ref}_N$, then rotate $z_t$ back from the Global NED-Frame to the local Body-Frame, $z_{m_f}$.

$$ z_{m_f} = R_{n}^{b} z_{t} = \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}_{q=x^-_k} \begin{bmatrix} z_{t_x} \\ z_{t_y} \\ 0 \end{bmatrix} $$

Finally, we normalize the components of $z_k$:

$$ z_{k} = \begin{bmatrix} \lVert z_a \rVert \\ \lVert z_{m_f} \rVert \\ \end{bmatrix} $$

Finally, we are able to solve for our current quaternion values, $x^+_k$, using the previously determined variables:

$$ x^{+}_{k} = x_{k}^{-} + \boldsymbol{K}_{k} \left( z_k - d^-_{k} \right) $$

The Covariance Matrix for the current time step can then be updated, as shown below.

$$ \boldsymbol{P}^+_k = \left( \boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k \right) \boldsymbol{P}^-_k $$

After updating the system state variables, $x^+_k$ and $P^+_k$, we are able to calculate the system residual error, $\varepsilon_k$.

$$ \varepsilon_k = \left[ z_k - h_k(x^+_k)V^\mathrm{ref}_x \right] $$

With the system residual error, $\varepsilon_k$, we can now update the Process Noise Covariance, $Q_k$, and Measurement Noise Covariance, $R_k$, using the equations below.

$$ \boldsymbol{R}_k = \alpha\boldsymbol{R}_{k-1} + (1-\alpha)\left( \varepsilon_k \varepsilon^T_k + \boldsymbol{H}_k \boldsymbol{P}^-_{k} \boldsymbol{H}^T_k \right) \qquad \boldsymbol{Q}_k = \alpha\boldsymbol{Q}_{k-1} + (1 - \alpha)\left( \boldsymbol{K}_k d^-_k d^{-^T}_k \boldsymbol{K}^T_k \right) $$

Following this, we can update our previous value variables, $x_\mathrm{k-1}$ and $P_\mathrm{k-1}$.

$$ x^+_{k-1} = x^+_k \qquad \boldsymbol{P}^+_{k-1} = \boldsymbol{P}^+_k $$