Backpropagation-Based Analytical Derivatives of EKF Covariance for Active Sensing

To enhance accuracy of robot state estimation, perception-aware (or active sensing) methods seek trajectories that minimize uncertainty. To this aim, one possibility is to seek trajectories that minimize the final covariance of an extended Kalman filter (EKF), w.r.t. its control inputs over a given horizon. However, this can be computationally demanding. In this article, we derive novel backpropagation analytical formulas for the derivatives of the final covariance of an EKF w.r.t. its inputs. We then leverage the obtained gradients as an enabling technology to derive perception-aware optimal motion plans. Simulations validate the approach, showcasing improvements in both estimation accuracy and execution time. Experimental results on a real large ground vehicle also support the method.