No abstract

Abstract-This paper considers the problem of estimating the covariance of roto-translations computed by the Iterative Closest Point (ICP) algorithm. The problem is relevant for localization of mobile robots and vehicles equipped with depthsensing cameras (e.g., Kinect) or Lidar (e.g., Velodyne). The closed-form formulas for covariance proposed in previous literature generally build upon the fact that the solution to ICP is obtained by minimizing a linear least-squares problem. In this paper, we show this approach needs caution because the rematching step of the algorithm is not explicitly accounted for, and applying it to the point-to-point version of ICP leads to completely erroneous covariances. We then provide a formal mathematical proof why the approach is valid in the pointto-plane version of ICP, which validates the intuition and experimental results of practitioners.

Localization in indoor environments is a technique which estimates the robot's pose by fusing data from onboard motion sensors with readings of the environment, in our case obtained by scan matching point clouds captured by a low-cost Kinect depth camera. We develop both an Invariant Extended Kalman Filter (IEKF)-based and a Multiplicative Extended Kalman Filter (MEKF)-based solution to this problem. The two designs are successfully validated in experiments and demonstrate the advantage of the IEKF design.

No abstract

We introduce a magnetometer-plus-GPS aided inertial navigation system for a helicopter UAV. A nonlinear observer is required to estimate the navigation states, typically an Extended Kalman Filter (EKF). A novel approach is the invariant observer, a constructive design method applicable to systems possessing symmetries. We review the theory and design an invariant observer for our example. Using an invariant observer guarantees a simplified form of the nonlinear estimation error dynamics. These are stabilized using an adaptation of the Invariant EKF, a systematic approach to compute the gains of an invariant observer. The resulting design is successfully implemented and validated in experiment and shows an improvement in performance over a conventional EKF.