There are two Error Correction algorithms that were used in the making of this article. It was found that the Adaptive Adjustment of Noise Covariance algorithm performed better than the Sensor Output Error Correction algorithm.
The Process noise covariance and Observation noise covariance matrices, $Q_\mathrm{k-1}$ and $R_\mathrm{k-1}$, respectively, are important in the convergence of the EKF. In order to obtain useful values for these matrices, we define $Q$ and $R$ as shown:
$$ Q_{k-1} = \begin{bmatrix} \Omega_\mathrm{\omega, k-1} & 0 & 0 \\ 0 & \Omega_\mathrm{\omega, k-1} & 0 \\ 0 & 0 & \Omega_\mathrm{\omega, k-1} \\ \end{bmatrix} \qquad R_{k-1} = \begin{bmatrix} \Omega_\mathrm{m, k-1} & 0 & 0 & 0 & 0 & 0 \\ 0 & \Omega_\mathrm{m, k-1} & 0 & 0 & 0 & 0 \\ 0 & 0 & \Omega_\mathrm{m, k-1} & 0 & 0 & 0 \\ 0 & 0 & 0 & \Omega_\mathrm{a, k-1} & 0 & 0 \\ 0 & 0 & 0 & 0 & \Omega_\mathrm{a, k-1} & 0 \\ 0 & 0 & 0 & 0 & 0 & \Omega_\mathrm{a, k-1} \\ \end{bmatrix} $$
Where: $$ \Omega_\mathrm{\omega,k-1} = a \sqrt{\omega_x^2 + \omega_y^2 + \omega_z^2} + b \qquad \Omega_\mathrm{a,k-1} = c \sqrt{a_x^2 + a_y^2 + a_z^2} + d \qquad \Omega_\mathrm{m,k-1} = e \sqrt{m_x^2 + m_y^2 + m_z^2} + f $$
Where the coefficients a, b, d, d, e, f, are iteratively updated using a MRSE or Log Likelihood maximization function, such as:
$$ LL = -\frac{N}{2}\ln(2\pi) - \frac{1}{2}\sum_{j=1}^{N}\left( \ln(B_j) + \frac{y_j^2}{B_j} \right) $$
Initially, the first 15 samples of the IMU are measured and stored in order to obain an Expected average value, $x_0$. The calculation of the Expected State variable, $x^+_0$, at the k=0 time step, following the initial samples, is as follows:
$$ x^+_0 = E[x_0] = \frac{1}{N} \sum_{i}^{N} x_i = \begin{bmatrix} q_0 \\[0.5em] q_1 \\[0.5em] q_2 \\[0.5em] q_3 \\[0.5em] b_{\omega_x} \\[0.5em] b_{\omega_y} \\[0.5em] b_{\omega_z} \\[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] 0.0001 \\[0.5em] 0.0001 \\[0.5em] 0.0001 \\[0.5em] \end{bmatrix} $$
The initial value of the diagonal Error Covariance matrix is determined using:
$$ P^+_0 = E[(x_0 - x^+_0)(x_0 - x^+_0)^T] $$
The predicted measurement error is shown below:
$$ d_k = z_k - h_k(x^-_k)V^{ref}_x $$
The residual difference between actual measurement and estimated measurement is calculated using:
$$ \varepsilon_k = z_k - h_k(x^+_k)V^\mathrm{ref}_x $$
Based on these definitions, the adaptive estimates of $R_k$ and $Q_{k-1}$ are defined:
$$ \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) $$
$$ \boldsymbol{Q}_k = \alpha\boldsymbol{Q}_{k-1} + (1 - \alpha)\left( \boldsymbol{K}_k d_k d^T_k \boldsymbol{K}^T_k \right) $$