Error Correction
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.
Sensor Output Error Correction
The Process noise covariance and Observation noise covariance matrices, Q k − 1 Q_\mathrm{k-1} Q k − 1 R k − 1 R_\mathrm{k-1} R k − 1 Q Q Q R R R
Q k − 1 = [ Ω ω , k − 1 0 0 0 Ω ω , k − 1 0 0 0 Ω ω , k − 1 ] R k − 1 = [ Ω m , k − 1 0 0 0 0 0 0 Ω m , k − 1 0 0 0 0 0 0 Ω m , k − 1 0 0 0 0 0 0 Ω a , k − 1 0 0 0 0 0 0 Ω a , k − 1 0 0 0 0 0 0 Ω a , k − 1 ]
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}
Q k − 1 = ⎣ ⎡ Ω ω , k − 1 0 0 0 Ω ω , k − 1 0 0 0 Ω ω , k − 1 ⎦ ⎤ R k − 1 = ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎡ Ω m , k − 1 0 0 0 0 0 0 Ω m , k − 1 0 0 0 0 0 0 Ω m , k − 1 0 0 0 0 0 0 Ω a , k − 1 0 0 0 0 0 0 Ω a , k − 1 0 0 0 0 0 0 Ω a , k − 1 ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎤
Where:
Ω ω , k − 1 = a ω x 2 + ω y 2 + ω z 2 + b Ω a , k − 1 = c a x 2 + a y 2 + a z 2 + d Ω m , k − 1 = e m x 2 + m y 2 + m z 2 + f
\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
Ω ω , k − 1 = a ω x 2 + ω y 2 + ω z 2 + b Ω a , k − 1 = c a x 2 + a y 2 + a z 2 + d Ω m , k − 1 = e 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:
L L = − N 2 ln ( 2 π ) − 1 2 ∑ j = 1 N ( ln ( B j ) + y j 2 B j )
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)
L L = − 2 N ln ( 2 π ) − 2 1 j = 1 ∑ N ( ln ( B j ) + B j y j 2 )
Adaptive Adjustment of Noise Covariance
Initially, the first 15 samples of the IMU are measured and stored in order to obain an Expected average value, x 0 x_0 x 0 x 0 + x^+_0 x 0 +
x 0 + = E [ x 0 ] = 1 N ∑ i N x i = [ q 0 q 1 q 2 q 3 b ω x b ω y b ω z ] = [ cos ϕ 2 cos θ 2 cos ψ 2 + sin ϕ 2 sin θ 2 sin ψ 2 sin ϕ 2 cos θ 2 cos ψ 2 − cos ϕ 2 sin θ 2 sin ψ 2 cos ϕ 2 sin θ 2 cos ψ 2 + sin ϕ 2 cos θ 2 sin ψ 2 cos ϕ 2 cos θ 2 sin ψ 2 − sin ϕ 2 sin θ 2 cos ψ 2 0 . 0 0 0 1 0 . 0 0 0 1 0 . 0 0 0 1 ]
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}
x 0 + = E [ x 0 ] = N 1 i ∑ N x i = ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎡ q 0 q 1 q 2 q 3 b ω x b ω y b ω z ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎤ = ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎡ cos 2 ϕ cos 2 θ cos 2 ψ + sin 2 ϕ sin 2 θ sin 2 ψ sin 2 ϕ cos 2 θ cos 2 ψ − cos 2 ϕ sin 2 θ sin 2 ψ cos 2 ϕ sin 2 θ cos 2 ψ + sin 2 ϕ cos 2 θ sin 2 ψ cos 2 ϕ cos 2 θ sin 2 ψ − sin 2 ϕ sin 2 θ cos 2 ψ 0 . 0 0 0 1 0 . 0 0 0 1 0 . 0 0 0 1 ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎤
The initial value of the diagonal Error Covariance matrix is determined using:
P 0 + = E [ ( x 0 − x 0 + ) ( x 0 − x 0 + ) T ]
P^+_0 = E[(x_0 - x^+_0)(x_0 - x^+_0)^T]
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 x r e f
d_k = z_k - h_k(x^-_k)V^{ref}_x
d k = z k − h k ( x k − ) V x r e f
The residual difference between actual measurement and estimated measurement is calculated using:
ε k = z k − h k ( x k + ) V x r e f
\varepsilon_k = z_k - h_k(x^+_k)V^\mathrm{ref}_x
ε k = z k − h k ( x k + ) V x r e f
Based on these definitions, the adaptive estimates of R k R_k R k Q k − 1 Q_{k-1} Q k − 1
R k = α R k − 1 + ( 1 − α ) ( ε k ε k T + H k P k − H k T )
\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)
R k = α R k − 1 + ( 1 − α ) ( ε k ε k T + H k P k − H k T )
Q k = α Q k − 1 + ( 1 − α ) ( K k d k d k T K k T )
\boldsymbol{Q}_k = \alpha\boldsymbol{Q}_{k-1} + (1 - \alpha)\left( \boldsymbol{K}_k d_k d^T_k \boldsymbol{K}^T_k \right)
Q k = α Q k − 1 + ( 1 − α ) ( K k d k d k T K k T )