Constructive Equivariant Observer Design for Inertial Navigation

Inertial Navigation Systems (INS) are algorithms that fuse inertial measurements of angular velocity and specific acceleration with supplementary sensors including GNSS and magnetometers to estimate the position, velocity and attitude, or extended pose, of a vehicle. The industry-standard extended Kalman filter (EKF) does not come with strong stability or robustness guarantees and can be subject to catastrophic failure. This paper exploits a Lie group symmetry of the INS dynamics to propose the first nonlinear observer for INS with error dynamics that are almost-globally asymptotically and locally exponentially stable, independently of the chosen gains. The observer is aided only by a GNSS measurement of position. As expected, the convergence guarantee depends on persistence of excitation of the vehicle's specific acceleration in the inertial frame. Simulation results demonstrate the observer's performance and its ability to converge from extreme errors in the initial state estimates.Comment: 10 pages, 2 figures, to appear in Proceedings of IFAC World Congress 202

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

Real-time Aerial Magnetic and Vision-aided Navigation

Aerial magnetic navigation has shown to be a viable alternative navigation method that has the potential for world-wide availability, to include over oceans. Obtaining GPS-level accuracy using magnetic navigation alone is challenging, but magnetic navigation can be combined with other alternative navigation methods that are more posed to obtaining GPS-level accuracy in their current state. This research presents an aerial navigation solution combining magnetic navigation and vision-aided navigation to aid an inertial navigation system (INS). The navigation solution was demonstrated in real-time playback using simulated magnetic field measurements and flight-test captured visual imagery. Additionally, the navigation solution was flight-tested on a USAF F-16 to demonstrate magnetic navigation in the challenging magnetic environment seen on operationally representative dynamic platforms

Rigid Body Attitude Estimation: An Overview and Comparative Study

The attitude estimation of rigid body systems has attracted the attention of many researchers over the years. The development of efficient estimation algorithms that can accurately estimate the orientation of a rigid body is a crucial step towards a reliable implementation of control schemes for underwater and flying vehicles. The primary focus of this thesis consists in investigating various attitude estimation techniques and their applications. Two major classes are discussed. The first class consists of the earliest static attitude determination techniques relying solely on a set of body vector measurements of known vectors in the inertial frame. The second class consists of dynamic attitude estimation and filtering techniques, relying on body vector measurements as well other measurements, and using the dynamical equations of the system under consideration. Various attitude estimation algorithms, including the latest nonlinear attitude observers, are presented and discussed, providing a survey that covers the evolution and structural differences of these estimation methods. Simulation results have been carried out for a selected number of such attitude estimators. Their performance in the presence of noisy measurements, as well as their advantages and disadvantages are discussed

Solutions and algorithms for inertial navigation of railroad vehicles

Obiettivo di questa tesi è lo studio e lo sviluppo di soluzioni innovative di navigazione inerziale per applicazioni ferroviarie, strumento utile per il tracciamento del moto durante l'assenza prolungata di sistemi di localizzazione esterni, tipo GPS, come può avvenire in galleria. Definiti gli strumenti di lavoro, è stata poi eseguita un'analisi dello stato dell'arte al fine di mettere in evidenza le metodologie teoriche utilizzate, nonchè le prestazioni dei sistemi già esistenti. Sono poi caratterizzati i sensori e le misure disponibili. Sono proposte varie soluzioni al problema della navigazione inerziale, con l'obiettivo di valutarne le prestazioni durante periodi prolungati assenza del GPS e con varie condizioni al contorno. Dopo una prima versione basata su un singolo EKF, si è scelto di svilupparne una seconda classe in cui il problema di stimadi assetto (AHRS) e diposizione/velocità sono separati e risolti mediante due algoritmi distinti. È stato implementato un AHRS basato su EKF e uno mediante un osservatore non lineare; inoltre, sono stati sviluppati un EKF di ordine completo e uno ridotto per le dinamiche di traslazione. È stata poi sviluppata una soluzione per l'integrazione dei dati delle mappe, in modo da fornire correzioni più frequenti all'INS, mantenendo inoltre un ridotto carico computazionale e facilità di integrazione. Si è infine proceduto implementando e simulando la soluzione a singolo stadio e le varie combinazioni di INS a due stadi in ambiente Matlab-Simulink. Gli algoritmi a due stadi hanno mostrato in simulazione prestazioni migliori rispetto alla struttura a EKF singolo la quale presenta un dominio di convergenza troppo limitato per fini pratici. A conclusione del lavoro, svolto avvalendosi della collaborazione di Sadel, sono state gettate le basi per una successiva analisi atta a verificare se la struttura a due stadi consente la convergenza anche dei bias di accelerometr

Inertial navigation algorithms based on symmetry-preserving theory

Nowadays, the usage of low-cost sensors on Unmanned Aerial Vehicles (UAV) has become more popular due to the increase in small UAV platforms. Some of them are used for research on new algorithms in all the different parts of flight dynamics research like flight control, navigation, data fusion, etc. Furthermore, these low-cost and small sensors are used on a huge range of new applications such as filming, photography and agricultural uses. For all of these purposes, the UAVs rely on a host of sensors to position, navigate and compute all the necessary data for the application they are designed for. The key part in that process is to obtain accurate and reliable state estimations from available measurements. Taking into account the comparably weak performance of employed low-cost sensors, algorithms to estimate the measured and non-measured variables are of primary importance. In this thesis, three Inertial Navigation Algorithms for a quadrotor UAV are implemented and compared to show its essential advantage and drawbacks. The first two algorithms are both designed based on Symmetry-preserving theory and differ from each other by the type of estimation: a non-linear observer and an Extended Kalman Filter (EKF), whereas the third one is an EKF without the Symmetry-preserving theory behind it. Besides numeric simulations, these algorithms are applied to real-time data to be able to evaluate the properties of each algorithm

Homography-Based State Estimation for Autonomous Exploration in Unknown Environments

This thesis presents the development of vision-based state estimation algorithms to enable a quadcopter UAV to navigate and explore a previously unknown GPS denied environment. These state estimation algorithms are based on tracked Speeded-Up Robust Features (SURF) points and the homography relationship that relates the camera motion to the locations of tracked planar feature points in the image plane. An extended Kalman filter implementation is developed to perform sensor fusion using measurements from an onboard inertial measurement unit (accelerometers and rate gyros) with vision-based measurements derived from the homography relationship. Therefore, the measurement update in the filter requires the processing of images from a monocular camera to detect and track planar feature points followed by the computation of homography parameters. The state estimation algorithms are designed to be independent of GPS since GPS can be unreliable or unavailable in many operational environments of interest such as urban environments. The state estimation algorithms are implemented using simulated data from a quadcopter UAV and then tested using post processed video and IMU data from flights of an autonomous quadcopter. The homography-based state estimation algorithm was effective, but accumulates drift errors over time due to the relativistic homography measurement of position

All Source Sensor Integration Using an Extended Kalman Filter

The global positioning system (GPS) has become an ubiquitous source for navigation in the modern age, especially since the removal of selective availability at the beginning of this century. The utility of the GPS is unmatched, however GPS is not available in all environments. Heavy reliance on GPS for navigation makes the warfighter increasingly vulnerability as modern warfare continues to evolve. This research provides a method for incorporating measurements from a massive variety of sensors to mitigate GPS dependence. The result is the integration of sensor sets that encompass those examined in recent literature as well as some custom navigation devices. A full-state extended Kalman filter is developed and implemented, accommodating the requirements of the varied sensor sets and scenarios. Some 19 types of sensors are used in multiple quantities including inertial measurement units, single cameras and stereo pairs, 2D and 3D laser scanners, altimeters, 3-axis magnetometers, heading sensors, inclinometers, a stop sign sensor, an odometer, a step sensor, a ranging device, a signal of opportunity sensor, global navigation satellite system sensors, an air data computer, and radio frequency identification devices. Simulation data for all sensors was generated to test filter performance. Additionally, real data was collected and processed from an aircraft, ground vehicles, and a pedestrian. Measurement equations are developed to relate sensor measurements to the navigation states. Each sensor measurement is incorporated into the filter using the Kalman filter measurement update equations. Measurement types are segregated based on whether they observe instantaneous or accumulated state information. Accumulated state measurements are incorporated using delayed-state update equations. All other measurements are incorporated using the numerically robust UD update equations

Multiple-hypothesis vision-based landing autonomy

Unmanned aerial vehicles (UAVs) need humans in the mission loop for many tasks, and landing is one of the tasks that typically involves a human pilot. This is because of the complexity of a maneuver itself and flight-critical factors such as recognition of a landing zone, collision avoidance, assessment of landing sites, and decision to abort the maneuver. Another critical aspect to be considered is the reliance of UAVs on GPS (global positioning system). A GPS system is not a reliable solution for landing in some scenarios (e.g. delivering a package in an urban city, and a surveillance UAV repatriating a home ship with the jammed signals), and a landing solely based on a GPS extremely decreases the UAV operation envelope. Vision is promising to achieve fully autonomous landing because it is a rich-sensing, light, affordable device that functions without any external resource. Although vision is a powerful tool for autonomous landing, the use of vision for state estimation requires extensive consideration. Firstly, vision-based landing faces a problem of occlusion. The target detected at a high altitude would be lost at certain altitudes while a vehicle descends; however, a small visual target can not be recognized at high altitude. Second, standard filtering methods such as extended Kalman filter (EKF) face difficulty due to the complex dynamics of the measurement error created due to the discrete pixel space, conversion from the pixel to physical units, the complex camera model, and complexity of detection algorithms. The vision sensor produces an unfixed number of measurements with each image, and the measurements may include false positives. Plus, the estimation system is excessively tasked in realistic conditions. The landing site would be moving, tilted, or close to an obstacle. The available landing location may not be limited to one. In addition to assessing these statuses, understanding the confidence of the estimations is also the tasks of the vision, and the decisions to initiate, continue, and abort the mission are made based on the estimated states and confidence. The system that handles those issues and consistently produces the navigation solution while a vehicle lands eliminates one of the limitations of the autonomous UAV operation. This thesis presents a novel state estimation system for UAV landing. In this system, vision data is used to both estimate the state of the vehicle and map the state of the landing target (position, velocity, and attitude) within the framework of simultaneous localization and mapping (SLAM). Using the SLAM framework, the system becomes resilient to a loss of GPS and other sensor failures. A novel vision algorithm that detects a portion of the marker is developed, and the stochastic properties of the algorithm are studied. This algorithm extends the detectable range of the vision system for any known marker. However, this vision algorithm produces highly nonlinear, non-Gaussian, and multi-modal error distribution, and a naive implementation of filters would not accurately estimate the states. A vision-aided navigation algorithm is derived within extended Kalman particle filter (PF-EKF) and Rao-Blackwellized particle filter (RBPF) frameworks in addition to a standard EKF framework. These multi-hypothesis approaches not only deal well with a highly nonlinear and non-Gaussian distribution of the measurement errors of vision but also result in numerically stable filters. The computational costs are reduced compared to a naive implementation of particle filter, and these algorithms run in real time. This system is validated through numerical simulation, image-in-the-loop simulation, and flight tests.Ph.D

Accurate position tracking with a single UWB anchor

Accurate localization and tracking are a fundamental requirement for robotic applications. Localization systems like GPS, optical tracking, simultaneous localization and mapping (SLAM) are used for daily life activities, research, and commercial applications. Ultra-wideband (UWB) technology provides another venue to accurately locate devices both indoors and outdoors. In this paper, we study a localization solution with a single UWB anchor, instead of the traditional multi-anchor setup. Besides the challenge of a single UWB ranging source, the only other sensor we require is a low-cost 9 DoF inertial measurement unit (IMU). Under such a configuration, we propose continuous monitoring of UWB range changes to estimate the robot speed when moving on a line. Combining speed estimation with orientation estimation from the IMU sensor, the system becomes temporally observable. We use an Extended Kalman Filter (EKF) to estimate the pose of a robot. With our solution, we can effectively correct the accumulated error and maintain accurate tracking of a moving robot.Comment: Accepted by ICRA202