GMKF: Generalized Moment Kalman Filter for Polynomial Systems with Arbitrary Noise

TL;DR

A new filtering approach for state estimation in polynomial systems corrupted by arbitrary noise and a GMKF for recursive state estimation in polynomial systems with arbitrary noise is developed, which performs well under highly non-Gaussian noise and outperforms common alternatives.

Abstract

This paper develops a new filtering approach for state estimation in polynomial systems corrupted by arbitrary noise, which commonly arise in robotics. We first consider a batch setup where we perform state estimation using all data collected from the initial to the current time. We formulate the batch state estimation problem as a Polynomial Optimization Problem (POP) and relax the assumption of Gaussian noise by specifying a finite number of moments of the noise. We solve the resulting POP using a moment relaxation and prove that under suitable conditions on the rank of the relaxation, (i) we can extract a provably optimal estimate from the moment relaxation, and (ii) we can obtain a belief representation from the dual (sum-of-squares) relaxation. We then turn our attention to the filtering setup and apply similar insights to develop a GMKF for recursive state estimation in polynomial systems with arbitrary noise. The GMKF formulates the prediction and update steps as POPs and solves them using moment relaxations, carrying over a possibly non-Gaussian belief. In the linear-Gaussian case, GMKF reduces to the standard Kalman Filter. We demonstrate that GMKF performs well under highly non-Gaussian noise and outperforms common alternatives, including the Extended and Unscented Kalman Filter, and their variants on matrix Lie group.

Figures (8)

• Figure 1: We generalize the classical Kalman Filter (KF) to operate on polynomial systems with arbitrary noise. In linear Gaussian systems, the noise is described by its mean and covariance and the KF "summarizes" past measurements into a Gaussian belief, whose negative log-likelihood takes the form $\|{\bm x} - \hat{{\bm x}}\|^2_{{\bm \Sigma}^{-1}}$ (up to constants). The proposed GMKF generalizes the KF by considering an arbitrary number of moments of the noise, and summarizes the measurements into a Sum-of-Squares belief, which generalizes the quadratic form of the KF to include higher-order moments (the vector-valued function ${\bm \phi}_r({\bm x})$ contains monomials of degree up to $r$).
• Figure 2: Binary noise
• Figure 3: Trigonometric noise
• Figure 4: Average estimation errors for BLUE, batch BPUE, and recursive BPUE (update-only GMKF) for increasing noise scale, for the case of (a) binary noise and (b) trigonometric noise.
• Figure 5: Comparison of the empirical error covariances for the BLUE and BPUE estimates over 500 Monte Carlo runs and for noise scale $s=1$. Blue dots correspond to BLUE estimation errors, while orange dots correspond to BPUE estimation errors.

Theorems & Definitions (20)

• Definition 1: Equality-constrained POP
• Definition 2: Moment Relaxation of \ref{['prob:pop']} lasserre2001globallasserre2015introduction
• Definition 3: SOS Relaxation of \ref{['prob:pop']} parrilo2003semidefinite
• Definition 4: Best Polynomial Unbiased Estimator
• Proposition 1
• Definition 5: Quadratic Belief
• Proposition 1: Quadratic Belief of
• Definition 6: SOS Belief
• Theorem 2: SOS Belief of Unconstrained BPUE
• Definition 7: Optimality Condition of Rank-1 \ref{['prob:pop_sdp']}