Algorithm 1: EKF for magnetic field SLAM

Input: ${\left\{\Delta p_t^{\mathrm{w}},\Delta q_t^{\mathrm{b}},y_t^{\mathrm{b}}\right\}}_{t=1}^N$    Output: ${\left\{{\widehat{p}}_{t|t}^{\mathrm{w}}\right\}}_{t=1}^N$, ${\left\{{\widehat{q}}_{t|t}^{\mathrm{w}\mathrm{b}}\right\}}_{t=1}^N$, ${\left\{{\widehat{m}}_{t|t}\right\}}_{t=1}^N$           Initialisation: ${\widehat{p}}_{0|0}^{\mathrm{w}}=0_{3\times 1}$, ${\widehat{q}}_{0|0}^{\mathrm{w}\mathrm{b}}=q_0^{\mathrm{w}\mathrm{b}}$, ${\widehat{m}}_{0|0}=0_{(N_m+3)\times 0}$, (19) 1: for $t=1$ to N do 2:    Dynamic update $${\widehat{p}}_{t|t-1}^{\mathrm{w}}={\widehat{p}}_{t-1|t-1}^{\mathrm{w}}+\Delta p_t^{\mathrm{w}}$$ (26a) $${\widehat{q}}_{t|t-1}^{\mathrm{w}\mathrm{b}}={\widehat{q}}_{t-1|t-1}^{\mathrm{w}\mathrm{b}}\odot \Delta q_t^{\mathrm{b}}$$ (26b) $${\widehat{m}}_{t|t-1}={\widehat{m}}_{t-1|t-1}$$ (26c) $$P_{t|t-1}=P_{t-1|t-1}+Q$$ (26d) 3:    Measurement update $$z_t={\widehat{R}}_{t|t-1}^{\mathrm{w}\mathrm{b}}{y}_t^{\mathrm{b}}-\nabla {\mathrm{\Phi }}_{\mathrm{p}}\left({\widehat{p}}_{t|t-1}^{\mathrm{w}}\right){\widehat{m}}_{t|t-1}$$ (27a) $$S_t=H_t{P}_{t|t-1}{H}_t^{\top }+{\sigma }_{\mathrm{m}}^2{\mathcal{I}}_3$$ (27b) $$K_t=P_{t|t-1}{H}_t^{\top }{S}_t^{-1}$$ (27c) $${\widehat{\xi }}_t=K_t{z}_t$$ (27d) $$P_{t|t}=P_{t|t-1}-K_t{S}_t{K}_t^{\top }$$ (27e) 4:    Relinearization $${\widehat{p}}_{t|t}^{\mathrm{w}}={\widehat{p}}_{t|t-1}^{\mathrm{w}}+{\widehat{\delta }}_t^{\mathrm{w}}$$ (28a) $${\widehat{q}}_{t|t}^{\mathrm{w}\mathrm{b}}={exp}_{\mathrm{q}}\left({\widehat{\eta }}_t^{\mathrm{w}}\right)\odot {\widehat{q}}_{t|t-1}^{\mathrm{w}\mathrm{b}}$$ (28b) $${\widehat{m}}_{t|t}={\widehat{m}}_{t|t-1}+{\widehat{\nu }}_t$$ (28c) 5: end for