Algorithm 1 RBF-ESKF multi-sensor fusion for underwater navigation

Initialization: 1:Initialize ESKF variables ${\mathit{x}}_0^{-},{\mathit{P}}_0^{-},{\mathit{Q}}_0,{\mathit{R}}_0$ 2:Initialize RBF variables ${\mathit{w}}_0^{,}{\mathit{c}}_0,{\mathit{\sigma }}_0,{\eta }_w,{\eta }_c$    Kalman gain update: 3: calculate Kalman gain $${\mathit{K}}_k={\mathit{P}}_k^{-}{\mathit{H}}_k^{\top }{\left({\mathit{H}}_k{\mathit{P}}_k^{-}{\mathit{H}}_k^{\top }+{\mathit{R}}_k\right)}^{-1}$$    RBF Gaussian function update: 4: Learning non-linearity of error state vector $${\mathit{y}}_k\left(i\right)=exp\left(-\frac{{∥\delta {\mathit{x}}_k^{-}-{\mathit{c}}_{ik}∥}^2}{2{\mathit{\sigma }}_i^2}\right),i=1,2,\dots ,N_c$$    Innovation update: 5: Non-linearity influence is minimized by using output of RBF neural network in innovation term $${\tilde{\mathit{s}}}_{\mathit{k}}=\delta {\mathit{z}}_k-({\mathit{H}}_k\delta {\mathit{x}}_k^{-}+{\mathit{W}}_{\mathit{k}}{\mathit{y}}_k)$$    Measurement update: 6: Estimate error state by using innovation term and Kalman gain $$\delta {\mathit{x}}_k^{+}=\delta {\mathit{x}}_k^{-}+{\mathit{K}}_k{\tilde{\mathit{s}}}_{\mathit{k}}$$ 7: Error state covariance update. $${\mathit{P}}_k^{+}=\left(\mathit{I}-{\mathit{K}}_k{\mathit{H}}_k\right){\mathit{P}}_k^{-}$$    Full State correction 8: Full state is corrected by error adding error estimate. $$\mathit{x}=\widehat{\mathit{x}}+\delta {\mathit{x}}_k^{+}$$    RBF Neural Network Weight and Center update: 9: RBF weight update $${\mathit{w}}_{mk+1}={\mathit{w}}_{mk}+{\eta }_w(\delta {\mathit{z}}_k\left(m\right)-{\mathit{H}}_k\left(m\right)\delta {\mathit{x}}_k^{-}\left(m\right)-{\mathit{w}}_{mk}{\mathit{y}}_k){\mathit{y}}_k^T$$ 10: RBF center update $$\begin{array}{c}\hfill {\mathit{c}}_{mk+1}\left(j\right)={\mathit{c}}_{ik}\left(j\right)+{\eta }_c\left(\frac{{\mathit{w}}_{ik}\left(j\right){\mathit{y}}_k\left(j\right){\mathit{c}}_{ik}\left(j\right)}{{\mathit{\sigma }}_i^2}\right).\sum _{i=1}^M(\delta {\mathit{z}}_k\left(i\right)-({\mathit{H}}_k\left(i\right)\delta {\mathit{x}}_k^{-}\left(i\right)+{\mathit{w}}_{ik}{\mathit{y}}_k))\end{array}$$   Time propagation: 11: Time propagation of error state and covariance $$\delta {\mathit{x}}_{k+1}^{-}={\mathbf{\Phi }}_k\delta {\mathit{x}}_k^{+}$$ $${\mathit{P}}_{k+1}^{-}={\mathbf{\Phi }}_k{\mathit{p}}_k^{+}{\mathbf{\Phi }}_k^{\top }+{\mathit{Q}}_k$$ 12:Next iteration (posterior becomes prior)