A Novel EM Implementation for Initial Alignment of SINS Based on Particle Filter and Particle Swarm Optimization

For nonlinear systems in which the measurement noise parameters vary over time, adaptive nonlinear filters can be applied to precisely estimate the states of systems. The expectation maximization (EM) algorithm, which alternately takes an expectation- (E-) step and a maximization- (M-) step, has been proposed to construct a theoretical framework for the adaptive nonlinear filters. Previous adaptive nonlinear filters based on the EM employ analytical algorithms to develop the two steps, but they cannot achieve high filtering accuracy because the strong nonlinearity of systems may invalidate the Gaussian assumption of the state distribution. In this paper, we propose an EM-based adaptive nonlinear filter APF to solve this problem. In the E-step, an improved particle filter PF_new is proposed based on the Gaussian sum approximation (GSA) and the Monte Carlo Markov chain (MCMC) to achieve the state estimation. In the M-step, the particle swarm optimization (PSO) is applied to estimate the measurement noise parameters. The performances of the proposed algorithm are illustrated in the simulations with Lorenz 63 model and in a semiphysical experiment of the initial alignment of the strapdown inertial navigation system (SINS) in large misalignment angles.