Abstract
Integrity monitoring (IM) in autonomous navigation has been extensively researched, but currently available solutions are mainly applicable to specific algorithms and sensors, or limited by linearity or 'Gaussianity' assumptions. This study investigates a Solution Separation (SS) based framework for universal IM, scalable to multi-sensor fusion as each hypothesis assumes a whole sensor measurement set as faulty. Architecturally we consider that: 1) multi sensor systems must account for various sensor noise models which lead to inconsistent estimates of uncertainties, 2) a module must be able to detect sensor failure or sensor noise mismodeling and suggest better bounds for the error, without being constantly conservative, 3) some algorithms are computationally heavy to monitor in the SS setting or the provided covariances cannot be interpreted in IM. A hybrid SS architecture can be practical, where some solutions are evaluated with a navigation algorithm with known characteristics, although the all-sensor-in solution is evaluated with the monitored algorithm. Experiments are run on filter and smoothing-based navigation algorithms. In addition, we experiment with hybrid SS monitoring and time-correlated noise to evaluate the appropriability of our framework in the context of the above-mentioned requirements. This is a novel framework in the IM domain, directly integrable in existing navigation solutions and, in our opinion, it will facilitate the quantification of the effect of different sensors in navigation safety.
Article PDF
Similar content being viewed by others
Avoid common mistakes on your manuscript.
Data Availability
The KITTI trajectory is publicly available at http://www.cvlibs.net/datasets/kitti/. The UAV simulated trajectory can be provided to anyone upon request to the corresponding author of this paper.
Code Availability
Not applicable.
Change history
13 October 2022
Missing Open Access funding information has been added in the Funding Note.
References
European GNSS Agency: Report on The Performance and Level of Integrity For Safety and Liability Critical Multi-Applications. (2015). https://www.gsa.europa.eu/sites/default/files/calls_for_proposals/Annex%202.pdf. Accessed 11 Jan 2022
Joerger, M., Chan, F.-C., Pervan, B.: Solution Separation Versus Residual-Based RAIM. Navigation. 61 (2014)
Calhoun, S.-M., Raquet, J.: Integrity determination for a vision based precision relative navigation system. 2016 IEEE/ION Position, Location and Navigation Symposium (PLANS), Savannah, GA, pp. 294–304 (2016)
Joerger, M., Chan, C., Langel, S., Pervan, B.: RAIM detector and estimator design to minimize the integrity risk. 2012, ION GNSS (2012)
Joerger, M., Jamoom, M., Spenko, M., Pervan, B.: Integrity of laser-based feature extraction and data association. 2016 IEEE/ION Position, Location and Navigation Symposium (PLANS), pp. 557–571 (2016)
Mohanty, A., Gupta, S., Gao, G.-X.: A particle filtering framework for integrity risk of GNSS-camera sensor fusion. Proceedings of the 33rd International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS+ 2020) (2021)
Meng, Q., Hsu, L.-T.: Integrity monitoring for all source navigation enhanced by Kalman Filter based Solution Separation. 2020. IEEE Sensors J. (2020)
Crespillo, O. G., Joerger, M., Langel, S.: Overbounding GNSS/INS Integration with Uncertain GNSS Gauss-Markov Error Parameters. 2020 IEEE/ION Position, Location and Navigation Symposium (PLANS), pp. 481–489 (2020)
He, P., Liu G., Tan, C., Lu Y.-E: Nonlinear fault detection threshold optimization method for RAIM algorithm using a heuristic approach. GPS Solut 20, 863–875 (2016)
Gupta, S., Gao, G.-X.: Particle RAIM for integrity monitoring. ION GNSS+ (2019)
Appleget, A., Leishman R.-C., Gipson, J.: Evaluation of sensor-agnostic all-source residual monitoring for navigation. Proceedings of the 2021 International Technical Meeting of The Institute of Navigation (2021)
Bhamidipati, S., Gao, G.-X.: SLAM-based Integrity Monitoring Using GPS and Fish-eye Camera. Proceedings of the 32nd International Technical Meeting of the Satellite Division of The Institute of Navigation, ION GNSS+ 2019, pp. 4116–4129 (2019)
Arana, G.-D., Hafez, O.-A., Joerger, M., Spenko, M.: Recursive Integrity Monitoring for Mobile Robot Localization Safety. 2019 International Conference on Robotics and Automation (ICRA), pp. 305–311 (2019)
Tanil, C., Khanafseh, S., Joerger, M., Pervan, B.: Sequential Integrity Monitoring for Kalman Filter Innovations-Based Detectors. Proceedings of the 31st International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS+ 2018), Miami, Florida, September 2018, pp. 2440–2455 (2018)
Jurado, J., Raquet, J., Schubert Kabban, C.M., Gipson, J.: Residual-based multi-filter methodology for all-source fault detection, exclusion, and performance monitoring. Navigation 67, 493–509 (2020)
Li, C., Waslander, S.-L.: Visual Measurement Integrity Monitoring for UAV Localization. 2019 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR) (2019)
Reid, T.-G.-R., Houts, S.-E., Cammarata, R., Mills, G., Agarwal, S., Vora, A., Pandey, G.: Localization Requirements for Autonomous Vehicles. SAE Int. J. Connect. Autom. Veh. 2(3), 173–190 (2019)
Fu, L., Zhang, J., Li, R., Cao, X., Wang, J.: Vision-Aided RAIM: A New Method for GPS Integrity Monitoring in Approach and Landing Phase. Sensors 15, 22854–22873 (2015)
Mohleji, S.-C., Wang, G.: Modeling ADS-B position and velocity errors for airborne merging and spacing in interval management application. The MITRE Corporation (2010)
Langel S., Crespillo, O.-G., Joerger, M.: Bounding sequential estimation errors due to gauss-markov noise with uncertain parameters. Proceedings of the 32nd International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS+ 2019), Miami, Florida, pp. 3079–3098 (2019)
Geiger, A., Lenz, P., Stiller, C., Urtasun, R. Vision meets robotics: The KITTI Dataset. Int. J. Robot. Res. (IJRR) (2013)
Brown, R. G., & Hwang P.-Y.-C.: Transition from the discrete to continuous filter equations. In: Brown, R. G., & Hwang P. Y. C. Introduction to Random Signals and Applied Kalman Filtering with Matlab Exercises, pp. 371–375. Wiley. ISBN: 978-0-470-60969-9 (2012)
Madyastha, V., Ravindra, V., Mallikarjunan, S., Goyal, A.: Extended Kalman Filter vs. Error State Kalman Filter for Aircraft Attitude Estimation. AIAA Guidance, Navigation, and Control Conference 2011 (2011)
Roumeliotis, S.-I., Sukhatme, G.-S., & Bekey, G.-A.: Circumventing dynamic modeling: evaluation of the error-state Kalman filter applied to mobile robot localization. Proceedings 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C), pp. 1656–1663 vol.2 (1999)
Dellaert, F.: Factor Graphs and GTSAM: A Hands-on Introduction. Georgia Tech Technical Report, GT-RIM-CP&R-2012–002, Tech. Rep. (2012)
Lupton T., Sukkarieh, S.: Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions. IEEE Trans. Robot. 28(1) (2012)
Funding
Open access funding provided by NTNU Norwegian University of Science and Technology (incl St. Olavs Hospital - Trondheim University Hospital). This work was supported by the Research Council of Norway and Centre for Autonomous Marine Operations and Systems for the corresponding PhD project at the Norwegian University of Science and Technology (RCN grant number: 305051).
Author information
Authors and Affiliations
Contributions
V. Bosdelekidis: methodology, software implementation, experimental tests, paper editing. T. H. Bryne: methodology, experimental tests, paper editing, work supervision. N. Sokolova: methodology, experimental tests, paper editing, work supervision. T. A. Johansen: methodology, experimental tests, paper editing, work supervision.
Corresponding author
Ethics declarations
Ethics Approval
Not applicable.
Consent to Participate
Not applicable.
Consent for Publication
Not applicable.
Conflicts of Interest/Competing Interests
The authors report no conflict of interest or competing interests.
Additional information
Publisher's Note
Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.
Appendices
Appendix 1: ESKF and Gauss-Markov Process Noise Overbounding
IMUs often suffer stochastic errors that cannot be estimated. It is common to model these errors as the sum of random constant turn-on-bias, a time-correlated process and a white Gaussian noise term [8]. Therefore, the angular rate and specific force measurements responsible for the linear acceleration of the sensor can be expressed as:
where \({\widetilde{{\varvec{w}}}}^{b}\) and \({\widetilde{{\varvec{f}}}}^{b}\) are respectively the measured turn rates and specific forces in 3 axes with respect to the body frame \(b\), \({{\varvec{w}}}^{b}\) and \({{\varvec{f}}}^{b}\) their true values, \({{\varvec{b}}}_{w,0}\) and \({{\varvec{b}}}_{f,0}\) are respectively the turn-on random constant bias of the angular rates and the specific forces, \({{\varvec{b}}}_{w}\) and \({{\varvec{b}}}_{f}\) are the time-correlated biases, and \({{\varvec{n}}}_{w}\) and \({{\varvec{n}}}_{f}\) are white Gaussian noise vectors. The time-correlated biases are usually modeled with a Gauss-Markov process.
The ESKF estimated error state \(\delta {\varvec{x}}\) consists of the errors in position \(\delta{\varvec{\rho}}\), velocity \(\delta {\varvec{v}}\), angles vector \(\delta{\varvec{\theta}}\), linear acceleration bias \(\delta {{\varvec{b}}}_{a}\) and angular velocity bias \(\delta {{\varvec{b}}}_{w}\). In our experiments the second position sensor is affected by time-correlated noise and, therefore, we have to account for it as well. Generally, the state vector can be augmented with additional states to account for additional time correlated errors, e.g. in position sensor’s measurements. Therefore, in the general case, the state vector becomes:
where \(m\) is the number of position sensors. In our experiments \(m=2\) and, therefore, the total number of states will be \(21\).
1.1 Prediction
The continuous error state kinematics will be:
where \({\varvec{n}}\) is the process noise with spectral density \({\varvec{Q}}\) \(\in {\mathbb{R}}^{\left(12+3\mathrm{m}\right)\mathrm{x}(12+3m)}\) where the velocity, orientation and bias estimates are modeled by white Gaussian processes.
The error state dynamics matrix \({\varvec{A}}({\varvec{x}})\) and error state noise matrix \({\varvec{G}}({\varvec{x}})\) are formulated by means of first order-approximations:
with \(0\) and \({\varvec{I}}\) without subscript indicating a \(3x3\) matrix of zeros and the \(3x3\) identity matrix respectively, the symbol \(S\) denoting the cross-product operation, \({\varvec{q}}\) the attitude quaternion, \(R({\varvec{q}})\) the rotation matrix, \({p}_{{b}_{*}}\) are the inverse time constants of each sensor and \({{g}_{b}}_{*}\) a component that will inflate the corresponding covariance of a bias state in \({\varvec{Q}}\). Then:
where \({\sigma }_{a}^{2}\) and \({\sigma }_{w}^{2}\) the noise variance of the accelerometer and the gyroscope respectively.
The FDO module conditionally selects to inflate the uncertainty that corresponds to a sensor by setting \({p}_{{b}_{*}}=1/{T}_{{b}_{*}}\) and \({g}_{{{\varvec{b}}}_{\boldsymbol{*}}}=\sqrt{\frac{2}{{T}_{{b}_{\boldsymbol{*}}}}{\sigma }_{{b}_{*}}^{2}}\) with \({T}_{{b}_{*}}\) and \({\sigma }_{{b}_{*}}\) given in Eqs. 6 and 7 as this design choice has been proven to lead in an estimated covariance \(\widehat{{\varvec{P}}}\) that is larger than the true covariance \({\varvec{P}}\) [8].
The propagation matrix \({{\varvec{A}}}_{k}\) and covariance matrix \({{\varvec{Q}}}_{d}\) in the discrete time model can be computed with the Van Loan’s method and the predicted covariance in discrete—time, similarly to a standard KF becomes:
1.2 Update
The measurement model relates to the true state as:
denoting as \({{\varvec{x}}}_{t}\) the true state vector and \({\varvec{w}}\) the measurement noise modeled as a zero mean Gaussian with covariance matrix \({{\varvec{R}}}_{k}\).
However, as we aim for the estimation of the error state instead of the true state we require the relationship between the true state and the error state. This is formulated as:
Substituting in Eq. 16 and by linearizing around \(\delta {\varvec{x}}=0\) (error state is small):
with \({\varvec{H}}\) denoting the Jacobian. Having this model, we can directly retrieve the updated error state and covariance as in the EKF framework.
In the experiments presented in this paper the fixes are directly the measurements from the position sensors and the matrix \({\varvec{H}}\) for the position update from POS_SENSOR \(i\) will be:
where \({{\varvec{r}}}_{po{s}_{i}}\) the lever arm from body to POS_SENSOR \(i\) and \({{\varvec{H}}}_{{\varvec{p}}{\varvec{o}}{{\varvec{s}}}_{{\varvec{i}}}}^{\boldsymbol{^{\prime}}}\) is a \(3 x 3m\) matrix which is non-zero only for the elements that correspond to \({{\varvec{b}}}_{po{s}_{i}}\):
This completes an introduction to our ESKF implementation, where we focused on showing the application of the overbounding method.
Appendix 2: LLR for Faulty Sensor Detection
Let:
\({y}_{k}^{0}\) the subset that contains all measurements at time \(k\)
\({y}_{k}^{j}=[{\rho }_{k}^{1},{ \rho }_{k}^{2}, \dots ,{\rho }_{k}^{j-1},{\rho }_{k}^{j+1},\dots ,{\rho }_{k}^{m}]\) the subset that contains all measurements except the \(j\)-th (\(m\) the total number of measurements at time \(k\)).
We define the likelihood:
\({\rho }_{k}\) the observation from the sensor, \({\widetilde{\rho }}_{k}\) the algorithm’s estimation for the same measurement.
For a time range \(a\) to \(b\), the cumulative LLR between two observation subsets is:
Define \({\beta }_{k}^{j}=\underset{k-x+1\le a\le k}{\mathrm{max}}\{{S}_{a}^{k,j}\}\), x is the accumulation time window size, and \({{\varvec{\beta}}}_{k}=[{\beta }_{k}^{1},{\beta }_{k}^{2},\dots ,{\beta }_{k}^{m}]\) is the test statistic.
So:
-
If we have sensors that provide redundant measurements
-
If we assume only up to one faulty sensor at each time
Then having hypotheses with one sensor excluded at each, a hypothesis that correctly assumes a sensor faulty will have much higher LLR than other hypotheses. Intuitively, it can be seen as a measurement of better agreement of all sensors.
Rights and permissions
Open Access This article is licensed under a Creative Commons Attribution 4.0 International License, which permits use, sharing, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons licence, and indicate if changes were made. The images or other third party material in this article are included in the article's Creative Commons licence, unless indicated otherwise in a credit line to the material. If material is not included in the article's Creative Commons licence and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder. To view a copy of this licence, visit http://creativecommons.org/licenses/by/4.0/.
About this article
Cite this article
Bosdelekidis, V., Bryne, T.H., Sokolova, N. et al. Navigation Algorithm-Agnostic Integrity Monitoring based on Solution Separation with Constrained Computation Time and Sensor Noise Overbounding. J Intell Robot Syst 106, 7 (2022). https://doi.org/10.1007/s10846-022-01692-3
Received:
Accepted:
Published:
DOI: https://doi.org/10.1007/s10846-022-01692-3