Backpropagation-Based Analytical Derivatives of EKF Covariance for Active Sensing

TL;DR

Novel backpropagation analytical formulas for the derivatives of the covariance matrices of an EKF w.r.t. all its inputs are derived, leveraging the obtained analytical gradients as an enabling technology to derive perception-aware optimal motion plans.

Abstract

To enhance accuracy of robot state estimation, active sensing (or perception-aware) methods seek trajectories that maximize the information gathered by the sensors. To this aim, one possibility is to seek trajectories that minimize the (estimation error) covariance matrix output by an extended Kalman filter (EKF), w.r.t. its control inputs over a given horizon. However, this is computationally demanding. In this article, we derive novel backpropagation analytical formulas for the derivatives of the covariance matrices of an EKF w.r.t. all its inputs. We then leverage the obtained analytical gradients as an enabling technology to derive perception-aware optimal motion plans. Simulations validate the approach, showcasing improvements in execution time, notably over PyTorch's automatic differentiation. Experimental results on a real vehicle also support the method.

Figures (9)

• Figure 1: Dependencies of EKF's variables in Riccati equations \ref{['eq:riccati_propagation']} and \ref{['eq:information_update']}. Each variable (node) is a function of its predecessors.
• Figure 2: Dependency diagram of all the variables involved in an EKF.
• Figure 3: On the left, an example of an initial random guess in blue. The other two trajectories are solutions to the perception-aware problem where the loss is the trace (in orange) and the Schatten norm (in green). The right plot shows the expected trace of the covariance evolution for each trajectory.
• Figure 4: Absolute estimation error of the lever arm during the trajectory. On the left the error for the lever arm in x and on the right in y. One-$\sigma$ envelope illustrates the dispersion of errors over trials.
• Figure 5: Off-road trajectories of the RTK-GPS in a local tangent plane coordinates oriented East-North-Up of the scenario at 5 km/h.