# A Real-Time Mismatch Detection Method for Underwater Database-Referenced Navigation

^{*}

## Abstract

**:**

## 1. Introduction

## 2. MSAC and WGTM

#### 2.1. RANSAC and MSAC

#### 2.2. GTM and WGTM

- (1)
- Like GTM, WGTM also generates two median KNN graphs ${G}_{p}=\left\{{V}_{p},{E}_{p}\right\}$ (with adjacency matrix ${A}_{p}$) and ${G}_{q}=\left\{{V}_{q},{E}_{q}\right\}$ (with adjacency matrix ${A}_{q}$) for the sets $P=\left\{{p}_{i}\right\}$ and $Q=\left\{{q}_{i}\right\}$. The difference is that the two graphs of ${G}_{p}$ and ${G}_{q}$ are directed graphs. A directed edge $(i,j)$ exists when ${p}_{j}$ is one of the K-nearest neighbors of ${p}_{i}$, and also, $\Vert {p}_{i}-{p}_{j}\Vert \le \eta $, $\eta $ is defined by (4). The adjacency matrix is defined by (5).
- (2)
- Find all vertices of ${G}_{p}$ with at most one edge with other vertices, and remove them and their correspondences from ${G}_{p}$ and ${G}_{q}$. Regenerate ${G}_{p}$ and ${G}_{q}$. Repeat this process until all vertices of ${G}_{p}$ have a minimum of two edges.
- (3)
- For the edge that connects ${v}_{i}$ to ${v}_{m}$ compute a weight value using the following equation:$$W(i,m)=\left|\mathrm{arccos}\left(\frac{({p}_{m}-{p}_{i})\cdot \left(Rot(\theta ({k}_{\mathrm{min}},i))({q}_{m}-{q}_{i})\right)}{\Vert {p}_{m}-{p}_{i}\Vert \Vert {q}_{m}-{q}_{i}\Vert}\right)\right|$$$$Rot(\theta ({k}_{\mathrm{min}},i))=\left[\begin{array}{cc}\mathrm{cos}(\theta ({k}_{\mathrm{min}},i))& -\mathrm{sin}(\theta ({k}_{\mathrm{min}},i))\\ \mathrm{sin}(\theta ({k}_{\mathrm{min}},i))& \mathrm{cos}(\theta ({k}_{\mathrm{min}},i))\end{array}\right]$$
- (4)
- For each vertex ${v}_{i}$ in ${V}_{p}$, find the percentage of edges that are connected to ${v}_{i}$ with their correspondences connected to ${v}_{i}^{\prime}$ in ${V}_{q}$. The weight value is set to be $\pi $ if the percentage is smaller than 50%.
- (5)
- For each vertex ${v}_{i}$ in ${V}_{p}$, compute the mean of all weights by (9). Remove the vertex corresponding to the maximum value of $w$ and all of its corresponding vertices from $P$ and $Q$:$$w(i)=\underset{\forall j,(i,j)\in {E}_{p}}{\mathrm{mean}}(W(i,j))$$
- (6)
- If the maximum value of $w$ is less than $\pi $, and the change in the mean value of $w(i)$ is less than the threshold, the iteration is terminated; otherwise, it goes to the next iteration.

## 3. The INS Error Propagation Model

_{M}is the local radius of the curvature in the meridian, and R

_{N}is the local radius of the curvature in the prime vertical. $\widehat{L}=L+\delta L$, $\widehat{h}=h+\delta h$. $\delta L$ and $\delta h$ are both slight errors. The following equations have been derived:

## 4. The Real-Time Triple Constraint Mismatch Detection Method

- (1)
- MSAC and WGTM have poor real-time performance. These two algorithms eliminate all of the outliers by iteration. Conversely, in a navigation system, the most important thing is to judge whether the current position is mismatched.
- (2)
- MSAC cannot detect the mismatched point that fits the model, and WGTM cannot detect the mismatched point with the same structure.
- (3)
- Both MSAC and WGTM do not take into account the distance constraint between the sampling points.

#### 4.1. Model Fitting Detection

**M**. Functions (1) and (3) are used to generate the value of cost function C

**.**

_{M}**M**with the largest number of inliers, and the smallest value of C

**is the best linear model**

_{M}**M**

_{best}. In practical experiments, the correct error threshold T is set to three grids (found empirically). Finally, the error ${e}_{{x}_{i}^{\mathrm{match}}}$ is calculated for each point in ${X}^{\mathrm{match}}$ relative to

**M**

_{best}. If ${x}_{N}^{\mathrm{match}}$ is an outlier and ${e}_{{x}_{N}^{\mathrm{match}}}$ is the largest error among all points in ${X}^{\mathrm{match}}$, then ${x}_{N}^{\mathrm{match}}$ is a mismatched point. The pseudo-code of model fitting detection is shown in Algorithm 1.

Algorithm 1. Model fitting detection |

Begin1. select input point set ${X}^{\mathrm{match}}$ 2. initialize ${N}_{{M}_{\mathrm{best}}}^{\mathrm{inliers}}=0$ 3. for I = 1 to $N-1$ 4. for j = 2 to N calculate the parameters of the linear model M according to ${x}_{i}^{\mathrm{match}}$ and ${x}_{j}^{\mathrm{match}}$ |

compute ${N}_{M}^{\mathrm{inliers}}$ (the number of inliers) in ${X}^{\mathrm{match}}$ based on M and ${\rho}_{2}$calculate the cost function C _{M}if ${N}_{M}^{\mathrm{inliers}}>{N}_{{M}_{\mathrm{best}}}^{\mathrm{inliers}}$ or (${N}_{M}^{\mathrm{inliers}}={N}_{{M}_{\mathrm{best}}}^{\mathrm{inliers}}$ and ${C}_{M}<{C}_{{M}_{\mathrm{best}}}$) ${M}_{\mathrm{best}}=M$ end if end for j end for i 5. if ${x}_{N}^{\mathrm{match}}$ is an outlier and ${e}_{{x}_{i}^{\mathrm{match}}}^{\mathrm{max}}={e}_{{x}_{N}^{\mathrm{match}}}$, $i=\left\{1,2,\dots ,N\right\}$, ${x}_{N}^{\mathrm{match}}$ is a mismatched point |

End |

#### 4.2. Spatial Structure Detection

**Q**the covariance matrix:

**Q**and the rotation angle from ${X}^{\mathrm{INS}}$ to ${X}^{\mathrm{match}}$ are calculated through a quaternion algorithm [33]:

**Q**, and ${\lambda}_{\mathrm{m}}$ is the maximum eigenvalue. $\tau $ is the rotation angle from ${X}^{\mathrm{INS}}$ to ${X}^{\mathrm{match}}$. The spatial structure detection module can be described by the following steps.

- (1)
- Two median KNN graphs ${G}^{\mathrm{INS}}=\left\{{V}^{\mathrm{INS}},{E}^{\mathrm{INS}}\right\}$ (with the adjacency matrix ${A}^{\mathrm{INS}}$) and ${G}^{\mathrm{match}}=\left\{{V}^{\mathrm{match}},{E}^{\mathrm{match}}\right\}$ (with adjacency matrix ${A}^{\mathrm{match}}$) are generated for the trajectories ${X}^{\mathrm{INS}}=\left\{{x}_{1}^{\mathrm{INS}},{x}_{2}^{\mathrm{INS}},\dots ,{x}_{N}^{\mathrm{INS}}\right\}$ and ${X}^{\mathrm{match}}=\left\{{x}_{1}^{\mathrm{match}},{x}_{2}^{\mathrm{match}},\dots ,{x}_{N}^{\mathrm{match}}\right\}$. A directed edge $(i,j)$ exists when ${x}_{j}$ is one of the K-nearest neighbors of ${x}_{i}$ and also $\Vert {x}_{i}-{x}_{j}\Vert \le \eta $, $\eta $ is defined by (4). The adjacency matrix is defined by (5).
- (2)
- For the edge that connects ${v}_{i}^{\mathrm{match}}$ to ${v}_{m}^{\mathrm{match}}$ compute a weight value using the following equation:$$W(i,m)=\left|\mathrm{arccos}\left(\frac{({x}_{m}^{\mathrm{INS}}-{x}_{i}^{\mathrm{INS}})\cdot \left({R}_{\tau}({x}_{m}^{\mathrm{match}}-{x}_{i}^{\mathrm{match}})\right)}{\Vert {x}_{m}^{\mathrm{INS}}-{x}_{i}^{\mathrm{INS}}\Vert \Vert {x}_{m}^{\mathrm{match}}-{x}_{i}^{\mathrm{match}}\Vert}\right)\right|$$$${R}_{\tau}=\left[\begin{array}{cc}\mathrm{cos}\tau & -\mathrm{sin}\tau \\ \mathrm{sin}\tau & \mathrm{cos}\tau \end{array}\right]$$Here, $\tau $ represents the optimal rotation angle calculated by (22). ${R}_{\tau}$ is the corresponding rotation matrix.
- (3)
- For vertex ${v}_{N}^{\mathrm{match}}$, find the percentage of edges that are connected to ${v}_{N}^{\mathrm{match}}$, with their correspondences connected to ${v}_{N}^{\mathrm{INS}}$. If the percentage is smaller than 50%, the weight value of all different edges should be replaced by $\pi $.
- (4)
- For each vertex ${v}_{i}^{\mathrm{match}}$, compute the mean of all weights by the following expression:$$w(i)=\underset{\forall j,(i,j)\in {E}^{\mathrm{match}}}{\mathrm{mean}}(W(i,j))$$
- (5)
- If the maximum value of $w$ is not $w(N)$, ${x}_{N}$ pass the spatial structure detection; otherwise, set:$${\mu}_{\mathrm{old}}=\underset{\forall i}{\mathrm{mean}}(w(i))$$

Algorithm 2. Spatial Structure Detection |

Begin:1. select input point sets ${X}^{\mathrm{INS}}=\left\{{x}_{i}^{\mathrm{INS}}\right\}$ and ${X}^{\mathrm{match}}=\left\{{x}_{i}^{\mathrm{match}}\right\}$, $i=\left\{1,2,\dots ,N\right\}$ 2. create ${G}^{\mathrm{INS}}$, ${G}^{\mathrm{match}}$, ${A}^{\mathrm{INS}}$, ${A}^{\mathrm{match}}$ 3. compute W 4. $\forall (i,j)\in {E}^{\mathrm{match}}$, compute $w(i)=\underset{}{\mathrm{mean}}(W(i,j))$ 5. if $\mathrm{max}(w(i))\ne w(N)$ ${x}_{N}$ pass the spatial structure detection else ${\mu}_{\mathrm{old}}=\underset{\forall i}{\mathrm{mean}}(w(i))$ delete ${x}_{N}$ and regenerate ${X}^{\mathrm{INS}}=\left\{{x}_{i}^{\mathrm{INS}}\right\}$, ${X}^{\mathrm{match}}=\left\{{x}_{i}^{\mathrm{match}}\right\}$, $i=\left\{1,2,\dots ,N-1\right\}$ repeat steps 1 to 4, calculate ${\mu}_{\mathrm{new}}$ if $\left|{\mu}_{\mathrm{new}}-{\mu}_{\mathrm{old}}\right|<\epsilon $ ${x}_{N}$ pass the spatial structure detection else ${x}_{N}$ is a mismatched point end if end if |

End |

#### 4.3. Distance Ratio Detection

Algorithm 3. Distance ratio detection |

Begin:1. select input point sets ${X}^{\mathrm{INS}}=\left\{{x}_{i}^{\mathrm{INS}}\right\}$ and ${X}^{\mathrm{match}}=\left\{{x}_{i}^{\mathrm{match}}\right\}$, $i=\left\{1,2,\dots ,N\right\}$ 2. compute $\frac{{d}_{N-1,N}^{\mathrm{match}}}{{d}_{N-1,N}^{\mathrm{INS}}}$ 3. if ${S}_{\mathrm{min}}\le \frac{{d}_{N-1,N}^{\mathrm{match}}}{{d}_{N-1,N}^{\mathrm{INS}}}\le {S}_{\mathrm{max}}$ ${x}_{N}$ passes the distance ratio detection else ${x}_{N}$ is a mismatched point end if |

End |

## 5. Simulation and Analysis

#### 5.1. Simulation of INS

#### 5.2. Simulation of the Proposed Algorithm

#### 5.3. Comparison between the RSOC-Based Mismatch Diagnostic Algorithm and the Proposed Method

#### 5.4. Influence of the Threshold Value

## 6. Conclusions

## Author Contributions

## Funding

## Conflicts of Interest

## References

- Paull, L.; Saeedi, S.; Seto, M.; Li, H. AUV navigation and localization: A review. IEEE J. Ocean. Eng.
**2013**, 39, 131–149. [Google Scholar] [CrossRef] - Ramesh, R.; Jyothi, V.B.N.; Vedachalam, N.; Ramadass, G.A. Development and performance validation of a navigation system for an underwater vehicle. J. Navig.
**2016**, 69, 1097–1113. [Google Scholar] [CrossRef] - Caffaz, A.; Caiti, A.; Casalino, G.; Turetta, A. The hybrid glider/AUV folaga. IEEE Robot. Autom. Mag.
**2010**, 17, 31–44. [Google Scholar] [CrossRef] - Allotta, B.; Caiti, A.; Costanzi, R.; Fanelli, F.; Fenucci, D.; Meli, E.; Ridolfi, A. A new AUV navigation system exploiting unscented Kalman filter. Ocean Eng.
**2016**, 113, 121–132. [Google Scholar] [CrossRef] - Wynn, R.B.; Huvenne, V.A.I.; Le Bas, T.P.; Murton, B.J.; Connelly, D.P.; Bett, B.J.; Ruhl, H.A.; Morris, K.J.; Peakall, J.; Parsons, D.R.; et al. Autonomous underwater vehicles (AUVs): Their past, present and future contributions to the advancement of marine geoscience. Mar. Geol.
**2014**, 352, 451–468. [Google Scholar] [CrossRef] - Stutters, L.; Liu, H.; Tiltman, C.; Brown, D.J. Navigation technologies for autonomous underwater vehicles. IEEE Trans. Syst. Man Cybern. Part C
**2008**, 38, 581–589. [Google Scholar] [CrossRef] - Pérez-Alcocer, R.; Torres-Méndez, L.A.; Olguín-Díaz, E.; Maldonado-Ramírez, A.A. Vision-based autonomous underwater vehicle navigation in poor visibility conditions using a model-free robust control. J. Sens.
**2016**, 2016, 1–16. [Google Scholar] [CrossRef] - Canciani, A.; Raquet, J. Airborne magnetic anomaly navigation. IEEE Trans. Aerosp. Electron. Syst.
**2017**, 53, 67–80. [Google Scholar] [CrossRef] - Han, Y.; Wang, B.; Deng, Z.; Wang, S.; Fu, M. A mismatch diagnostic method for TERCOM-based underwater gravity aided navigation. IEEE Sens. J.
**2017**, 17, 2880–2888. [Google Scholar] [CrossRef] - Rice, H.; Kelmenson, S.; Mendelsohn, L. Geophysical navigation technologies and applications. In Proceedings of the IEEE Position Location and Navigation Symposium, Monterey, CA, USA, 26–29 April 2004; pp. 618–624. [Google Scholar] [CrossRef]
- Canciani, A.; Raquet, J. Absolute positioning using the Earth’s magnetic anomaly field. Navigation
**2016**, 63, 111–126. [Google Scholar] [CrossRef] - Lee, J.; Kwon, J.H.; Yu, M. Performance evaluation and requirements assessment for gravity gradient referenced navigation. Sensors
**2015**, 15, 16833–16847. [Google Scholar] [CrossRef] - Hegrenaes, O.; Hallingstad, O. Model-aided INS with sea current estimation for robust underwater navigation. IEEE J. Ocean. Eng.
**2011**, 36, 316–337. [Google Scholar] [CrossRef] - Wu, L.; Wang, H.; Chai, H.; Zhang, L.; Hsu, H.; Wang, Y. Performance Evaluation and Analysis for Gravity Matching Aided Navigation. Sensors
**2017**, 17, 769. [Google Scholar] [CrossRef] - Kim, T.; Kim, J.; Byun, S.W. A comparison of nonlinear filter algorithms for terrain-referenced underwater navigation. Int. J. Control Autom. Syst.
**2018**, 16, 2977–2989. [Google Scholar] [CrossRef] - Simanek, J.; Reinstein, M.; Kubelka, V. Evaluation of the ekf-based estimation architectures for data fusion in mobile robots. IEEE/ASME Trans. Mechatron.
**2015**, 20, 985–990. [Google Scholar] [CrossRef] - Kim, Y.; Park, J.; Bang, H. Terrain-referenced navigation using an interferometric radar altimeter. Navig. J. Inst. Navig.
**2018**, 65, 157–167. [Google Scholar] [CrossRef] - Kim, Y.; Hong, K.; Bang, H. Utilizing out-of-sequence measurement for ambiguous update in particle filtering. IEEE Trans. Aerosp. Electron. Syst.
**2018**, 54, 493–501. [Google Scholar] [CrossRef] - Han, Y.; Wang, B.; Deng, Z.; Wang, S.; Fu, M. An improved TERCOM-based algorithm for gravity-aided navigation. IEEE Sens. J.
**2016**, 16, 2537–2544. [Google Scholar] [CrossRef] - Yang, Y.; Wang, K. Mismatching judgment using PDAF in ICCP algorithm. In Proceedings of the Fourth International Conference on Natural Computation, Jinan, China, 18–20 October 2008. [Google Scholar] [CrossRef]
- Niedfeldt, P.; Beard, R. Convergence and complexity analysis of recursive-RANSAC: A new multiple target tracking algorithm. IEEE Trans. Autom. Control
**2016**, 61, 456–461. [Google Scholar] [CrossRef] - Xu, B.; Jiang, W.; Shan, J.; Zhang, J.; Li, L. Investigation on the weighted RANSAC approaches for building roof plane segmentation from lidar point clouds. Remote Sens.
**2015**, 8, 5. [Google Scholar] [CrossRef] - Li, Y.; Gans, N.R. Predictive ransac: Effective model fitting and tracking approach under heavy noise and outliers. Comput. Vis. Image Underst.
**2017**, 161, 99–113. [Google Scholar] [CrossRef] - Aguilar, W.; Frauel, Y.; Escolano, F.; Martinez-Perez, M.E.; Espinosa-Romero, A.; Lozano, M.A. A robust graph transformation matching for non-rigid registration. Image Vis. Comput.
**2009**, 27, 897–910. [Google Scholar] [CrossRef] - Izadi, M.; Saeedi, P. Robust weighted graph transformation matching for rigid and nonrigid image registration. IEEE Trans. Image Process.
**2012**, 21, 4369–4382. [Google Scholar] [CrossRef] - Liu, Z.; An, J.; Yu, J. A simple and robust feature point matching algorithm based on restricted spatial order constraints for aerial image registration. IEEE Trans. Geosci. Remote Sens.
**2012**, 50, 514–527. [Google Scholar] [CrossRef] - Torr, P.H.S.; Zisserman, A. MLESAC: A new robust estimator with application to estimating image geometry. J. Comput. Vis. Image Underst.
**2000**, 78, 138–156. [Google Scholar] [CrossRef] - Nurunnabi, A.; Belton, D.; West, G. Robust statistical approaches for local planar surface fitting in 3d laser scanning data. ISPRS J. Photogramm. Remote Sens.
**2014**, 96, 106–122. [Google Scholar] [CrossRef] - Choi, S.; Kim, T.; Yu, W. Performance evaluation of RANSAC family. Br. Mach. Vision Conf.
**2009**. [Google Scholar] [CrossRef] - Yan, G.; Yan, W.; Xu, D. Application of simplified UKF in SINS initial alignment for large misalignment angles. J. Chin. Inert. Technol.
**2008**, 16, 253–264. [Google Scholar] - Wu, L.; Wang, H.; Chai, H.; Hsu, H.; Wang, Y. Research on the relative positions-constrained pattern matching method for underwater gravity-aided inertial navigation. J. Navig.
**2015**, 68, 937–950. [Google Scholar] [CrossRef] - Claus, B.; Bachmayer, R. Terrain-aided navigation for an underwater glider. J. Field Robot.
**2015**, 32, 935–951. [Google Scholar] [CrossRef] - Berthold, K.P.H. Closed-form solution of absolute orientation using unit quaternions. J. Opt. Soc. Am. A
**1987**, 4, 629–642. [Google Scholar] [CrossRef] - Song, Z.; Zhang, J.; Zhu, W.; Xi, X. The vector matching method in geomagnetic aiding navigation. Sensors
**2016**, 16, 1120. [Google Scholar] [CrossRef] - Maus, S.; Barckhausen, U.; Berkenbosch, H.; Bournas, N.; Brozena, J.; Childers, V.; Dostaler, F.; Fairhead, J.D.; Finn, C.; von Frese, R.R.B.; et al. Emag2: A 2-arc min resolution earth magnetic anomaly grid compiled from satellite, airborne, and marine magnetic measurements. Geochem. Geophys. Geosyst.
**2009**, 10. [Google Scholar] [CrossRef]

**Figure 6.**Mismatch detection results of the restricted spatial order constraints (RSOC)-based mismatch diagnostic algorithm and the proposed method.

Parameters | Quantity | Unit |
---|---|---|

Gyro constant drift | 0.02 | °/hr |

Gyro random drift ($1\sigma $) | 0.02 | °/hr |

Accelerometer constant bias | 100 | $\mathsf{\mu}\mathrm{g}$ |

Accelerometer random bias ($1\sigma $) | 100 | $\mathsf{\mu}\mathrm{g}$ |

Velocity | 7.71 | $\mathrm{m}/\mathrm{s}$ |

Acceleration | 0 | $\mathrm{m}/\mathrm{s}$ |

Initial angle error | 0 | $\xb0$ |

Azimuth angle | 60 | $\xb0$ |

Initial longitude error | 0.1 | ′ |

Initial latitude error | 0.1 | ′ |

Simulation time | 10 | hr |

Parameters | Quantity | Unit |
---|---|---|

Number of grid points | 840 × 840 | points |

Grid step | 0.3 | ′ |

Minimum value | −719.21 | nT |

Maximum value | 253.44 | nT |

Mean | −2.46 | nT |

Parameters | Parameter Values |
---|---|

Maximum number of iterations | 500 |

Number of sampling points per sequence | 13 |

Sampling interval | 5 min |

The variance of the magnetic anomaly measurement noise | 1 nT |

Detection Method | CD | TN | CR | DR |
---|---|---|---|---|

RSOC-based mismatch diagnostic algorithm | 30 | 34 | 88.24% | 46.88% |

Proposed algorithm | 45 | 54 | 83.33% | 70.31% |

Threshold Value | CD | TN | CR | DR | |
---|---|---|---|---|---|

T | $\mathit{\epsilon}$ | ||||

3 grid | 0.001 | 59 | 86 | 68.60% | 92.19% |

3 grid | 0.1 | 41 | 47 | 87.23% | 64.06% |

0.5 grid | 0.01 | 62 | 97 | 63.92% | 96.88% |

7 grid | 0.01 | 36 | 41 | 87.90% | 56.25% |

© 2019 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Dai, T.; Miao, L.; Guo, Y.
A Real-Time Mismatch Detection Method for Underwater Database-Referenced Navigation. *Sensors* **2019**, *19*, 307.
https://doi.org/10.3390/s19020307

**AMA Style**

Dai T, Miao L, Guo Y.
A Real-Time Mismatch Detection Method for Underwater Database-Referenced Navigation. *Sensors*. 2019; 19(2):307.
https://doi.org/10.3390/s19020307

**Chicago/Turabian Style**

Dai, Tian, Lingjuan Miao, and Yanbing Guo.
2019. "A Real-Time Mismatch Detection Method for Underwater Database-Referenced Navigation" *Sensors* 19, no. 2: 307.
https://doi.org/10.3390/s19020307