Discovering and exploiting active sensing motifs for estimation

TL;DR

A framework to refine sporadic estimates from bouts of active sensing that combines data-driven state and observability estimation from artificial neural networks with model-based estimation, which is called the Augmented Information Kalman Filter (AI-KF) and is validated using altitude estimation given GPS-denied data from an outdoor quadcopter flight.

Abstract

From organisms to machines, autonomous systems rely on measured sensory cues to estimate unknown information about themselves or their environment. For nonlinear systems, carefully selected sensor motion can be exploited to extract information that is otherwise unavailable, i.e. active sensing. Empirical, yet mathematically rigorous, tools are needed to (1) quantify how sensor movement can contribute to estimation performance, and (2) leverage this knowledge to improve state estimates. Here, we introduce "BOUNDS: Bounding Observability for Uncertain Nonlinear Dynamic Systems", and Python package pybounds, which can discover patterns of sensor motion that increase information for individual state variables. Crucially, it is suitable for partially observable nonlinear systems, accounts for sensor noise, and can be applied to either simulated or observed trajectories. We demonstrate BOUNDS through a case study on a flying agent with limited sensors, showing how active sensing can be leveraged to estimate key variables such as ground speed, altitude, and ambient wind direction. Finally, we present a framework to refine sporadic estimates from bouts of active sensing that combines data-driven state and observability estimation from artificial neural networks with model-based estimation, which we call the Augmented Information Kalman Filter (AI-KF). We validate our framework using altitude estimation given GPS-denied data from an outdoor quadcopter flight. Collectively, our work will help decode active sensing strategies and inform the design of estimation algorithms in sensorimotor systems.

Figures (11)

• Figure 1: Overview of BOUNDS, a method for discovering active sensing motifs with empirical observability.a. A simulated (or measured) state trajectory (in this case, of a flying agent) that is described by a collection of time-series of observed state variables $\mathbf{\tilde{X}}$. b. Model predictive control (MPC) is used to find the time-series of control inputs $\mathbf{U}$ required to reconstruct the state trajectory in a through a simulation model. c. The MPC control inputs $\mathbf{U}$ are used to simulate the dynamics associated with the state trajectory, yielding the nominal reconstructed trajectory $\mathbf{X}$. A measurement model $\mathbf{h}(\mathbf{x}_k, \mathbf{u}_k)$ simulates the sensor measurements. d. Each initial state variable $x_{i,0}$ is perturbed in the positive $x_{i,0}^{i+}$ and negative $x_{i,0}^{i-}$ directions by $\epsilon$. Then the corresponding measurements over time, $\mathbf{Y}^{i+}$ and $\mathbf{Y}^{i-}$, are computed. The difference between these measurements is used to calculate a numerical Jacobian: $\Delta \mathbf{Y}^{i} / 2\epsilon$, yielding one column of the observability matrix $\mathcal{O}$. e. An illustration of an observability matrix $\mathcal{O}$, where the color grid indicates the values in the matrix (corresponding to d). The columns indicate the difference of the initial state perturbation $\Delta x_{i,0}$ and the rows indicate the corresponding difference in measurements $\Delta \mathbf{y}_k$ over time for a discrete number of time steps $\omega$. f. An illustration of the Fisher information matrix, calculated as $\mathcal{F} = \mathcal{O}^{\intercal} \mathcal{R}^{-1} \mathcal{O}$, where $\mathcal{R}$ is the block diagonal sensor noise covariance matrix. Colors indicate matrix values as in e. g. The inverse of the Fisher information matrix $\mathcal{F}^{-1}$, which encodes the minimum error covariance of the estimate of the state's initial condition $\mathbf{x}_0$. The diagonal elements represent the minimum error variance of each individual state variable---inversely correlated with the observability level---where smaller values indicate a higher observability level. h. The minimum error variance time-series (normalized units) of all state variables (diagonal of $\mathcal{F}^{-1}$) over time for the first three sensors $[y_1, y_2, y_3]$ (left) and sensors $[y_2, \cdots, y_p]$ (right). Note that some states are persistently observable or unobservable, while other states exhibit time-varying observability, which is dependent on the sensor set. i. The minimum error variance time-series of the first state variable $x_1$ for the first three sensors $[y_1, y_2, y_3]$ (left) and the corresponding correlation with the active sensing motifs in the simulated trajectory (right) from a. Time-series corresponds to the first column in h. (left). By correlating the time-series with the trajectory, we reveal that only turns in heading improve the observability of $x_1$. To make the relationship between temporal patterns in observability and the features of the trajectory more intuitive, we shift the time series back in time by half of the sliding window length, $\omega/2$. j. Same as i but for state variable $x_2$ and sensors $[y_2, \cdots, y_p]$. Corresponds to the second column in h (right). This sensor set renders acceleration/deceleration and turns offset to heading observable, but headings turns have no effect.
• Figure 2: Evaluating putative active sensing motifs.a. Illustration of the trigonometric and sensor kinematics of a flying agent in the presence of ambient wind in the XY plane (left) and XZ plane (right). b. Each vector quantity in a is decomposed into a magnitude and angle, which can be considered separate measurements for observability analysis. c. The observability (minimum error variance) of the same simulated flight trajectory for different state variables (columns) and different subsets of measurements from b (rows). Observability was calculated for 0.5s time windows, corresponding to 5 discrete measurements ($\omega=5$). Note that there are complex interactions between the state variable of interest, the available measurements, and the movement patterns corresponding to an increase in observability.
• Figure 3: Observability-informed state estimation.a. Artificial neural network (ANN) state estimator for wind direction ($\mathcal{H}_{\zeta}$). Inputs are the yaw heading angle ($\psi$), apparent wind angle ($\gamma$), and course direction angle ($\beta$) in the time window $\omega$. b. A subset of simulated flying agent trajectories used for training and testing the ANN state estimator. $N=40,000$ trajectories total. c. The distribution (bottom) and cumulative distribution function (top) of wind direction observability ($\mathcal{F}^{-1}_{\zeta}$) across all training and testing trajectories. d. A comparison of the testing error variance for the wind direction state estimator ANN ($\mathcal{H}_{\zeta}$) when trained on different percentages of the training data---for data sorted by observability (sorted high-to-low, top) and randomly sorted (bottom). The y-axis indicates how much data was used in training, and the x-axis indicates what bin percentile was used to test the ANN (testing data always sorted by observability). e. Same as k. but showing the relative change in error variance between observability-sorted and randomly sorted training data. Red values ($<$ 0) indicate better performance when sorting by observability. f. The mean observability $\text{mean}(\mathcal{F}^{-1}_{\zeta})$ of each bin in the training dataset vs the ANN error variance $\text{Var}(\check{\zeta} - \zeta)$ for ANN's trained on different percentages of the training dataset for observability-sorted (left) and randomly sorted (right) training data. The theoretical bound of the ANN performance is shown as a dashed line (bounded at 1 for circular data). g. (top) An example simulated trajectory of a flying agent in the presence of time-varying wind direction. Color along trajectory indicates observability. (bottom) A comparison of true wind direction (${\zeta}$), ANN wind direction estimate ($\check{\zeta}$), and the observability-filtered estimate ($\hat{\zeta}$). Measurement noise covariance was set to $\mathcal{R} = 10^{-3} I_{p \times p}$.
• Figure 4: Augmented Information Kalman Filter.a. Overview of the Augmented Information Kalman Filter (AI-KF) state estimation framework. The measurement vector $\mathbf{y}_k$ is augmented with a data-driven state estimate $\check{x}_{i,k}$. The corresponding state noise covariance is estimated $\check{\mathcal{R}}_{i, k}$ and used to dynamically adjust the full augmented noise covariance matrix $\mathcal{R}'_k$ based on the state estimate accuracy (or observability). b. (left) The AI-KF framework is applied to estimate altitude for a simulated trajectory consisting of straight flight with a brief deceleration and sequential acceleration. Altitude is observable during acceleration/deceleration (see \ref{['fig:figure_2_c']}v) given measurements of forward optic flow, forward acceleration, and vertical acceleration. (right) Noisy measurements used in our simulation with additive zero-mean Gaussian noise with a variance of $10^{-2}$ for each measurement. There is also a brief period of biased (nonzero-mean) noise added to the vertical acceleration measurement. c. A comparison of an Unscented Kalman Filter (UKF, purple) and Augmented Information Unscented Kalman Filter (AI-UKF, green) state estimation results with varied initial altitude state estimates, for the trajectory in b. The shading around the curve insinuates the 2-$\sigma$ estimate bound. The noise variance of the augmented altitude measurement $\mathrm{Var}(\check{z})$ (top, pink scaled) was dynamically adjusted proportionally to the mean acceleration magnitude in the time window $\omega$. d. A comparison of the median error of the UKF and AI-UKF for varying process noise covariance matrix ($Q$) scales and varying initial state covariance matrix ($P_0$) scales for varying values of the initial altitude estimate. The results in d correspond to the square with a bold outline. e. Comparison of the UKF and AI-UKF median error across different mean accelerations for varying values of the initial altitude estimate. The AI-UKF always performs better or equal to the vanilla UKF. The advantage of the AI-UKF is most pronounced for small but non-zero levels of acceleration.
• Figure 5: Augmented Information Kalman Filter application for outdoor quadcopter flight.a. Quadcopter (DJI Matrice 300 RTK) flying in an outdoor environment with a representative ventral camera image and computed optic flow vectors. b. An experimental measured quadcopter trajectory with estimated altitude observability computed using the approach in \ref{['fig:figure_1']}, illustrating how our BOUNDS framework can be used to determine observability for measured, rather than simulated, trajectories. c. Same as \ref{['fig:figure_4_c']} but for experimental data from the trajectory in b. As in \ref{['fig:figure_4_c']}, the clear benefit of the AI-UKF is its robustness to different initial conditions and faster convergence to correct estimates.