102 research outputs found
Autonomous navigation with constrained consistency for C-Ranger
Autonomous underwater vehicles (AUVs) have become the most widely used tools for undertaking complex exploration tasks in marine environments. Their synthetic ability to carry out localization autonomously and build an environmental map concurrently, in other words, simultaneous localization and mapping (SLAM), are considered to be pivotal requirements for AUVs to have truly autonomous navigation. However, the consistency problem of the SLAM system has been greatly ignored during the past decades. In this paper, a consistency constrained extended Kalman filter (EKF) SLAM algorithm, applying the idea of local consistency, is proposed and applied to the autonomous navigation of the C-Ranger AUV, which is developed as our experimental platform. The concept of local consistency (LC) is introduced after an explicit theoretical derivation of the EKF-SLAM system. Then, we present a locally consistency-constrained EKF-SLAM design, LC-EKF, in which the landmark estimates used for linearization are fixed at the beginning of each local time period, rather than evaluated at the latest landmark estimates. Finally, our proposed LC-EKF algorithm is experimentally verified, both in simulations and sea trials. The experimental results show that the LC-EKF performs well with regard to consistency, accuracy and computational efficiency
Convergence and Consistency Analysis for A 3D Invariant-EKF SLAM
In this paper, we investigate the convergence and consistency properties of
an Invariant-Extended Kalman Filter (RI-EKF) based Simultaneous Localization
and Mapping (SLAM) algorithm. Basic convergence properties of this algorithm
are proven. These proofs do not require the restrictive assumption that the
Jacobians of the motion and observation models need to be evaluated at the
ground truth. It is also shown that the output of RI-EKF is invariant under any
stochastic rigid body transformation in contrast to based EKF
SLAM algorithm (-EKF) that is only invariant under
deterministic rigid body transformation. Implications of these invariance
properties on the consistency of the estimator are also discussed. Monte Carlo
simulation results demonstrate that RI-EKF outperforms -EKF,
Robocentric-EKF and the "First Estimates Jacobian" EKF, for 3D point feature
based SLAM
KD-EKF: A Consistent Cooperative Localization Estimator Based on Kalman Decomposition
In this paper, we revisit the inconsistency problem of EKF-based cooperative
localization (CL) from the perspective of system decomposition. By transforming
the linearized system used by the standard EKF into its Kalman observable
canonical form, the observable and unobservable components of the system are
separated. Consequently, the factors causing the dimension reduction of the
unobservable subspace are explicitly isolated in the state propagation and
measurement Jacobians of the Kalman observable canonical form. Motivated by
these insights, we propose a new CL algorithm called KD-EKF which aims to
enhance consistency. The key idea behind the KD-EKF algorithm involves perform
state estimation in the transformed coordinates so as to eliminate the
influencing factors of observability in the Kalman observable canonical form.
As a result, the KD-EKF algorithm ensures correct observability properties and
consistency. We extensively verify the effectiveness of the KD-EKF algorithm
through both Monte Carlo simulations and real-world experiments. The results
demonstrate that the KD-EKF outperforms state-of-the-art algorithms in terms of
accuracy and consistency
Towards consistent visual-inertial navigation
Visual-inertial navigation systems (VINS) have prevailed in various applications, in part because of the complementary sensing capabilities and decreasing costs as well as sizes. While many of the current VINS algorithms undergo inconsistent estimation, in this paper we introduce a new extended Kalman filter (EKF)-based approach towards consistent estimates. To this end, we impose both state-transition and obervability constraints in computing EKF Jacobians so that the resulting linearized system can best approximate the underlying nonlinear system. Specifically, we enforce the propagation Jacobian to obey the semigroup property, thus being an appropriate state-transition matrix. This is achieved by parametrizing the orientation error state in the global, instead of local, frame of reference, and then evaluating the Jacobian at the propagated, instead of the updated, state estimates. Moreover, the EKF linearized system ensures correct observability by projecting the most-accurate measurement Jacobian onto the observable subspace so that no spurious information is gained. The proposed algorithm is validated by both Monte-Carlo simulation and real-world experimental tests.United States. Office of Naval Research (N00014-12-1- 0093, N00014-10-1-0936, N00014-11-1-0688, and N00014-13-1-0588)National Science Foundation (U.S.) (Grant IIS-1318392
Consistent Right-Invariant Fixed-Lag Smoother with Application to Visual Inertial SLAM
State estimation problems without absolute position measurements routinely
arise in navigation of unmanned aerial vehicles, autonomous ground vehicles,
etc., whose proper operation relies on accurate state estimates and reliable
covariances. Unaware of absolute positions, these problems have immanent
unobservable directions. Traditional causal estimators, however, usually gain
spurious information on the unobservable directions, leading to over-confident
covariance inconsistent with actual estimator errors. The consistency problem
of fixed-lag smoothers (FLSs) has only been attacked by the first estimate
Jacobian (FEJ) technique because of the complexity to analyze their
observability property. But the FEJ has several drawbacks hampering its wide
adoption. To ensure the consistency of a FLS, this paper introduces the right
invariant error formulation into the FLS framework. To our knowledge, we are
the first to analyze the observability of a FLS with the right invariant error.
Our main contributions are twofold. As the first novelty, to bypass the
complexity of analysis with the classic observability matrix, we show that
observability analysis of FLSs can be done equivalently on the linearized
system. Second, we prove that the inconsistency issue in the traditional FLS
can be elegantly solved by the right invariant error formulation without
artificially correcting Jacobians. By applying the proposed FLS to the
monocular visual inertial simultaneous localization and mapping (SLAM) problem,
we confirm that the method consistently estimates covariance similarly to a
batch smoother in simulation and that our method achieved comparable accuracy
as traditional FLSs on real data.Comment: 13 pages, 4 figures, AAAI 2021 Conferenc
Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation
This paper derives a contact-aided inertial navigation observer for a 3D
bipedal robot using the theory of invariant observer design. Aided inertial
navigation is fundamentally a nonlinear observer design problem; thus, current
solutions are based on approximations of the system dynamics, such as an
Extended Kalman Filter (EKF), which uses a system's Jacobian linearization
along the current best estimate of its trajectory. On the basis of the theory
of invariant observer design by Barrau and Bonnabel, and in particular, the
Invariant EKF (InEKF), we show that the error dynamics of the point
contact-inertial system follows a log-linear autonomous differential equation;
hence, the observable state variables can be rendered convergent with a domain
of attraction that is independent of the system's trajectory. Due to the
log-linear form of the error dynamics, it is not necessary to perform a
nonlinear observability analysis to show that when using an Inertial
Measurement Unit (IMU) and contact sensors, the absolute position of the robot
and a rotation about the gravity vector (yaw) are unobservable. We further
augment the state of the developed InEKF with IMU biases, as the online
estimation of these parameters has a crucial impact on system performance. We
evaluate the convergence of the proposed system with the commonly used
quaternion-based EKF observer using a Monte-Carlo simulation. In addition, our
experimental evaluation using a Cassie-series bipedal robot shows that the
contact-aided InEKF provides better performance in comparison with the
quaternion-based EKF as a result of exploiting symmetries present in the system
dynamics.Comment: Published in the proceedings of Robotics: Science and Systems 201
- …