We study the problem of devising a closed-loop strategy to control the position of a robot that is tracking a possibly moving target. The robot is capable of obtaining noisy measurements of the target's position. The key idea in active target tracking is to choose control laws that drive the robot to measurement locations that will reduce the uncertainty in the target's position. The challenge is that measurement uncertainty often is a function of the (unknown) relative positions of the target and the robot. Consequently, a closed-loop control policy is desired which can map the current estimate of the target's position to an optimal control law for the robot. Our main contribution is to devise a closed-loop control policy for target tracking that plans for a sequence of control actions, instead of acting greedily. We consider scenarios where the noise in measurement is a function of the state of the target. We seek to minimize the maximum uncertainty (trace of the posterior covariance matrix) over all possible measurements. We exploit the structural properties of a Kalman Filter to build a policy tree that is orders of magnitude smaller than naive enumeration while still preserving optimality guarantees. We show how to obtain even more computational savings by relaxing the optimality guarantees. The resulting algorithms are evaluated through simulations.
@cite_12 presented an algorithm that computes the optimal scheduling of measurements for a linear dynamical system. Their formulation does not directly model a target tracking problem. Instead, the goal is to track a linear dynamical system using a set of sensors such that one sensor can be activated at any time instance. The posterior covariance in estimating a linear system in a Kalman filter depends on the prior covariance and sensor variance but not on the actual measurement values (unlike the case in non-linear systems). Thus, one can build a search tree enumerating all possible sensor selections and choosing the one that minimizes the final covariance. The main contribution of was to present a pruning technique to reduce the size of the tree while still preserving optimality.
@cite_12
2160024900 2119940423 2056358962 2735532146
Consider a set of sensors estimating the state of a process in which only one of these sensors can operate at each time-step due to constraints on the overall system. The problem addressed here is to choose which sensor should operate at each time-step to minimize a weighted function of the error covariance of the state estimation at each time-step. This work investigates the development of tractable algorithms to solve for the optimal and suboptimal sensor schedule. First, a condition on the non-optimality of an initialization of the schedule is presented. Second, using this condition, both an optimal and a suboptimal algorithm are devised to prune the search tree of all possible sensor schedules. This pruning enables the solution of larger systems and longer time horizons than with enumeration alone. The suboptimal algorithm trades off the quality of the solution and the complexity of the problem through a tuning parameter. Third, a hierarchical algorithm is formulated to decrease the computation time of the suboptimal algorithm by using results from a low complexity solution to further prune the tree. Numerical simulations are performed to demonstrate the performance of the proposed algorithms. We consider the optimal sensor scheduling problem formulated as a partially observed Markov decision process (POMDP). Due to operational constraints, at each time instant, the scheduler can dynamically select one out of a finite number of sensors and record a noisy measurement of an underlying Markov chain. The aim is to compute the optimal measurement scheduling policy, so as to minimize a cost function comprising of estimation errors and measurement costs. The formulation results in a nonstandard POMDP that is nonlinear in the information state. We give sufficient conditions on the cost function, dynamics of the Markov chain and observation probabilities so that the optimal scheduling policy has a threshold structure with respect to a monotone likelihood ratio (MLR) ordering. As a result, the computational complexity of implementing the optimal scheduling policy is inexpensive. We then present stochastic approximation algorithms for estimating the best linear MLR order threshold policy. In this paper, we focus on the problem of motion tracking in unknown environments using visual and inertial sensors. We term this estimation task visual-inertial odometry (VIO), in analogy to the well-known visual-odometry problem. We present a detailed study of extended Kalman filter (EKF)-based VIO algorithms, by comparing both their theoretical properties and empirical performance. We show that an EKF formulation where the state vector comprises a sliding window of poses (the multi-state-constraint Kalman filter (MSCKF)) attains better accuracy, consistency, and computational efficiency than the simultaneous localization and mapping (SLAM) formulation of the EKF, in which the state vector contains the current pose and the features seen by the camera. Moreover, we prove that both types of EKF approaches are inconsistent, due to the way in which Jacobians are computed. Specifically, we show that the observability properties of the EKF's linearized system models do not match those of the underlying system, which causes the filters to underestimate the uncertainty in the state estimates. Based on our analysis, we propose a novel, real-time EKF-based VIO algorithm, which achieves consistent estimation by (i) ensuring the correct observability properties of its linearized system model, and (ii) performing online estimation of the camera-to-inertial measurement unit (IMU) calibration parameters. This algorithm, which we term MSCKF 2.0, is shown to achieve accuracy and consistency higher than even an iterative, sliding-window fixed-lag smoother, in both Monte Carlo simulations and real-world testing. We consider the problem of scheduling a set of sensors to observe the state of a discrete-time linear system subject to a limited energy budget. Our goal is to devise a sensor schedule that minimizes the mean squared error (MSE) of an optimal estimator (i.e., the Kalman Filter). Both the minimum-MSE and the minimum-cardinality optimal sensor scheduling problems are inherently combinatorial, and computationally intractable. We remedy the combinatorial complexity by using a greedy heuristic; the greedy heuristic is guaranteed to return near-optimal solutions when minimizing supermodular objectives. While it is known that the MSE is not supermodular (with counterexamples available in literature), we provide conditions on the prior information matrix and on the observation matrix under which supermodularity holds. More specifically, we require the prior information matrix to be a strictly-diagonally-dominant M-matrix (plus an extra technical requirement on its inverse). Empirical results confirm that random M-matrices lead to supermodular problems, while this is not the case for generic prior information matrices. M-matrices naturally arise in estimation problems over networks and we provide a practical application of our findings to an energy-constrained multi robot localization problem.