A Robust Parallel Initialization Method for Monocular Visual-Inertial SLAM.

In order to improve the initialization robustness of visual inertial SLAM, the complementarity of the optical flow method and the feature-based method can be used in vision data processing. The parallel initialization method is proposed, where the optical flow inertial initialization and the monocular feature-based initialization are carried out at the same time. After the initializations, the state estimation results are jointly optimized by bundle adjustment. The proposed method retains more mapping information, and correspondingly is more adaptable to the initialization scene. It is found that the initialization map constructed by the proposed method features a comparable accuracy to the one constructed by ORB-SLAM3 in monocular inertial mode. Since the online extrinsic parameter estimation can be realized by the proposed method, it is considered better than ORB-SLAM3 in the aspect of portability. By the experiments performed on the benchmark dataset EuRoC, the effectiveness and robustness of the proposed method are validated.

An Extensible Positioning System for Locating Mobile Robots in Unfamiliar Environments.

In this paper, an extensible positioning system for mobile robots is proposed. The system includes a stereo camera module, inertial measurement unit (IMU) and an ultra-wideband (UWB) network which includes five anchors, one of which is with the unknown position. The anchors in the positioning system are without requirements of communication between UWB anchors and without requirements of clock synchronization of the anchors. By locating the mobile robot using the original system, and then estimating the position of a new anchor using the ranging between the mobile robot and the new anchor, the system can be extended after adding the new anchor into the original system. In an unfamiliar environment (such as fire and other rescue sites), it is able to locate the mobile robot after extending itself. To add the new anchor into the positioning system, a recursive least squares (RLS) approach is used to estimate the position of the new anchor. A maximum correntropy Kalman filter (MCKF) which is based on the maximum correntropy criterion (MCC) is used to fuse data from the UWB network and IMU. The initial attitude of the mobile robot relative to the navigation frame is calculated though comparing position vectors given by a visual simultaneous localization and mapping (SLAM) system and the UWB system respectively. As shown in the experiment section, the root mean square error (RMSE) of the positioning result given by the proposed positioning system with all anchors is 0.130 m. In the unfamiliar environment, the RMSE is 0.131 m which is close to the RMSE (0.137 m) given by the original system with a difference of 0.006 m. Besides, the RMSE based on Euler distance of the new anchor is 0.061 m.

An Ultra-Short Baseline Positioning Model Based on Rotating Array & Reusing Elements and Its Error Analysis.

The USBL (Ultra-Short Base Line) positioning system is widely used in underwater acoustic positioning systems due to its small size and ease of use. The traditional USBL positioning system is based on 'slant range and azimuth'. The positioning error is an increasing function with the increase in distance and the positioning accuracy depends on the ranging accuracy of the underwater target. This method is not suitable for long-distance underwater positioning operations. This paper proposes a USBL positioning calculation model based on depth information for 'rotating array and reusing elements'. This method does not need to measure the distance between the USBL acoustic array and target, so it can completely eliminate the influence of long-distance ranging errors in USBL positioning. The theoretical analysis and simulation experiments show that the new USBL positioning model based on 'rotating array and reusing elements' can completely eliminate the influence of the wavelength error and spacing error of underwater acoustic signals on the positioning accuracy of USBL. The positioning accuracy can be improved by approximately 90%, and the horizontal positioning error within a positioning distance of 1000 m is less than 1.2 m. The positioning method has high precision performance in the long distance, and provides a new idea for the engineering design of a USBL underwater positioning system.

A Novel Hybrid of a Fading Filter and an Extreme Learning Machine for GPS/INS during GPS Outages.

In this paper, a novel algorithm based on the combination of a fading filter (FF) and an extreme learning machine (ELM) is presented for Global Positioning System/Inertial Navigation System (GPS/INS) integrated navigation systems. In order to increase the filtering accuracy of the model, a variable fading factor fading filter based on the fading factor is proposed. It adjusts the fading factor by the ratio of the estimated covariance before and after the moment which proves to have excellent performance in our experiment. An extreme learning machine based on a Fourier orthogonal basis function is introduced that considers the deterioration of the accuracy of the navigation system during GPS outages and has a higher positioning accuracy and faster learning speed than the typical neural network learning algorithm. In the end, a simulation and real road test are performed to verify the effectiveness of this algorithm. The results show that the accuracy of the fading filter based on a variable fading factor is clearly improved, and the proposed improved ELM algorithm can provide position corrections during GPS outages more effectively than the other algorithms (ELM and the traditional radial basis function neural network).

Robust Time-Difference-of-Arrival (TDOA) Localization Using Weighted Least Squares with Cone Tangent Plane Constraint.

Finding the position of a radiative source based on time-difference-of-arrival (TDOA) measurements from spatially separated receivers has been widely applied in sonar, radar, mobile communications and sensor networks. For the nonlinear model in the process of positioning, Taylor series and other novel methods are proposed. The idea of cone constraint provides a new way of solving this problem. However, these approaches do not always perform well and are away from the Cramer-Rao-Lower-Bound (CRLB) in the situations when the source is set at the array edge, the noise in measurement is loud, or the initial position is biased. This paper presents a weighted-least-squares (WLS) algorithm with the cone tangent plane constraint for hyperbolic positioning. The method adds the range between the source and the reference sensor as a dimension. So, the space-range frame is established. Different from other cone theories, this paper sets the reference sensor as the apex and finds the optimal source estimation on the cone. WLS is used for the optimal result from the measurement plane equations, a vertical constraint and a cone constraint. The cone constraint equation is linearized by a tangent plane. This method iterates through loops and updates the tangent plane, which approximates the truth-value on the cone. The proposed algorithm was simulated and verified under various conditions of different source positions and noises. Besides, some state-of-the-art algorithms were compared in these simulations. The results show that this algorithm is accurate and robust under poor external environment.

A RLS-SVM Aided Fusion Methodology for INS during GPS Outages.

In order to maintain a relatively high accuracy of navigation performance during global positioning system (GPS) outages, a novel robust least squares support vector machine (LS-SVM)-aided fusion methodology is explored to provide the pseudo-GPS position information for the inertial navigation system (INS). The relationship between the yaw, specific force, velocity, and the position increment is modeled. Rather than share the same weight in the traditional LS-SVM, the proposed algorithm allocates various weights for different data, which makes the system immune to the outliers. Field test data was collected to evaluate the proposed algorithm. The comparison results indicate that the proposed algorithm can effectively provide position corrections for standalone INS during the 300 s GPS outage, which outperforms the traditional LS-SVM method. Historical information is also involved to better represent the vehicle dynamics.

An IMM-Aided ZUPT Methodology for an INS/DVL Integrated Navigation System.

Inertial navigation system (INS)/Doppler velocity log (DVL) integration is the most common navigation solution for underwater vehicles. Due to the complex underwater environment, the velocity information provided by DVL always contains some errors. To improve navigation accuracy, zero velocity update (ZUPT) technology is considered, which is an effective algorithm for land vehicles to mitigate the navigation error during the pure INS mode. However, in contrast to ground vehicles, the ZUPT solution cannot be used directly for underwater vehicles because of the existence of the water current. In order to leverage the strengths of the ZUPT method and the INS/DVL solution, an interactive multiple model (IMM)-aided ZUPT methodology for the INS/DVL-integrated underwater navigation system is proposed. Both the INS/DVL and INS/ZUPT models are constructed and operated in parallel, with weights calculated according to their innovations and innovation covariance matrices. Simulations are conducted to evaluate the proposed algorithm. The results indicate that the IMM-aided ZUPT solution outperforms both the INS/DVL solution and the INS/ZUPT solution in the underwater environment, which can properly distinguish between the ZUPT and non-ZUPT conditions. In addition, during DVL outage, the effectiveness of the proposed algorithm is also verified.

A Kalman Filter for SINS Self-Alignment Based on Vector Observation.

In this paper, a self-alignment method for strapdown inertial navigation systems based on the q-method is studied. In addition, an improved method based on integrating gravitational apparent motion to form apparent velocity is designed, which can reduce the random noises of the observation vectors. For further analysis, a novel self-alignment method using a Kalman filter based on adaptive filter technology is proposed, which transforms the self-alignment procedure into an attitude estimation using the observation vectors. In the proposed method, a linear psuedo-measurement equation is adopted by employing the transfer method between the quaternion and the observation vectors. Analysis and simulation indicate that the accuracy of the self-alignment is improved. Meanwhile, to improve the convergence rate of the proposed method, a new method based on parameter recognition and a reconstruction algorithm for apparent gravitation is devised, which can reduce the influence of the random noises of the observation vectors. Simulations and turntable tests are carried out, and the results indicate that the proposed method can acquire sound alignment results with lower standard variances, and can obtain higher alignment accuracy and a faster convergence rate.

A Coarse Alignment Method Based on Digital Filters and Reconstructed Observation Vectors.

In this paper, a coarse alignment method based on apparent gravitational motion is proposed. Due to the interference of the complex situations, the true observation vectors, which are calculated by the apparent gravity, are contaminated. The sources of the interference are analyzed in detail, and then a low-pass digital filter is designed in this paper for eliminating the high-frequency noise of the measurement observation vectors. To extract the effective observation vectors from the inertial sensors' outputs, a parameter recognition and vector reconstruction method are designed, where an adaptive Kalman filter is employed to estimate the unknown parameters. Furthermore, a robust filter, which is based on Huber's M-estimation theory, is developed for addressing the outliers of the measurement observation vectors due to the maneuver of the vehicle. A comprehensive experiment, which contains a simulation test and physical test, is designed to verify the performance of the proposed method, and the results show that the proposed method is equivalent to the popular apparent velocity method in swaying mode, but it is superior to the current methods while in moving mode when the strapdown inertial navigation system (SINS) is under entirely self-contained conditions.

An Adaptive Damping Network Designed for Strapdown Fiber Optic Gyrocompass System for Ships.

The strapdown fiber optic gyrocompass (strapdown FOGC) system for ships primarily works on external horizontal damping and undamping statuses. When there are large sea condition changes, the system will switch frequently between the external horizontal damping status and the undamping status. This means that the system is always in an adjustment status and influences the dynamic accuracy of the system. Aiming at the limitations of the conventional damping method, a new design idea is proposed, where the adaptive control method is used to design the horizontal damping network of the strapdown FOGC system. According to the size of acceleration, the parameters of the damping network are changed to make the system error caused by the ship's maneuvering to a minimum. Furthermore, the jump in damping coefficient was transformed into gradual change to make a smooth system status switch. The adaptive damping network was applied for strapdown FOGC under the static and dynamic condition, and its performance was compared with the conventional damping, and undamping means. Experimental results showed that the adaptive damping network was effective in improving the dynamic performance of the strapdown FOGC.

FOG Random Drift Signal Denoising Based on the Improved AR Model and Modified Sage-Husa Adaptive Kalman Filter.

In order to reduce the influence of fiber optic gyroscope (FOG) random drift error on inertial navigation systems, an improved auto regressive (AR) model is put forward in this paper. First, based on real-time observations at each restart of the gyroscope, the model of FOG random drift can be established online. In the improved AR model, the FOG measured signal is employed instead of the zero mean signals. Then, the modified Sage-Husa adaptive Kalman filter (SHAKF) is introduced, which can directly carry out real-time filtering on the FOG signals. Finally, static and dynamic experiments are done to verify the effectiveness. The filtering results are analyzed with Allan variance. The analysis results show that the improved AR model has high fitting accuracy and strong adaptability, and the minimum fitting accuracy of single noise is 93.2%. Based on the improved AR(3) model, the denoising method of SHAKF is more effective than traditional methods, and its effect is better than 30%. The random drift error of FOG is reduced effectively, and the precision of the FOG is improved.

Initial Alignment of Large Azimuth Misalignment Angles in SINS Based on Adaptive UPF.

The case of large azimuth misalignment angles in a strapdown inertial navigation system (SINS) is analyzed, and a method of using the adaptive UPF for the initial alignment is proposed. The filter is based on the idea of a strong tracking filter; through the introduction of the attenuation memory factor to effectively enhance the corrections of the current information residual error on the system, it reduces the influence on the system due to the system simplification, and the uncertainty of noise statistical properties to a certain extent; meanwhile, the UPF particle degradation phenomenon is better overcome. Finally, two kinds of non-linear filters, UPF and adaptive UPF, are adopted in the initial alignment of large azimuth misalignment angles in SINS, and the filtering effects of the two kinds of nonlinear filter on the initial alignment were compared by simulation and turntable experiments. The simulation and turntable experiment results show that the speed and precision of the initial alignment using adaptive UPF for a large azimuth misalignment angle in SINS under the circumstance that the statistical properties of the system noise are certain or not have been improved to some extent.

A Self-Alignment Algorithm for SINS Based on Gravitational Apparent Motion and Sensor Data Denoising.

Initial alignment is always a key topic and difficult to achieve in an inertial navigation system (INS). In this paper a novel self-initial alignment algorithm is proposed using gravitational apparent motion vectors at three different moments and vector-operation. Simulation and analysis showed that this method easily suffers from the random noise contained in accelerometer measurements which are used to construct apparent motion directly. Aiming to resolve this problem, an online sensor data denoising method based on a Kalman filter is proposed and a novel reconstruction method for apparent motion is designed to avoid the collinearity among vectors participating in the alignment solution. Simulation, turntable tests and vehicle tests indicate that the proposed alignment algorithm can fulfill initial alignment of strapdown INS (SINS) under both static and swinging conditions. The accuracy can either reach or approach the theoretical values determined by sensor precision under static or swinging conditions.

In-motion coarse alignment method for SINS/DVL with the attitude dynamics.

For underwater Strapdown Inertial Navigation System (SINS)/Doppler Velocity Log (DVL) integration system, there are intervals between DVL's transmitting and receiving epochs, which introduces velocity errors when the attitude dynamic occurs. To accelerate the coarse alignment process for SINS/DVL with attitude dynamics, an improved in-motion coarse alignment solution is proposed. First, the DVL aided in-motion coarse alignment method is explored. Then, a DVL velocity compensation algorithm for coarse alignment is proposed. Simulations and a field test are conducted to evaluate the effectiveness of the proposed algorithm under various trajectories. The results indicate that the proposed coarse alignment solution effectively applies the velocity compensation algorithm to the coarse alignment mission, which shows greater performance than the traditional optimization-based alignment (OBA) method in various trajectories.

A misalignment angle error calibration method of underwater acoustic array in strapdown inertial navigation system/ultrashort baseline integrated navigation system based on single transponder mode.

The angle misalignment error of a USBL (Ultrashort Baseline) acoustic array is one of the major error sources of the strapdown inertial navigation system/USBL positioning system, which will directly affect the positioning accuracy of the USBL positioning system. For the traditional calibration method cannot accurately estimate the angle misalignment error due to its strict trajectory requirements in the field experiment and the high-precision layout of the transceiver array elements, a new method for estimating the angle misalignment error of a USBL acoustic array based on single transponder and dual-vector reconstruction is studied in this paper. The precondition of USBL misalignment calibration is to locate the underwater transponder accurately. In this paper, the single transponder segmentation iterative long baseline method is used to locate the underwater target transponder. The dual-vector reconstruction method is studied to control the estimation accuracy of USBL misalignment error calibration based on the traditional single transponder method, which provides a theoretical basis for the determination of the iteration times to the USBL angle misalignment error estimation module. The underwater experiment results show that the positioning error could be reduced to less than 1 m after the angle misalignment error compensation. The underwater transponder positioning and the angle misalignment error estimation of USBL could be accomplished in a circle sailing. It is a new method with good performance of high estimation accuracy, simple operation, and easy realization.

In-motion coarse alignment method based on reconstructed observation vectors.

In this paper, an in-motion coarse alignment method is proposed based on the reconstructed observation vectors. Since the complicated noises are contained in the outputs of the inertial sensors, the components of measurement observation vectors, which are constructed by the sensors' outputs, are analyzed in detail. To suppress the high-frequency noises, an effective digital filter based on the Infinite Impulse Response technology is employed. On the basis of the parameter models of the observation vectors, a new form Kalman filter, which is also an adaptive filter, is designed for the recognition of the parameter matrix. Furthermore, a robust filter technology, which is based on the Huber's M-estimation, is employed to suppress the gross outliers, which are caused by the movement of the carrier. Simulation test and field trial are designed to verify the proposed method. All the alignment results demonstrate that the performance of the proposed method is superior to the conventional optimization-based alignment and the digital filter alignment, which are the current popular methods.