Algorithm 1: IRACKF.

Input: Initial state $\left[x_0,y_0,v_{x0},v_{y0}\right]$, sampling number T observation dimension n prediction noise covariance matrix Q, observation noise covariance matrix R, error covariance matrix of the priori state P, variance threshold Var_LOS, the average value of $\mathsf{\Delta }\overline{v}$ in LOS environment D.

Output: Estimate position and velocity $\left[{\mathit{X}}_1 {\mathit{X}}_2 \dots {\mathit{X}}_T\right]$ 1: ${\mathit{X}}_1$$\text{←}\left[x_0,y_0,v_{x0},v_{y0}\right]$ 2: for t ← 2 to T do 3: ${\mathit{Y}}_t$ ← Read UWB observations 4: // Calculate the variance of errors in UWB observations 5:  Var ← get_var $\left({\mathit{Y}}_t\right)$ 6: // Prediction update 7: // Calculate the state prediction values and the prediction covariance matrix 8: $({\mathit{x}}_t,{\mathit{p}}_t)$ ← get_pre(${\mathit{X}}_{t-1}$$,{\mathit{P}}_{t-1}$) 9: // Calculate the robust factor 10: // Calculate the innovation and innovation covariance matrix 11: $({\mathit{e}}_t$$,\mathit{p}{\mathit{y}}_t$$) \text{←} \mathrm{get}\text{\_}\mathrm{py}({\mathit{x}}_t$$,{\mathit{p}}_t$$,{\mathit{Y}}_t$) 12: $\mathit{\alpha }$← identity matrixfor 13: for j ← 1 to n do 14: if Var$\left(j\right)$ > Var_LOS 15: $\mathsf{\Delta }{\overline{v}}_{j,t}$$\text{←}\frac{{\mathit{e}}_j}{\mathit{p}{\mathit{y}}_{jj}}$ 16: $\mathit{\alpha }\left(j,j\right)$$\text{←}\frac{{\mathit{D}}_j}{\Delta {\overline{v}}_{j,t}}$ 17: end if 18: j ← j + 1 19: end for 20: // Measurements update 21: if det$\left(\mathit{\alpha }\right)$<>1 22: ${\mathit{R}}_{t-1}$ $\text{←}\mathit{\alpha }*{\mathit{R}}_{t-1}$ 23: $\left({\mathit{X}}_t,{\mathit{P}}_t\right)$←Mu_RCKF$\left({\mathit{x}}_t,{\mathit{p}}_t,{\mathit{Q}}_{t-1},{\mathit{R}}_{t-1},{\mathit{Y}}_t\right)$ 24: else 25: $\left({\mathit{X}}_t,{\mathit{P}}_t,{\mathit{Q}}_t\right)$←Mu_ACKF$\left({\mathit{x}}_t,{\mathit{p}}_t,{\mathit{Q}}_{t-1},{\mathit{R}}_{t-1},{\mathit{Y}}_t,t\right)$ 26: end if 27: t ← t + 1 28: end for