Trajectory Estimation Algorithm of Sonde Under NLOS State Based on Factor Graph and Particle Filter

For the positioning problem of the sonde in NLOS and LOS mixed environments, this paper proposes an algorithm combining factor graphs and particle filtering to effectively estimate the target's motion state. Its core advantage lies in the adaptive processing of LOS (line-of-sight), NLOS (non-line-ofsight), and mixed signal environments, without relying on prior knowledge such as map information or training data. Even in obstructed line-of-sight conditions, the algorithm can still provide high-precision position and velocity estimates by utilizing signals from reflected paths. Simulation results show that the proposed method outperforms conventional LOS positioning algorithms in terms of real-time performance and accuracy in complex environments, especially when the signal is severely obstructed, maintaining high estimation accuracy.