Marginalized Particle Filter for Accurate and Reliable Terrain-Aided Navigation

This paper details an approach to the integration of INS (inertial navigation system) and TAP (terrain-aided positioning). The solution is characterized by a joint design of INS and TAP, meaning that the highly nonlinear TAP is not designed separately but jointly with the INS using one and the same filter. The applied filter extends the theory of the MPF (marginalized particle filter) given by. The key idea with MPF is to estimate the nonlinear part using the particle filter (PF), and the part which is linear, conditional upon the nonlinear part, is estimated using the Kalman filter. The extension lies in the possibility to deal with a third multimodal part, where the discrete mode variable is also estimated jointly with the linear and nonlinear parts. Conditionally upon the mode and the nonlinear part, the resulting subsystem is linear and estimated using the Kalman filter. Given the nonlinear motion equations which the INS uses to compute navigation data, the INS equations must be linearized for the MPF to work. A set of linearized equations is derived and the linearization errors are shown to be insignificant with respect to the final result. Simulations are performed and the result indicates near-optimal accuracy when compared with the Cramer-Rao lower bound.