Cooperative localization of mobile nodes in NLOS

In this paper, cooperative localization of mobile nodes in non-line of sight (NLOS) situation is considered using a constrained square root unscented Kalman filter (CSRUKF). The NLOS measurements are used as quadratic constraints, which form a convex feasible region inside which the positions of the mobile nodes are supposed to be. The CSRUKF consists of two main stages: square root unscented Kalman filter (SRUKF) and sigma point projection. In the former, a conventional SRUKF is used to estimate the state vector and the Cholesky factor of the error covariance matrix. In the latter, a new set of sigma points are generated, and the ones violating the constraints are projected onto the feasible region by solving a set of convex quadratically constrained quadratic programs (QCQP). Each QCQP can be solved independently and in parallel for each sigma point violating the constraint, thus the algorithm is suitable for distributed processing. The simulation results show that our algorithm can perform well in different NLOS scenarios.