Online Update of Compensation Model
Date:
The magnetometer compensation method based on the ellipsoidal error model requires the assumption that the carrier operates in a uniform magnetic field environment during the calibration of the compensation model. However, this inevitably introduces errors caused by magnetic field gradients.
During the calibration data collection process, external magnetic field data is simultaneously collected as ground truth for training the compensation model. Additionally, in geomagnetic navigation, magnetic map data is acquired in real-time for magnetic field matching. Therefore, this information can be leveraged to perform online updates of the magnetic compensation model.
KF-based Model Update
Sensor Model
The measurement model is given as described here.
\[\boldsymbol{y}_{k}^m=D\boldsymbol{m}_k^m+\boldsymbol{o}+\boldsymbol{e}_{k}^m\]The measurements of the magnetometer outside the cabin are denoted by the superscript \(m’\).
\[\boldsymbol{y}_{k}^m=DR^{mm'}\boldsymbol{m}_k^{m'}+\boldsymbol{o}+\boldsymbol{e}_{k}^m\]let
\[C=DR^{mm'}\]then
\[\boldsymbol{y}_{k}^m=C\boldsymbol{m}_k^{m'}+\boldsymbol{o}+\boldsymbol{e}_{k}^m\]Sensor Calibration with Known Magnetic Field
Instead of rotating the platform in a homogeneous magnetic field, the calibration parameters can also be ensured by changing the external magnetic field, e.g., by moving the sensor in an inhomogeneous but known magnetic field.
Define the compensation model parameters
\[\theta= \begin{bmatrix} o \\ {\rm vec}(C) \end{bmatrix}\]The sensor model can be rewritten as (drop the superscript for convenience)
\[\boldsymbol{y}_{k}=h(\boldsymbol{m}_k)\theta+\boldsymbol{e}_{k} \tag{1}\]where
\[h(\boldsymbol{m}_k)= \begin{bmatrix} I_{3\times3} & x_{mk}I_{3\times3} & y_{mk}I_{3\times3} & z_{mk}I_{3\times3} \end{bmatrix}\]Kalman Filter
The Kalman filter can be used to the LSE to estimate the parameters. This is particularly helpful when the estimation is performed online in a recursive manner. The prediction model for a KF can be described by a time discrete random walk
\[\theta_k=\theta_{k-1}+{\rm w}_{k-1}^\theta\]with
\[{\rm w}_{k}^\theta\sim\mathcal{N}(0,R_k)\]The measurement model is decribed by (1), with the process noise
\[e_{k}\sim\mathcal{N}(0,Q_k)\]The Kalman Filter process is summarized by
\[\begin{aligned} &\bar{\theta}_k=\theta_{k-1} \\ &\bar{\Sigma}_k=\Sigma_{k-1}+R_k \\ &K_k=\bar{\Sigma}_k h(m_k)^T \left(h(m_k)\bar{\Sigma}_kh(m_k)^T+Q_k\right)^{-1} \\ &\theta_k=\bar\theta_k+K_k\left(y_k-h(m_k)\bar\theta_k\right) \\ &\Sigma_k=\left(I-K_kh(m_k)\right)\bar\Sigma_k \end{aligned}\]The Kalman filter intrinsically accounts for the observability of the parameters. If the measurements are correlated and the Kalman filter is not already converged, the state covariance is large indicating large uncertainty in the estimation and vice versa. 1
EKF-based Model Update
Sensor Model
The measurement model is given by
\[\boldsymbol{y}_{k}^m=D\boldsymbol{m}_k^m+\boldsymbol{o}+\boldsymbol{e}_{k}^m\] \[\boldsymbol{y}_{k}^m=DR^{mb}R_k^{bn}\boldsymbol{m}_k^{n}+\boldsymbol{o}+\boldsymbol{e}_{k}^m\]let
\[C=DR^{mb}\]then
\[\boldsymbol{y}_{k}^m=CR_k^{bn}\boldsymbol{m}_k^{n}+\boldsymbol{o}+\boldsymbol{e}_{k}^m\]Sensor Calibration with Unknown Inhomogeneous Magnetic Field
If the platform is in an unknown and inhomogeneous magnetic field, the calibration parameters should be adjusted to include not only the model coefficients, but also the magnetic field around the platform.
Define the compensation model parameters
\[\theta= \begin{bmatrix} o \\ {\rm vec}(C) \\ m^n \end{bmatrix} \in\mathbb{R}^{15}\]Note that the measurement model is non-linear. Representing the non-linear model by \(h(\cdot)\), the sensor model can be rewritten as (drop the superscript for convenience)
\[\boldsymbol{y}_{k}=h(\theta)+\boldsymbol{e}_{k}\]In an EKF, the Taylor expansion of the measurement model is computed by
\[h(\theta)\approx h(\bar\theta)+H_k(\theta-\bar\theta)\]where
\[H_k= \left.\frac{\partial h(\theta)}{\partial \theta}\right|_{\theta=\bar\theta} = \begin{bmatrix} I_{3\times3} & \bar m^b_{1}I_{3\times3} & \bar m^b_{2}I_{3\times3} & \bar m^b_{3}I_{3\times3} & \bar{C}R^{bn}_k \end{bmatrix}\]and
\[R^{bn}_k\bar m^n\triangleq \begin{bmatrix} \bar m^b_{1} & \bar m^b_{2} & \bar m^b_{3} \end{bmatrix}\]Extended Kalman Filter
The prediction model for the EKF is the same as the one in KF.
\[\theta_k=\theta_{k-1}+{\rm w}_{k-1}^\theta\]with
\[{\rm w}_{k}^\theta\sim\mathcal{N}(0,R_k)\]The measurement model is
\[\boldsymbol{y}_{k}=h(\theta)+\boldsymbol{e}_{k}\]with the process noise
\[e_{k}\sim\mathcal{N}(0,Q_k)\]The Extended Kalman Filter process is summarized by
\[\begin{aligned} &\bar{\theta}_k=\theta_{k-1} \\ &\bar{\Sigma}_k=\Sigma_{k-1}+R_k \\ &K_k=\bar{\Sigma}_k H_k^T \left(H_k\bar{\Sigma}_kH_k^T+Q_k\right)^{-1} \\ &\theta_k=\bar\theta_k+K_k\left(y_k-h(\bar\theta_k)\right) \\ &\Sigma_k=\left(I-K_kH_k\right)\bar\Sigma_k \end{aligned}\]Simultaneous Calibration and Localization
magnetometer-inertial navigation system
The measurement model is given by
\[\boldsymbol{y}_{k}^m=D\boldsymbol{m}_k^m+\boldsymbol{o}+\boldsymbol{e}_{k}^m\] \[\boldsymbol{y}_{k}^m=DR^{mb}R_k^{bn}\boldsymbol{m}_k^{n}+\boldsymbol{o}+\boldsymbol{e}_{k}^m\]let
\[C=DR^{mb}\]then
\[\boldsymbol{y}_{k}^m=CR_k^{bn}\boldsymbol{m}_k^{n}+\boldsymbol{o}+\boldsymbol{e}_{k}^m\]Sensor Calibration with Known Magnetic Field
Define the compensation model parameters
\[\theta= \begin{bmatrix} o \\ {\rm vec}(C) \end{bmatrix}\]Define
\[m_k^b=R_k^{bn}m_k^n\doteq \begin{bmatrix} x^b_k & y^b_k & z^b_k \end{bmatrix}^T\]The sensor model can be rewritten as (drop the superscript for convenience)
\[{y}_{k}=h(m_k^b)\theta+{e}_{k}\]where
\[h(\boldsymbol{m}_k)= \begin{bmatrix} I_{3\times3} & x^b_kI_{3\times3} & y^b_kI_{3\times3} & z^b_kI_{3\times3} \end{bmatrix}\]Simultaneous Calibration and Localization
The state of the system is defined as
\[x_k= \begin{bmatrix} p_k^T & q_k^T & \theta_k^T \end{bmatrix}^T \doteq \begin{bmatrix} d_k^T & \theta_k^T \end{bmatrix}^T\]Then the time evolution of the state can be represented by
\[x_k= \begin{bmatrix} f(d_{k-1},{\rm w}^d_{k-1}) \\ \theta_{k-1}+{\rm w}_{k-1}^\theta \end{bmatrix}\]with
\[{\rm w}_{k}^d\sim\mathcal{N}(0,R_k^d)\] \[{\rm w}_{k}^\theta\sim\mathcal{N}(0,R_k^\theta)\]The measurement model is decribed by
\[{y}_{k}=h(R_k^{bn}m(p_k))\theta+{e}_{k}\]with
\[e_{k}\sim\mathcal{N}(0,Q_k)\]and \(m(\cdot)\) represents the map of magnetic field.
Particle Filter Algorithm for Calibration and Localization
For each particle \(i\), perform the following steps:
- Sample process noise
- Particle filter time update (for nonlinear part of the state)
- Kalman covariance time update (for linear part of the state)
- Kalman measurement data prediction
- Weight update
- Kalman gain computtion
- Kalman state and covariance update