Algorithm 1. IEKF estimation model

(1)Step 1 (initialization): Initialization of IEKF is similar to EKF as the initial state vector ${\tilde{\mathrm{x}}}_{0|0}$ and initial error covariance ${\overline{\mathrm{P}}}_{0|0}$, and state prediction step are the same as in EKF. (2) Step 2 (measurement update iterations): Measurement iterations are started by initializing ${\hat{\mathrm{x}}}_{t+1}^0={\mathrm{x}}_{t+1|t}$ and $i=0$, computation of Jacobian matrix, Kalman gain and state estimate for the next iteration: $$H_{t+1}^i={\frac{\partial h_{t+1}\left(s\right)}{\partial s}|}_{s={\hat{\mathrm{x}}}_{t+1}^i}$$ $$K_{t+1}^i={\overline{\mathrm{P}}}_{t+1|t}{\left[{\mathrm{H}}_{t+1}^i\right]}^T{\left({\mathrm{H}}_{t+1}^i{\overline{\mathrm{P}}}_{t+1|t}{\left[{\mathrm{H}}_{t+1}^i\right]}^T+{\mathrm{R}}_{t+1}\right)}^{-1}$$ $${\hat{\mathrm{x}}}_{t+1}^{i+1}={\hat{\mathrm{x}}}_{t+1|t}+K_{t+1}^i\left({\mathrm{z}}_{t+1}-h_{t+1}\left({\hat{\mathrm{x}}}_{t+1}^i\right)-{\mathrm{H}}_{t+1}^i\left({\hat{\mathrm{x}}}_{t+1|t}-{\hat{\mathrm{x}}}_{t+1}^i\right)\right)$$ Step 2 is iteratively executed once a stopping criterion is achieved, i.e., the difference between two successive approximations is less than a predefined threshold $\xi$, $\Vert {\mathrm{x}}_{t+1}^{i+1}-{\mathrm{x}}_{t+1}^i\Vert <\xi$. (3)Step 3 (finalization): Once the stopping criteria is achieved, the state vector and covariance matrix is finalized