On-Manifold Preintegration for Real-Time Visual--Inertial Odometry

Accurate and real-time state estimation is the first step to realize safe flight and operation of unmanned aerial vehicles (UAVs). Multi-sensor fusion, e.g., vision, IMU, and GPS, can improve the accuracy of state estimation and even make it work when some sensor is unavailable. Thus, this paper proposes a multi-sensor fusion method based on the combination of filtering and optimization to achieve locally accurate and globally drift-free state estimation. The proposed method has two components, i.e., the Kalman filter and global optimization. The Kalman filter is considered the main structure of the fusion framework, which fuses a local sensor (IMU) and global sensors (aligned global visual inertial odometry, GPS, magnetometer, and barometer sensors) to obtain global state estimation in real time. Global optimization estimates the transformation between local base frame of the visual inertial odometry and global base frame to obtain an accurate global visual estimation. However, given discontinuity of optimization and odometry delay, the aligned visual odometry is then input into the Kalman filter to achieve accurate and drift-free state estimation in real time. Finally, flight and localization tests on a practical UAV were conducted. The experimental results demonstrate the effectiveness and robustness of the proposed multi-sensor fusion method.

Recent years have seen multiple impressive results in visual-inertial odometry (VIO) techniques, by which accurate state estimation can be achieved via the extended Kalman filter (EKF) or nonlinear optimization. However, these approaches are rarely open source, and they tend to fail in real experiments due to the temporary lack of feature points and fast motion. Therefore, in this study, we used encoders to overcome the temporary failure of purely vision-based simultaneous localization and mapping (SLAM). Here, we propose a generative measurement model for encoders and derive an expression for the maximum a posteriori (MAP) estimate and necessary Jacobians for optimization. We use our theory to present a novel tightly coupled visual-inertial encoder RGB-Depth (RGB-D) SLAM system. Tests on our system on an in-house dataset confirmed that our modeling effort led to accurate (with a root mean squared error (RMSE) of approximately 2–7 cm) and robust state estimation in real time. The source code and our dataset containing the encoder information have been published for verification.

Real-time state estimation is one of the important tools for modern control center operation in this smart grid era. Efficient and accurate dynamic state estimation is essential for a reliable operation of power system. Synchrophasor technology or PMU devices provide the technological advantage to perform efficient and accurate dynamic real-time state estimation. In this study, a real-time dynamic state estimation tool with graphical user interface (GUI) has been developed and tested. It involves the setup of a phasor data concentrator (PDC) with SQL server database which stores streaming phasor measurements coming over the Internet. A new algorithm to identify the optimal PMU placements is also proposed in this study. It is found that this new algorithm is computationally very fast and efficient. A real-time simulation model of IEEE 39 Bus system has been developed in Real-Time Digital Simulator (RTDS) and is used to stream phasor data from the real-time simulation. OpenPDC has been used as PDC front-end software and MS SQL server has been used as back-end PDC database server. MATLAB has been used to develop real-time state estimator tool with GUI and successful testing has been performed.

Municipal infrastructure inspection and maintenance are vital to keep the integrity and safe operation of large-scale infrastructure such as pipelines, bridges and tunnels. For example, regular inspection and assessment of tunnels are essential to prevent any possible leakage or worse, collapse. Inspection of deep sewerage tunnels is even more challenging because of their complex environment where such tunnels are not only difficult to reach, they may also have dangerous bacteria and microorganisms and the sewer gas would also have mixture of flammable and toxic gases such as hydrogen sulphide, methane, ammonia and sulphur dioxide. In Singapore, deep tunnel sewerage system (DTSS) is 6 m in diameter and located about 20 to 50 m underground. Recently, quadcopters are widely used for inspection because of their ability to do vertical take-off and landing with high stability, reach to great height as well as ease of use and low cost. However, quadcopter localization in deep sewerage tunnel is very challenging as the environment there is dark, wet, featureless and structureless. The darkness and featureless environment causes significant visual degradation, while structureless and wet environment causes lidar degradation. Thus, multiple sensors are required for proper robot localization to mitigate such sensor degradation. Moreover, employing an inertial measurement unit (IMU) as an additional sensor can dramatically improve both reliability and accuracy of visual and lidar odometry. Therefore, data fusion paradigm has become the major focus for the visual inertial odometry (VIO) and lidar inertial odometry (LIO) algorithms. Among these, the filtering data fusion paradigm is a common fusion technique because it is fast and efficient for online estimation. However, it produces cumulative linearization error which will cause gradual deterioration in the accuracy. In this thesis, a data fusion paradigm based on nonlinear optimization called a full smoothing paradigm has been proposed as a better alternative to the filtering paradigm. The full smoothing paradigm takes into consideration the full history of the previous states without marginalization. Therefore, it is more accurate than the filtering paradigms. However, high IMU sampling rate represents a great challenge for online estimation. This is because the full state history grows rapidly making online estimation not feasible. Motion preintegration overcomes this problem whereby the IMU measurements can be integrated into a single equivalent value between two visual or lidar keyframes. This makes the full smoothing paradigm simpler and the nonlinear optimization process can be done at a lower frequency. In this work, a novel IMU motion integration model was proposed for the continuous-time of IMU kinematics which are modelled using a switched linear system. The novel IMU model has a closed-form solution. Based on that, a novel IMU factor was formulated as a closed-form discrete factor to compute the mean measurements, covariance matrix and Jacobians. As such, the proposed model is more accurate and more efficient for online state estimation than the state-of-the-art. To evaluate the proposed IMU factor, it is integrated with the VIO and LIO frameworks as tightly coupled joint-optimization. The proposed VIO has been evaluated using simulated and real-world datasets. In addition, indoor experiments have been done to test its effectiveness. Results obtained show that the proposed framework outperforms the state-of-the-art approach by up to 22 % and 38 % on real datasets and indoor experiments, respectively. A novel nonlinear de-skewing is then proposed to correct the motion distortion between all scanned point in LIO. The novel IMU motion integration model is further used to correct the distorted points. In addition, the novel IMU factor is used with lidar factor in joint-optimization to obtain high state estimation. Results obtained show that the proposed model outperforms the state-of-the-art by up to 21 %.

In this paper, we propose a fully tightly‐coupled multi‐sensor fusion framework termed FT‐LVIO, that fuses measurements from a light detection and ranging (LiDAR), a monocular camera and an inertial measurement unit (IMU) simultaneously to achieve robust and accurate state estimation in real time. FT‐LVIO is built atop the framework of an error‐state‐iterated Kalman filter. To take full advantage of the complimentary characteristics of individual sensors, LiDAR point clouds are undistorted by IMU prediction to the nearest camera exposure time and the filter is updated with measurements from all sensors. In addition, an efficient sampling method for the LiDAR point‐to‐plane measurements is proposed, which can help select the measurements providing sufficient constraints to the pose estimation and facilitate a low‐drift odometry. Extensive experiments are performed on both the public NTU dataset and the private handheld dataset, and the results show that the proposed FT‐LVIO outperforms the state‐of‐the‐art LiDAR‐inertial, visual‐inertial and LiDAR‐visual‐inertial methods in both accuracy and robustness. Furthermore, FT‐LVIO can survive in the challenging staircase environment.

Many technologies need to be established to realize autonomous ships. In particular, accurate state estimation in real time is one of the most important technologies. In the ship and ocean engineering fields, there have been many studies on state estimation using nonlinear Kalman filters. Several methods have been proposed for nonlinear Kalman filters. However, there is insufficient verification on the selection of which filter should be applied among them. Therefore, this study aims to validate the filter selection to provide a guideline for filter selection. The effects of modeling error, observation noise, and type of maneuvers on the estimation accuracy of the unscented Kalman filter (UKF) and ensemble Kalman filter (EnKF) used in this study were investigated. In addition, it was verified whether filtering could be performed in real time. The results show that modeling error significantly impacts the estimation accuracy of the UKF and EnKF. However, the observation noise and types of maneuvers did not have an impact like the modeling error. Thus, we obtained the guideline that UKF and EnKF should be used differently depending on the required computation time. We also obtained that keeping the modeling error sufficiently small is essential to improving the estimation accuracy.

Inertial odometry is an attractive solution to the problem of state estimation for agile quadrotor flight. It is inexpensive, lightweight, and it is not affected by perceptual degradation. However, only relying on the integration of the inertial measurements for state estimation is infeasible. The errors and time-varying biases present in such measurements cause the accumulation of large drift in the pose estimates. Recently, inertial odometry has made significant progress in estimating the motion of pedestrians. State-of-the-art algorithms rely on learning a motion prior that is typical of humans but cannot be transferred to drones. In this work, we propose a learning-based odometry algorithm that uses an inertial measurement unit (IMU) as the only sensor modality for autonomous drone racing tasks. The core idea of our system is to couple a model-based filter, driven by the inertial measurements, with a learning-based module that has access to the thrust measurements. We show that our inertial odometry algorithm is superior to the state-of-the-art filter-based and optimization-based visual-inertial odometry as well as the state-of-the-art learned-inertial odometry in estimating the pose of an autonomous racing drone. Additionally, we show that our system is comparable to a visual-inertial odometry solution that uses a camera and exploits the known gate location and appearance. We believe that the application in autonomous drone racing paves the way for novel research in inertial odometry for agile quadrotor flight.

This study presents a novel hazard avoidance guidance method using a dynamic safety margin index, to enhance robustness in the highly uncertain environments of planet and small body landing. As future planetary landing and sample return missions will seek to land in areas with high scientific value which may be located in hazardous terrains, onboard hazard avoidance capability is indispensable. Moreover, the dynamics environment of planet or small body landing is very uncertain due to many sources of perturbations, and the accuracy of lander state estimation in real time is limited. To cope with the impact of state uncertainty on hazard avoidance performance, this study introduces a dynamic safety margin index that changes with the lander state uncertainty, and derives the hazard avoidance guidance law based on evaluation of this index. The safety margin index takes into account the state uncertainty and control constraints of the lander, and quantitatively describes the safety state of the lander with respect to the hazards around. The index is then used to derive the guidance law that makes the system globally stable and guides the lander to the desired final landing state without collision with any hazard. The impact of the lander state uncertainty on trajectory safety is considered and quantified in safety margin index evaluation and in the development of the guidance law, so the proposed algorithm is adaptive to the lander state uncertainty exhibited in planetary landing practice. The hazard avoidance performance with limited control ability is also improved as the control constraints are considered. No offline trajectory generation is required, so the method is appropriate for real-time hazard avoidance following online hazard detection. The behavior and performance of the proposed guidance method is investigated via a set of numerical simulations, and the results show that the hazard avoidance performance with state uncertainty and control constraints is improved using the proposed method, advantageous to practical implementation in the uncertain dynamics environment of planetary landing.

Using cameras for localization and mapping with mobile robots is appealing as these sensors are small, inexpensive, and ubiquitous. However, since every camera image provides hundred thousands of measurements, it poses a great challenge to infer structure and motion from this wealth of data in real-time on computationally constrained robotic systems. Furthermore, robustness becomes an important factor when applying computer vision algorithms to mobile robots that are moving in uncontrolled environments. In this case, nuisances such as occlusions, illumination changes, or low textured surfaces increase the difficulty to track visual cues, which is fundamental to enable camera-based localization and mapping.
\nThe first contribution of this thesis is an efficient, robust, and accurate visual odometry algorithm that computes the motion of a single camera solely from its stream of images. Therefore, the use of direct methods that operate directly on pixel level intensities is investigated. The advantage of direct methods is that pixel correspondence between images is given directly by the geometry of the problem and can be refined by using the local intensity gradients. However, joint refinement of structure and motion by pixel-wise minimization of intensity differences becomes intractable as the map grows. Therefore, a novel semi-direct approach is proposed that establishes feature correspondence using direct methods and subsequently relies on proven feature-based methods for refinement. We further show how inertial measurements can seamlessly be integrated in the optimization of structure and motion. Therefore, the second contribution of this thesis is a preintegration theory that allows summarizing many inertial measurements between two frames into single relative motion constraints. We formally discuss the generative measurement model as well as the nature of the rotation noise and derive the expression for the maximum a posteriori state estimator. Experimental results confirm that our modeling efforts lead to accurate state estimation in real-time, outperforming state-of-the-art approaches.
\nTracking salient features in the image results in sparse point clouds; however, for robotic tasks such as path planning, manipulation, or obstacle avoidance, a denser surface representation is needed. Previous work on dense reconstruction from images aim at providing high fidelity reconstructions. However, for robotic applications, the accuracy of the reconstruction should be governed by the interaction task. Furthermore, it is crucial to have a measure of uncertainty in the reconstruction, which aids motion planning and fusion with complementary sensors. This motivates the third contribution of this thesis, which is an efficient algorithm for probabilistic dense depth estimation from a single camera. Therefore, we combine a multi-view and per-pixel-based recursive Bayesian depth estimation scheme with a fast smoothing method that takes into account the estimated depth uncertainty. 
\nWhile most computer vision approaches fuse depth-maps in a cost volume, care has to be taken in terms of scalability and memory consumption for robotic applications. Therefore, building upon the proposed dense depth estimation, the next contribution of this thesis is a robot-centric elevation mapping system that suits a flying robot with down-looking camera and can be applied on-board Micro Aerial Vehicles (MAVs) for fully autonomous landing-spot detection and landing.
\nWe further demonstrate the usefulness of dense depth-maps for localization of an MAV with respect to a ground robot. Therefore, we address the problem of registering the maps computed by two robots from distant vantage points, using different sensing modalities: a dense 3D reconstruction from the MAV is aligned with the map computed from the depth sensor on the ground robot.
\nThe most exciting opportunity of computer vision for mobile robotics is that robots can exhibit control on the data acquisition process. This motivated the investigation of the following problem: given the image of a scene, what is the trajectory that an MAV-mounted camera should follow to perform optimal dense depth estimation? The last contribution of this thesis addresses this question and introduces a method to compute the measurement uncertainty and, thus, the expected information gain, on the basis of the scene structure and appearance. This results in the MAV to choose motion trajectories that avoid perceptual ambiguities inferred by the texture in the scene.

In order to obtain more accurate pose estimation, the visual-inertial odometry (VIO) system with outlier rejection and loop closure is proposed in this paper. Considering that feature matching is an important part in the front-end of the VIO system, its accuracy will affect the performance of the entire system. So we introduce an outlier rejection method of the grid-based motion statistics (GMS) algorithm to the VIO system. And for more robust feature correspondence and better camera pose estimation, we propose an improved GMS method to eliminate the mismatched points. Besides, we adopt the loop closure strategy to correct the cumulative error of the VIO system. Finally, we estimate the camera pose, velocity and IMU bias simultaneously by minimizing the loss function which contains reprojection error and IMU error. A large number of experiments on EuRoC demonstrate that the proposed method outperforms the advanced VIO system ROVIO and is comparable to the state-of-the-art VIO system OKVIS.

To address the limitations of existing LiDAR–visual fusion methods in adequately accounting for map uncertainties induced by LiDAR measurement noise, this paper introduces a LiDAR–inertial–visual odometry framework leveraging mergeable probabilistic voxel mapping. The method innovatively employs probabilistic voxel models to characterize uncertainties in environmental geometric plane features and optimizes computational efficiency through a voxel merging strategy. Additionally, it integrates color information from cameras to further enhance localization accuracy. Specifically, in the LiDAR–inertial odometry (LIO) subsystem, a probabilistic voxel plane model is constructed for LiDAR point clouds to explicitly represent measurement noise uncertainty, thereby improving the accuracy and robustness of point cloud registration. A voxel merging strategy based on the union-find algorithm is introduced to merge coplanar voxel planes, reducing computational load. In the visual–inertial odometry (VIO) subsystem, image tracking points are generated through a global map projection, and outlier points are eliminated using a random sample consensus algorithm based on a dynamic Bayesian network. Finally, state estimation accuracy is enhanced by jointly optimizing frame-to-frame reprojection errors and frame-to-map RGB color errors. Experimental results demonstrate that the proposed method achieves root mean square errors (RMSEs) of absolute trajectory error at 0.478 m and 0.185 m on the M2DGR and NTU-VIRAL datasets, respectively, while attaining real-time performance with an average processing time of 39.19 ms per-frame on the NTU-VIRAL datasets. Compared to state-of-the-art approaches, our method exhibits significant improvements in both accuracy and computational efficiency.

In this article, we propose a novel optimization-based tightly coupled Direct Visual-Inertial Odometry (DVIO), which fuses the visual and inertial measurements to provide real-time full state estimation. Different from existing frameworks, the key novelty of the proposed method is to integrate the data association, state estimation, and outlier detection into a nonlinear optimization framework in a tightly coupled way. Specifically, by jointly minimizing the preintegration error of the inertial measurement unit and the photometric error of the camera, the data association is tightly coupled with the process of state estimation. Then, an iterative selection strategy is design to reject outliers during the data association and establish more visual constraints in the optimization. In addition, a hybrid weighting method is proposed to tightly integrate the iterative selection into the optimization by dynamically weighting residual terms. As a consequence, the proposed method aligns the image patches, estimates the motion and removes the outliers synchronously. Comparative experiments on the public dataset and extensive real-world experiments show that DVIO outperforms state-of-the-art visual-inertial odometries in terms of both the accuracy and the robustness. Thus, DVIO is highly applicable to the navigation or the simultaneous localization and mapping of mobile devices or agile robots like micro air vehicles.

The real-time state estimation is crucial to guarantee the stable operation of energy Internet (EI) which has variable loads and distributed power generations. Therefore, this paper proposes a real-time transient state estimation method for EI based on graph convolutional networks (GCN). Using data of SCADA and limited phasor measurement unit (PMU), the GCN in the proposed method fuses the heterogeneous data of EI buses with the adjacency matrix that represents the topology of EI. Then the transient states of EI buses without PMU measurement are estimated by SCADA data and adjacent PMU data through the training of GCN model. The case study on the simulation data of an IEEE 9 bus system that considers fault injection and disturbances verifies the effectiveness of the proposed approach. The result shows that the proposed approach achieves fast and accurate state estimation of all EI buses during the transient process of faults and disturbances.

This dissertation presents a comprehensive approach to addressing the challenges associated with the control and navigation of Unmanned Aerial Vehicles (UAVs), specifically focusing on robustness under GPS-denied, turbulent, and dark indoor conditions with various disturbances. Initial efforts involved designing and theoretically validating Sliding Mode Controllers (SMC) with Disturbance Observers using MATLAB/SIMULINK, optimized using offline Particle Swarm Optimization (PSO). In order to evaluate their performance, a lightweight simulator was developed to simulate both quadcopter and hexacopter configurations under the PX4 architecture. This simulator served to validate and compare the optimized SMC against PSO-optimized PID controllers. The results of these comparisons demonstrated the superior adaptability of the SMC, especially in environments characterized by significant sensor noise and disturbances. Subsequent practical indoor tests were conducted with real quadcopters, which provided additional verification of the robustness and reliability of the enhanced control theory with the actual UAVs under dynamic disturbances. To further evaluate the compatibility and adaptability of the SMC, this dissertation establishes a framework for interfacing with high-performance STMH7 chips, serving as the UAV microcontroller unit (MCU). This framework facilitates the connection of various sensors and provides guidance for constructing different quadcopter frames. Moreover, it enables the comparison of different flight controllers on distinct air-frames. Concurrently, the optimized SMC allows for remote dynamic parameter tuning during flight. In pursuit of improved localization capacity, the dissertation also explores the integration of an Intel Realsense D435i camera with an Inertial Measurement Unit (IMU) for Visual-Inertial Odometry (VIO) fusion within the Robot Operating System (ROS) environment. This integration employs an onboard companion computer for real-time VIO estimation and flight command. The effectiveness of the VIO algorithm is verified through both simulations and real-world experiments, highlighting its ability to enhance UAV localization and navigation. Additionally, a visual inertial odometry method for UAV Simultaneous Localization and Mapping (vSLAM) is tested and validated in both simulated and real drone environments. Furthermore, the dissertation introduces an innovative hybrid filtered multi-directional radar inertial odometry solution denoted as Hybrid-MRIO. This Error State Extended Kalman Filter (ES-EKF) based Radar Inertial Odometry (RIO) approach is implemented within the open-source PX4 autopilot system, enhancing UAV navigation accuracy and performance, and serves as a robust navigation alternative, particularly in challenging environments where traditional navigation systems prove ineffective, such as dark or smoky conditions. This solution integrates data from multiple software synchronized high-resolution Frequency-Modulated Continuous-Wave (FMCW) mmWave radars with an IMU. A subsequent 4D (x, y, z, doppler) mmWave radar SLAM (rSLAM) system has been developed, including three modules: front-end, loop detection module, and back-end. In the front-end, radar ego-velocity is utilized for states estimation and dynamic object removal, and a point cloud registration-based approach known as APDGICP (Adaptive Probability Distribution-GICP) is employed for keyframe detection. The loop detection module utilizes the intensity scan context to identify potential loop closure candidates. In the back-end, a pose graph is constructed, integrating RIO estimated odometry and identified loop closures for better localization. To develop fully-autonomous UAVs with their decision making modules, this dissertation details the development of a custom AI-powered fully-autonomous quadcopter equipped with companion computers and stereo IR cameras. Additionally, this dissertation discusses a swarm of small Unmanned Aerial Systems (SUAS) interconnected through a software-defined communications network. These drones demonstrated their capacity to independently/collaboratively execute a wide range of tasks, including target search, detection, identification, classification, tracking, and following, both in simulated and real-world scenarios. The system exhibits advanced collision avoidance capabilities and the ability to strategically respond to dynamic scenarios, such as changes in target behavior, highlighting its effectiveness in managing dynamic and challenging situations without the need for continuous human intervention.--Author's abstract

Given the payload limitation of unmanned aerial vehicles (UAVs), lightweight sensors such as camera, inertial measurement unit (IMU), and GPS, are ideal onboard measurement devices. By fusing multiple sensors, accurate state estimations can be achieved. Robustness against sensor faults is also possible because of redundancy. However, scale estimation of visual systems (visual odometry or visual inertial odometry, VO/VIO) suffers from sensor noise and special-case movements such as uniform linear motion. Thus, in this paper, a scale insensitive multi-sensor fusion (SIMSF) framework based on graph optimization is proposed. This framework combines the local estimation of the VO/VIO and global sensors to infer the accurate global state estimation of UAVs in real time. A similarity transformation between the local frame of the VO/VIO and the global frame is estimated by optimizing the poses of the most recent UAV states. In particular, for VO, an initial scale is estimated by aligning the VO with the IMU and GPS measurements. Moreover, a fault detection method for VO/VIO is also proposed to enhance the robustness of the fusion framework. The proposed methods are tested on a UAV platform and evaluated in several challenging environments. A comparison between our results and the results from other state-of-the-art algorithms demonstrate the superior accuracy, robustness, and real-time performance of our system. Our work is also a general fusion framework, which can be extended to other platforms as well.