Skip to main content
. 2026 Jun 28;26(13):4107. doi: 10.3390/s26134107
Algorithm 1. Computational workflow of the proposed ARTSS
Input: Forward ESKF parameters: x^f,k|k−1, Pf,k|k−1, Pf,k, Hk, Rk, Φk+1|k, and raw measurement zk
Output: Smoothed states x^s,k
1: For k=N−1 down to 0 do
2:  Compute standard innovation and its covariance using Equations (17) and (18)
3:  Calculate element-wise standardized innovation v˜k,i using Equation (19)
4:  Construct diagonal equivalent weight matrix Bk using Equation (20) with c=1.345
5:  Reconstruct robust forward gain K¯k, state x¯f,k, and covariance P¯f,k via Equations (21)–(23)
6:  Modulate smoothing gain Ks,k via Equation (24)
7:  Update smoothed state x^s,k via Equation (25)
8:  Apply 360-degree cross-period correction specifically to the heading angle error component (ϕU) within the state vector x^s,k to prevent angle wrap-around divergence:
     If ϕU>180°, then ϕU=ϕU−360°
     If ϕU<−180°, then ϕU=ϕU+360°
9:  Update smoothed error covariance Ps,k via Equation (26)
10: End For