Extended Kalman Filter and Sigma Point Filter

The purpose of this work is to compare the estimation performance of EKF, SPF, AEKF, and ASPF algorithms, given real sensor data and groundtruth. We seek to estimate the pose (translation and rotation) of a mobile robot driving in a planar environment amongst numerous plastic markers given odometry (forward and angular velocity) measurements and laser rangefinder (range and bearing) measurements. The AEKF and ASPF we derive, implement, and compare are different than those previously presented in the literature for similar mobile robot estimation problems. We wish to identify which algorithms provide precise and consistent state estimates. By consistent we mean the true error uncertainty is captured by the estimated error uncertainty. The paper is organized as follows: in Sec. II we describe the robot motion and observation models, and the experiment performed to generate data. In Secs. III, IV, V, and VI we present EKF, AEKF, SPF, and ASPF algorithms and their filtering results. The paper closes with a brief comparison of the filters, and some additional comments.
∗ Ph.D.
Candidate, Student Memeber AIAA. forbes@utias.utoronto.ca
1 of 15 American Institute Aeronautics Copyright © 2010 by the American Institute of Aeronautics and Astronautics, Inc. All of rights reserved. and Astronautics
AIAA Guidance, Navigation, and Control Conference 2 - 5 August 2010, Toronto, Ontario Canada
AIAA 2010-7748
Extended Kalman Filter and Sigma Point Filter Approaches to Adaptive Filtering
James Richard Forbes∗
University of Toronto Institute for Aerospace Studies
Downloaded by NATIONAL UNIVERSITY OF DEFENSE on April 13, 2014 | | DOI: 10.2514/6.2010-7748
4925 Dufferin Street, Toronto, Ontario, Canada, M3H 5T6
We compare the state estimation performance of various filters using experimental data. The experiment, a mobile robot driving on a planar surface, provides noisy odometry and laser rangefinder measurements, along with groundtruth provided by an accurate motion capture system. The sensor noise statistics are not purely normal. We investigate the performance of standard extended Kalman and sigma point filters, and compare their performance to adaptive extended Kalman and adaptive sigma point filters. The adaptive filters update the noise covariance matrices based on the measurements available at a given time step.stimate the state of a system given a set of noisy measurements is a common goal in many branches of engineering. For example, estimating the position, orientation, and velocity of a robotic manipulator, a satellite on orbit, or a mobile robot are common (nonlinear) state estimation problems. Without an accurate state estimate, control of the system, that is achieving a desired state, is rendered impossible. The Kalman filter1, 2 is considered the cornerstone of linear estimation theory. Its nonlinear extension, the extended Kalman filter (EKF),3, 4 is equally popular for nonlinear state estimation problems. Although the EKF is perhaps the most widely used nonlinear filter, the sigma point filter (SPF, also known as the unscented Kalman filter, or UKF) has proven to be better suited to nonlinear state estimation problems.5–7 The SPF is slowly being employed in industrial applications over the EKF (for example, see Ref. 8). Although it is assumed that the noise statistics of a system are well known and constant, these assumptions are generally false. Additionally, real noise statistics are almost never truly Gaussian. Noise statistics can often be estimated based on previous experience or measurements made in a laboratory setting. However, the real noise statistics will never be truly known, and often change over time due to changes in the system (for example, a vehicles pneumatic tire slowly deflating) or surrounding environment (for example, the surrounding vehicle temperature). Although EKF or SPF performance may be satisfactory given a priori estimated or measured noise properties (which are assumed constant), superior state estimates may be realized by adaptively updating the noise statistics given a suite of sensor measurements. Popular adaptive EKF (AEKF) methods are those of Myers & Tapley9–11 and Maybeck,3 although given certain assumptions the methods are considered equivalent.12 A similar AEKF algorithm can be found in Busse et al.13 Adaptive SPF (ASPF) algorithms are presented in Lee & Alfriend,14 Jiang et al.,15 and Song & Han.16 As pointed out by Maybeck (Ref. 3, pg. 128), although the noise statistics are adaptively updated, the primary objective of adaptation is to improve the performance of the state estimate. Thus, estimating the true noise statistics is really a means to an end. Mobile robot localization is a challenging problem; the motion of the robot is nonlinear, and the measurements available are corrupted by noise that is often difficult to estimate or model. Mobile robot localization using adaptive filters has been explored previously in Jetto et al.,17 Lippiello et al.,18 and Caballero et al.19 where AEKFs were used, and in Song & Han16 where an ASPF was used.