Nonlinear Fisher Particle Output Feedback Control and its application to Terrain Aided Navigation

This paper presents state estimation and stochastic optimal control gathered in one global optimization problem generating dual effect i.e. the control can improve the future estimation. As the optimal policy is impossible to compute, a sub-optimal policy that preserves this coupling is constructed thanks to the Fisher Information Matrix (FIM) and a Particle Filter. This method has been applied to the localization and guidance of a drone over a known terrain with height measurements only. The results show that the new method improves the estimation accuracy compared to nominal trajectories.