Safe-RTSKF: A Safety-Constrained Recursive Two-Stage Kalman Filter for Robust Sensor State Estimation in Strongly Nonlinear Systems

Robust state estimation is essential for sensor systems operating under strong nonlinearity, high uncertainty, and outlier-contaminated measurements. Recursive two-stage Kalman filtering provides an interpretable framework for systems with multiplicative hidden variable decompositions, but its raw form may suffer from hidden variable drift and numerical instability when the hidden variable deviates from its structural relation with the state. To address this problem, this paper proposes Safe-RTSKF, a safety-constrained recursive two-stage Kalman filter for robust sensor state estimation. The proposed method integrates normalized innovation squared gating, spectral-norm Kalman gain clipping, update damping, Joseph-form covariance updating, covariance spectral projection, and hidden variable consistency correction. The consistency correction constrains the estimated hidden variable toward its structural relation with the updated state, thereby reducing recursive error accumulation. Theoretical analysis provides local boundedness and consistency error contraction conditions. Monte Carlo experiments on one-, two-, and four-dimensional strongly nonlinear systems show that Safe-RTSKF substantially improves the stability of Raw RTSKF. Additional range–bearing and coordinated turn tracking experiments demonstrate that the safety update mechanism can also improve EKF robustness under outlier measurements. The results indicate that the proposed framework offers a practical accuracy–stability–runtime trade-off for nonlinear sensor state estimation.