Dual-Model Reverse CKF Algorithm in Cooperative Navigation for USV

As one of the most promising research directions, cooperative location with high precision and low-cost IMU is becoming an emerging research topic in many positioning fields. Low-cost MEMS/DVL is a preferred solution for dead-reckoning in multiUSV cooperative network. However, largemisalignment angles and large gyro drift coexist in low-costMEMS that leads to the poor observability. Based on cubature Kalman filter (CKF) algorithm that has access to high accuracy and relative small computation, dual-model filtering scheme is proposed. It divides the whole process into two subsections that cut off the coupling relations and improve the observability of MEMS errors: it first estimates large misalignment angle and then estimates the gyro drift. Furthermore, to improve the convergence speed of large misalignment angle estimated in the first subsection, “time reversion” concept is introduced. It uses a short period time to forward and backward several times to improve convergence speed effectively. Finally, simulation analysis and experimental verification is conducted. Simulation and experimental results show that the algorithm can effectively improve the cooperative navigation performance.


Introduction
In recent years, cooperative navigation technology has been gradually expanded in satellite [1], multirobot [2][3][4], intelligent underwater vehicles [5], and many other fields.Research team named Stergios I. Roumeliotis from America has conducted a series of theoretical studies for colocation of robots, including observability analysis, estimation error analysis, consistency analysis, and formation optimization [6].As one of the most promising research directions in navigation and positioning, cooperative navigation is attracting more and more attention in colocation industry and academia.
Unmanned surface vehicle (USV) refers to running crafts on surface of water that can be remotely operated by a person or operate independently.It has characteristic of small size, high mobility, and low-cost.Multi-USV formation can take advantage of network using coordination and communication technology.The US Navy issued "the navy unmanned surface craft master plan" [7] that shows the significance of cooperative combat techniques of multiple USV applied in the military.Cooperative navigation of multi-USV can be commonly divided into two types according to configuration: one is master-slave (leader-follower) structure (some with high-precision and others with low precision); the other is parallel structure (navigation accuracy of each USV is equivalent).Compared to master-slave structure, parallel cooperative navigation cannot reconcile the contradiction between high-precision and low-cost fundamentally, so master-slave USV cooperative navigation is the mainstream currently.
With the rapid development of computer and driven by demand of high-performance filtering in engineering, nonlinear filtering theory has been greatly developed [8][9][10][11][12][13][14].EKF, UKF, and PF are three commonly used nonlinear filtering methods in practical applications.EKF has high calculating efficiency, but the precision of EKF is limited because of the Taylor expansion that neglects high-order terms.UKF and PF have good performance in some applications; however, covariance calculated in the UKF filtering process might be nonpositive definite that leads to numerical instability and filtering divergence, while PF is easy to fall into the particles degradation problem and heavy computational complexity for high-dimensional applications [12].CKF can 2 Mathematical Problems in Engineering avoid linearization of the nonlinear system by using cubature point sets to approximate the mean and variance.Because of its high accuracy and low calculation load, CKF [14] has become a hotspot in research.In this paper, CKF is introduced in the cooperative navigation with nonlinear model.
As a branch of inertial field, inertial systems based on microelectromechanical system (MEMS) gained rapid development in recent years.Because of its low-cost, small size, light weight, and high reliability, there have been more and more applications in low-cost inertial systems.It has become a preferred solution for the inertial navigation device on follower boats in multi-USV cooperative network.However, due to the limitations of accuracy, it is necessary to construct reasonable error model of MEMS based on which estimation and compensation can be preferred [15].Commonly used error compensation methods include RBF neural network analysis, ARMA model used in the timing model of gyro random error, and piecewise interpolation compensation of scale factor [16].However, coexistence of larger initial misalignment and larger gyro drift is still unavoidable in practical use that leads to poor observability in cooperative navigation.Based on different character of initial misalignment and gyro drift, estimation with subsection idea is proposed to cut off the coupling relations.
In addition, how to obtain accurate estimation performance in short time is also a key technique.Li et al. proposed a new alignment method for AUV with backtracking framework.The fine alignment runs with the recorded data during the process of coarse alignment which effectively improves the convergence speed and improve navigation accuracy [17,18].Xixiang Liu also proposed gyrocompass alignment method based on reverse loop to solve compass alignment for SINS on moving base and it greatly reduces the alignment time.To improve the convergence speed of large initial misalignment, "time reversion" concept is introduced in this paper.As will be confirmed by simulation and experimental results, the convergence speed and estimation accuracy are improved greatly.
The rest of this paper is organized as follows.Section 2 presents cooperative navigation model of single master USV and the process of CKF.In Section 3, based on observability analysis of cooperative navigation, dual-model method and reverse filtering are proposed that improve the convergence speed and improve the accuracy of cooperative location.Simulation of the proposed algorithm is provided in Section 4. Aiming at validating the proposed algorithm further, the process and the results of experiment in Tai Lake are given in Section 5; Section 6 gives data analysis and discussions.In Section 7, a summary is provided, and future research directions are discussed.

Fundamental Theory of Cooperative Navigation Modeling.
Master USV and slave USV are equipped with hydroacoustic communication modem, respectively, at their bottoms to achieve information dissemination and distance measurements between USVs.The masters accomplish navigation using their high-precision device (usually strap-down inertial navigation system combined with GPS) while the slaves use their own equipped Doppler velocity log (DVL) and attitude and heading reference system (AHRS) based on MEMS to proceed dead-reckoning.The principle of multi-USV cooperative navigation system is as follows.Firstly master USV sends out fixed frequency acoustic pulse signal according to preset time interval.With this signal, slave USV can calculate the relative distance according to underwater acoustic propagation delay.Then master USV sends out its position information by hydroacoustic modem in the form of broadcasting.After receiving this information, slave USV can conduct information fusion with dead-reckoning, position of master USV, and the relative distance and then correct its navigation result using filtering algorithms.Moreover, route planning of USV can be carried out to improve the observability and further improve the accuracy of the cooperative navigation system.Positioning errors will accumulate over time (as shown by the purple dotted ellipse shown in Figure 1).Therefore, it cannot provide accurate positioning information for a long time.When the information sent by master USV is received, the slave USV can use cooperative navigation algorithm to correct the result of dead-reckoning and effectively reduce the positioning error (as shown by the green dotted ellipse shown in Figure 1).Through this cyclical filtering and correction, slave USV with low-precision navigation device can realize precise location for a long time [19,20].

System Model of Single Master USV.
When the follower does not receive the information sent by leaders, it can only use its own DVL and low accuracy MEMS to conduct deadreckoning, and the motion model can be expressed as follows: where    ,    represent the position of follower USV at time , V  represents the speed, Δ means the time interval, and   means the heading angle at time .However, the input model of system in practice is as follows: wherein V is the velocity measurements and C  means the scale factor error. α is the heading angle measured by MEMS,   = Δ  +Δ is error of α , Δ  is the initial misalignment angle, and  is gyro drift.Putting (2) into (1), the actual equation of states is as follows: Motion model can be simplified as Systematic observation equation can be expressed as: wherein   stands for the measurement of distance   between follower and leader, (   ,    ) represents the position of leader USV at time  while (   ,    ) represents the position of follower, and   ∼ (0,  2  ) is the measurement noise.

Cubature Kalman Filter (CKF).
Cubature Kalman filter (CKF) is proposed based on the spherical-radial cubature criterion [14].The core of the method is that the mean and variance of probability distribution can be approximated by cubature points.CKF first approximates the mean and variance of probability distribution through a set of 2 ( is the dimension of the input random vector) cubature points with the same weight, propagates the above cubature points through nonlinear functions, and then calculates the mean and variance of the current approximate Gaussian distribution by the propagated cubature points.
Systematic state equation and observation equation are expressed in (4) and (6).Assuming the posterior density at time  − 1 is known, the steps of CKF are shown as follows [14].
Time update is as follows: Measurement update is as follows: With the new measurement vector   , the estimation of the state vector X| and its covariance matrix  | at time  can be achieved by the following equations: CKF uses cubature rule and 2 cubature point sets [    ] to compute the mean and variance of probability distribution without any linearization of a nonlinear model.Thus, the model can reach the third-order or higher.

Problem Statement-Poor Observability of Error in MEMS.
A global observability analysis method proposed in [20] indicates that whether a state can be observed or not means if there exists unique solution of the equation.The problem that whether the initial misalignment error and MEMS gyro drift has unique solution will be preliminarily discussed in this section.Here in order to facilitate analysis, only the initial misalignment and gyro drift in MEMS are considered in the system temporarily.Equation ( 2) can be expressed as follows: from which we can obtain the expression of   : where  = arctan((   −   +1 )/(   −   +1 )).So   can be calculated that means it is observable.However, the proportion of initial misalignment and MEMS gyro drift cannot be accurately known.Therefore, the observability is poor.
As mentioned above, poor observability of MEMS is the main issue in cooperative navigation.The elaborate observability analysis and corresponding solutions will be provided in Section 3 below.

Cooperative Navigation Modeling and Observability Analysis
3.1.Observability Analysis.Observability directly determines the colocation accuracy of cooperative navigation system [20]; the observability theory of linear systems will be no longer suitable to multi-AUV cooperative navigation with nonlinear system.The idea here is to linearize the nonlinear model firstly and then to use observability theory of linearized systems for observability analysis.The system in this paper is stochastic system.However, the observability analysis method of stochastic system can be equivalent to the corresponding deterministic system when  > 0 and  > 0 [21,22], which is satisfied in our paper.
As can be seen from Figure 2, when time  > 100, 1/( − 1), 1/( − 2), 1/( − 3), 1/( − 4) is approximately equal.That means the third and fourth columns of the observation matrix Γ become linear-relative and the linear correlation becomes stronger over time.It can be concluded that the observability is poor and it gets worse as time flies.

Resolve Route 1:
Dual-Model Filtering Algorithm.Both of initial misalignment and gyro drift can cause the increase of error angle.The error coupled together results in poor observability.Error caused by the initial misalignment angle is a constant value while the error caused by gyro drift gradually increases over time.Their properties are shown in Figure 3.
The proportion of initial misalignment angle and gyro drift in total error angle over time are shown in Table 1 and Figure 4 (assuming initial misalignment is 90 ∘ and gyro drift is 10 ∘ /h).
From the above analysis, it can be seen that with the passing of time, gyro drift will take increasingly large proportion while the initial misalignment of MEMS occupies absolutely large part of total deviation angle in the beginning.Therefore, we can cut off the entire time into two phases.In the first phase initial misalignment angle is estimated and modified alone while the gyro drift is estimated in the second stage.As long as length of the first phase is short enough, it will not affect the estimation accuracy; that is, Subsection 1: Here we will take a look at observability of the system after subsection.When the system state vector is selected as , the Jacobi matrix is ] ,

Mathematical Problems in Engineering
According to the rank criterion, observability matrix can be written as follows: where rank Γ = 4; that is to say, the matrix Γ is full rank, so the system is observable.For the second phase of cooperative navigation system, we can get the same conclusion.Therefore, it can be concluded that the system becomes observable whenever in the first phase or the second phase after subsection.

Resolve Route 2:
Reverse Algorithm.In Section 3.2, it shows that after using the segmented dual-model filtering, state observability can be significantly improved.However, the proportion of gyro drift in total deviation angle gradually increases as time flies.If the time of first stage is too long, the gyro drift will seriously affect the accuracy of model and the precision of initial misalignment estimation.Currently MEMS devices are with low precision that large initial misalignment and large gyro drift coexist.Therefore, how to estimate the large initial misalignment angle in short time is another problem we have to face.Sampling data in the cooperative navigation system is typically a time sequence.The filter works with chronological sequence of real-time processing.The real-time results can be obtained without data storage.However, similar to strapdown inertial navigation system [18,23,24], one significant feature of cooperative navigation system is that all the information used in filtering process can be completely stored.That means the data can be copied into many identical copies, and each copy can be processed by different methods without interference between them.This feature is also called "the diverse existence of mathematical platform." It is easily conceivable that if storage capacity of the navigation computer is large enough and computing power is strong enough, the data can be stored and analyzed many times in very short time.By repeating the analysis in forward and backward, it is possible to improve the precision or shorten the time cost.
Based on this idea, we can take a short period of time as the first stage and carry forward filtering at the first time while the data is saved.Then through repeated use of the data for backward and forward filtering, the large initial misalignment angle can be accurately estimated in short time.The dream of improving accuracy of the initial misalignment angle in short time can be achieved.The process is shown in Figure 5.
The corresponding forward and reverse state equations of the algorithm are as follows.In order to distinguish between them, we use " → " and "←" to present forward and reverse, respectively.
(1) Forward filtering is as follows: (2) Backward filtering is as follows: The whole "subsection + reverse" filtering process is summarized as follows (as shown in Figure 6): (1) first, initialize the filter parameters; (2) in the first subsection (a short period of time), conduct the initial misalignment estimation ignoring effects of gyro drift and store the data at the same time; (3) use the stored data to estimate the initial misalignment reversely; (4) return to steps (2) and (3) and repeat two or three times until the covariance of the state decreases in demand range (the selection of repeated time is discussed in Section 6); (5) after compensation of initial misalignment using estimation result in the first subsection, estimate the gyro drift and conduct cooperative navigation filtering in the second subsection.

Simulation Analysis
To verify effectiveness of the proposed algorithms and validity of observability analysis above, simulation is conducted in this section.Simulation environment settings are as follows.
(1) A and C are leaders and B is follower.Three USVs are all equipped with underwater acoustic equipment for communication and ranging.A and C send their position alternately; time interval  is set to 5 s.
(2) The initial positions of A, B, and C are (0,200), (0,0), and (200,0).A and C are equipped with GPS receiver that can accomplish self-positioning, and the positioning error does not accumulate.The mean of positioning error is 0 and variance is set to 2 m.
(3) USV B can only conduct dead-reckoning navigation when the information of A or C is not received.The speed and heading angle are given by DVL and MEMS, respectively; the initial misalignment is set to 90 ∘ ; gyro drift is 10 ∘ /h.
(4) C  of speed measured by DVL in USV B is 0.005, and the noise is set to zero mean and variance is 0.5 m/s.Forwards: z 1 , z 2 , . . ., z m z m+1 , z m+2 , . . .x-axis (m) (5) Noise of ranging measurement is set with zero mean and variance is set to 2 m.The filter is initialized as follows: The trajecto ries of three USVs are shown in Figure 7.
Results comparison of corresponding methods is given in Section 4.1.It includes comparison between methods with segmentation and without segmentation and also comparison of method without reverse filtering and with reverse filtering (reverse once and reverse twice).

Comparison of Filtering Performance with and without
Subsection.The whole simulation time is set as 6000 seconds and we take 1500 seconds for the first stage.According to "dual-model algorithm with subsection" proposed in Section 4, the curves of estimation are shown in Figures 8 and  9.
From the comparison in Figures 8 and 9, the following conclusions can be drawn.
(1) As can be seen from Figure 8, the initial misalignment angle without segmentation begins to converge within a certain range but diverges later (as previously analyzed, observability grows gradually worse over time).The error estimation accuracy is better than that without segment, and the modification at the end of first stage effectively suppresses initial misalignment angle divergence.(2) It can be seen in Figures 8 and 9(b) that the estimation performance of gyro drift without segmentation is poor.It gradually converges in a very long time while it converges quickly after subsection.(3) As can be seen in Figure 9(a), the estimation of the initial misalignment angle in subsection 1 is better than that without subsection, but there is still a tendency of divergence.(4) Figure 9(c) tells us that due to better estimation of the initial misalignment angle and gyro drift after segmented, the performance of colocation is better than nonsegments.However, such divergent trend still exists in colocation estimation error due to certain bias of initial heading angle estimation.
In summary, the performance with segment is better than that without segmentation.But there is still space for further improvement especially in how to improve estimation accuracy of the initial misalignment angle of the first phase.

"Subsection + Reverse" Algorithm.
The reverse filtering is introduced on the base of subsection in this section.It gives comparison of methods without reverse, reverse once, and reverse twice.The velocity curve of USV varies between 8 m/s and 15 m/s without reverse.It moves in a straight line with acceleration and deceleration.Figure 10 shows velocity with reverse once and with reverse twice.The corresponding performance comparison is shown in Figure 11.
To make the description more clearly, several axes with different colors are used in the following figures.In Figures 10, 15, and 16, the blue time-axis stands for the algorithm with reverse once and with reverse twice.And in Figures 11, 17, and 18, the time-axes with blue color, green color, and red color represent the time axis of the algorithm without reverse, with reverse once, and with reverse twice, respectively.

Analysis of Simulation Results.
From Figure 11, it can be seen that performance of "subsection + reverse" filtering algorithm is significantly better than that without reverse.Specific analyses are as follows.
(1) From Figures 11(a) and 11(b), it can be seen that when reverse filtering is not used, the initial error converges to a certain angle and diverges later with time.At the end of the first subsection (1500 seconds), estimation reaches 92 ∘ .When using reverse filtering once, the length of sampling data segment is shortened to 500 seconds that reduces the impact of gyro drift.The precision of initial misalignment angle achieves significant improvement.It reaches 90.5 ∘ at the end of the first phase.When reverse filtering is used twice, length of sample data is shortened to 300 seconds that reduce the impact of gyro drift further.Estimation reaches 90.05 ∘ in 300 seconds (which is the end of the first stage).Therefore, estimation accuracy is significantly improved; it effectively suppresses the divergence of initial misalignment angle.
(2) As is shown in Figure 11(c), without the introduction of reverse filtering, the estimated effect of the gyro drift is poor.It costs about 6000 seconds to converge.With introduction of reverse filtering once, gyro drift estimation converges in 3500 seconds while the error estimation of gyro drift converges in 1500 seconds after using reverse algorithm twice.The convergence speed becomes significantly faster.
(3) It can be seen from Figure 11(c) that, at the end of the first stage, the initial misalignment is corrected.Because of the introduction of reverse filtering, the model becomes more accurate and the estimation accuracy becomes much higher.Therefore, the precision of cooperative navigation is always within 20 m as shown in Figure 11(d).

Water Test Verification
To verify the feasibility of proposed cooperative navigation algorithms further, water experiment is conducted at Tai Lake in Wuxi city of Jiangsu Province.For ease of operation, one self-made USV is used for followers and two patrol crafts are chosen to play the role of leader USVs instead.

Overview of the Test.
The equipment installation is as follow.As shown in Figure 12, each leader USV is equipped with GPS, acoustic communication devices, and data collection system.GPS is used for providing accurate positioning information, and acoustic communications equipment (Teledyne Benthos's ATM-885 sonar is selected that can achieve 360 ∘ sonar signal transmission and reception) is used to complete information transmission and measurement of distance between the USVs.Follower USV is equipped with PHINS, GPS, DVL, MEMS, and also ATM-885 sonar.PHINS works in combination mode with GPS.It is benchmark that provides high-accurate heading, attitude, and position and speed information.Dead-reckoning is conducted by speed (provided by DVL) and heading (provided by MEMS).The performance of each equipment is as shown in Table 2.
Equipment installation and signal flow are as shown in Figure 13.
The test procedure is as follows: (1) two water patrol boats are selected as pilot USV (labeled as A and B) and a USV as follower (labeled as C); the computer clock is synchronized precisely.(2) Dead-reckoning is conducted using speed provided by DVL and heading angle provided by MEMS to obtain the real-time location.(3) The two leaders send their location information to C alternately using acoustic device; the interval is selected as 10 s. (4) After receiving aided information and distance, information fusion is made by using the dead-reckoning information, location information from leaders, and the distance measurement.
In the experiment, complete test data are stored to make offline analysis and comparison of different filtering methods.The position information provided by GPS installed on USV C is used as benchmark to make comparison of different methods.Similarly, PHINS on C is used as the benchmark for heading.Shot by Google earth map photo, three USVs trajectories are shown in Figure 14 (about 20 kilometers of the whole voyage).

Data Processing.
We take 1250 seconds for the first stage in which initial misalignment is estimated, and gyro drift

Analysis.
From the simulation results in Figures 17 and  18, discussion is made as follows.
(1) Figure 17(a) shows that when backward filtering is not used, the initial misalignment converges to certain angle but becomes divergence later.Estimation reaches 95 ∘ at the 1250th second (which is the end of the first phase).When reverse filtering is used once, sampling data length is shortened to 417 seconds and the impact of gyro drift is suppressed sharply.Accuracy of initial misalignment estimation is improved (that is 92 ∘ at 417th second).To reduce the impact of gyro drift further, reverse filtering is used twice.Sampling data length is shortened to 250 seconds while estimation becomes 89.9 ∘ in 250 seconds.Accuracy of estimation is significantly improved, and the error is corrected in the end of the first subsection.
(2) From Figure 17(b), it can be seen that estimation performance of gyro drift is poor without the introduction of reverse filtering.It gradually converges in 3500 seconds.By using the reverse filtering once, the convergence of gyro drift costs about 1500 seconds while it converges in 500 seconds by using the reverse filtering twice.The convergence speed become much faster.
(3) Figure 18(b) shows the divergent trend of cooperative navigation error in 6000 seconds without inverse algorithm and the maximum error is larger than 500 meters.After the introduction of reverse filtering, cooperative navigation and positioning accuracy is significantly improved as initial misalignment angle error and gyro drift are estimated more accurately.The maximum positioning error is less than 400 meters with reverse once while it is within 300 meters with reverse twice.

Discussion
Through the above analysis and comparison of experimental data, it can be concluded as follows.
(1) Because dead-reckoning cannot provide precise positioning information in long period, the cooperative navigation algorithm with CKF is introduced and "subsection + reverse" solution is also proposed to improve accuracy effectively while time cost is reduced.
(2) There are still some problems to be solved in practical applications.For example, ranging accuracy and the communication quality greatly affect the performance of colocation.In speedup, a lot of bubbles appear around underwater acoustic equipment and information cannot be successfully transferred.More stable prediction algorithm or effective compensation is required and the data packet is lost.In addition, there are still some other problems such as uncertain noise and the influence of unknown current.
(3) Two new questions appear in front of us.The first one is whether the continuously shortened sampling data could result in continuously better performance.
The second is whether length of the first stage can be shortened unlimitedly.For these two questions we do two more simulations: the length of the first phase is taken as 150 seconds and 30 seconds separately.The reversion is set as 40 times and 200 times correspondingly.Initial misalignment estimation and colocation error convergence curve are shown in Figure 19.
From Figure 19 we can see that the estimation accuracy can be improved by shortening the length of sampling data period (such the case of 150 seconds), but the improvement is not that significant.When length of sampling data becomes less than certain value (e.g., 30 seconds) the estimation accuracy of the initial misalignment cannot be improved but be depressed.Then in the second subsection, divergent  tendency appears in the estimation of gyro drift and position error.It is mainly because when time period is too short, effect of noise would be amplified rapidly.Therefore, it is very important to make further study to select the data length more reasonably.

Summary
In this paper, we research on cooperative navigation based on dead-reckoning with MEMS/DVL.The nonlinear system model of cooperative navigation is constructed and CKF

Figure 2 :
Figure 2: Relative analysis of the column in observability matrix.

Figure 3 :
Figure 3: Characteristics of initial misalignment angle and gyro drift.

Figure 4 :
Figure 4: The proportion of initial misalignment angle and gyro drift.

( 6 )
A, B, and C are in linear motion with speedup and speed-down.The speed changes between 8 m/s and 15 m/s by ±0.05 m/s 2 without considering the dynamics and current disturbance.

Figure 8 :
Figure 8: Estimation comparison with and without subsection.
Gyro drift estimation in long time without subsection

Figure 9 :
Figure 9: Comparison of filtering performance with and without subsection.

Figure 11 :
Figure 11: Comparison with no reverse, reverse once, and reverse twice.

Figure 12 :Figure 13 :
Figure 12: Unmanned craft as follower (a) and two patrol crafts as leaders (b).

Figure 18 :
Figure 18: Comparison of the positioning errors.

Table 1 :
The proportion varies in the first 500 s.

Table 2 :
The performance of equipment.