A Study on Minimum Sigma Set SRUKF Based GPS / INS 1 Tightly-Coupled System 2

Maosong Wang 1,2, Xiaofeng He *, Wenqi Wu 1 and Zhenbo Liu 2 3 1 College of Mechatronics and Automation, National University of Defense Technology, 4 Changsha 410073, China; wangmaosong12@hotmail.com (M.W.); wenqiwu_lit@hotmail.com (W.W.) 5 2 Department of Geomatics Engineering, The University of Calgary, 2500 University Dr. N.W. Calgary, 6 Alberta, T2N 1N4, Canada; maosong.wang@ucalgary.ca (M.W.); zhenbo.liu2@ucalgary.ca (Z.L.) 7


Introduction
Extended Kalman filter (EKF) and unscented Kalman filter (UKF) have been widely used in tightly-coupled GPS/INS systems [1][2][3][4][5].Usually, for land vehicles, EKF is enough to recover their navigation accuracies [6,7].However, it is just a first-order sub-optimal filter and can easily lead to divergence, especially when the system has severe nonlinearities.UKF is a better mean to achieve approximation to the nonlinear system since it is easier to approximate a probability distribution through UT [8,9].However, there are also drawbacks to it.For example, its computation load is larger than EKF [10].To save the computation cost of UKF, some variants of UKF were proposed, for example, the simplex sigma set and spherical simplex sigma set proposed by Julier [11,12], and the minimum sigma set of [13].
Apart from the large computation load, the positive definiteness of the state covariance cannot be guaranteed by using UKF.SRUKF is the improvement of UKF [14], which uses QR decomposition instead of Cholesky decomposition.This can not only avoid the most costly expensive operation by merely calculating the square-root of the state covariance at each time updating, but also guarantee the positive semi-definiteness of the state covariance to ensure the stability of the computing.From this perspective, SRUKF is a better filter method to replace EKF and UKF.
The UKF and SRUKF family were systematically illustrated by Menegaz [15].However, there are some conceptual statements in the reduced sigma point set section, for example, Menegaz said that only the algorithm proposed by [16] that sigma set less than 2n matched the mean and covariance matrices (CM) of the system state.However, this statement is debatable.These statements will be illustrated explicitly in Section 3 in this paper.
Generally, for land vehicles, tightly-coupled GPS/INS systems have similar accuracy with loosely-coupled systems if the vehicles are not in high maneuver environments.However, for flight vehicles, like missiles and UAVs, a sharp maneuver (like a sudden turn) will cause the observation information not enough for the system.One method to improve the navigation accuracy during GPS outages is to add more sensors in the system, but it can be of high cost, like all-sources navigation system [17].Other methods use artificial neural network (ANN) in loosely-coupled GPS/INS systems [18][19][20][21].However, there are some limitations regarding the optimization of the ANN parameters during the operation of the system.For instance, the huge computation load was unsuitable for real-time implementation.Locata or pseudo satellite is a ground-based positioning system with configurable constellation design, which can help to ensure availability and continuity of Position, Navigation and Timing (PNT) services, even when individual GNSS services are disrupted.
The references have demonstrated that the feasibility of using this technology for a wide range of positioning applications [22][23][24].
In this paper, reduced sigma set SRUKFs are introduced to the GPS/INS tightly-coupled system.
Comparisons are made among the reduced sigma set SRUKFs and EKF.Simulations based on post-processing the data from the real UAV field test show that all the SRUKFs have higher accuracy than EKF especially when GPS signal is deficient.All the reduced sigma point SRUKFs have nearly the same accuracy with standard SRUKF, but their computation costs are greatly saved than the latter.And all the SRUKFs have faster convergence rates than EKF when GPS visible satellites numbers are recovered from partial outages to normal level.Additionally, the estimation of constant biases of gyros and accelerometers also show that SRUKFs have faster convergence rates than EKF.A pseudo-satellite aided mechanism is introduced to the M-SRUKF based GPS/INS tightly-coupled system when observation information is deficient.Experiments of UAV showed that this mechanism worked well in the pseudo-satellite aided GPS/INS tightly-coupled system.
The rest part of this paper is organized as follows.Section 2 designs the GPS/INS tightly-coupled system model.Section 3 states and revises the filter flows of the reduced sigma point SRUKFs to be used in the GPS/INS tightly-coupled system, whilst a pseudo aided mechanism is proposed in Section 4. Numerous simulations based on post-processing the UAV experiment data are done in Section 5. Some conclusions and major contributions are summarized in Section 6.

GPS/INS Tightly-Coupled Model
The dynamic models of tightly-coupled GPS/INS navigation system are defined in NED (north, east, down) navigation frame [25], which are in the error state form.
tan sec sec sec The sensor errors of gyros and accelerometers are modeled as random walk processes: The error state vector of the tightly-coupled system is represented as: (7) The noise vector is written as:  (8) For the observation equations used by EKF, readers can reference the paper [10].Here we only give the observation equations used by SR-UKFs.
The pseudorange corresponding to the i-th satellite can be modeled as [26].
i,sat sins i,sat sins The pseudorange rate corresponding to the i-th satellite can be modeled as (10) (10) where i,sat For low-cost GPS receivers, the observations models can also be written as (11) and (12), since the clock errors and clock drifts can be removed through the differences between satellites.
i,sat sins i,sat sins 0,sat sins 0,sat sins (11) i,sat sins i,sat sins i,sat sins i,sat sins 0,sat sins 0,sat sins 0,sat sins 0,sat sins (12) where 0,sat c r , 0,sat c v are the position and velocity vectors of the reference satellite.
However, there are drawbacks by using (11) and (12), for example, if there are not enough satellites observed by the receiver (for example, only three visible satellites), then the observation information will be deficient, because only six observation equations are used.In order to maintain the observation information, the observation equations used in this paper are the form in ( 9) and (10).
Assume that there are n visible satellites, then the observation equations of the SR-UKFs GPS/INS tightly-coupled system can be written as：  (13) where

Reduced Sigma Point Square-root Unscented Kalman Filter
EKF is a widely used means of Kalman filter, but it only uses the first-order approximation to the non-linear system.Therefore, it often introduces large errors in the estimated statistics of the posterior distributions of the states, while UKF does not need to linearize the observation equations and can be designed easily.SR-UKF is the improvements of UKF, and it has better performance than UKF, in terms of accuracy and computation load.Three key techniques are used in SRUKF, which are QR decomposition, Cholesky factor updating and efficient least squares.
SRUKF also has variants with reduced sigma sets [27,28], which were systematically illustrated in [15].However, there are conceptual oversights statements in [15], for example, the statement that only the minimum sigma set proposed by [16] matched the mean and covariance matrices (CM) of system state was questionable.
In this section, the questionable statements and debatable formulas of some reduced sigma set UTs are revised.

Simplex Unscented Transformation
The simplex unscented transformation was proposed by [11], however, the UT flow in his paper is questionable.
However, the vector sequence in the UT flow was still questionable in Simon's book.
2) The questionable formula of the vector sequence The formula of the vector sequence 1 j i σ + is debatable.For example, when is three dimensional, which should be equal to the dimension of the state n .
Simon revised this formula, but the formula of Simon was still questionable.
It is well known that the most important character of UT method is that it captures the mean and the covariance matrices (CM) of the system state.But, the formula in Simon's book did not satisfy this character.For instance, let the state vector before UT X equals [ ] 2 1 0 × ，the covariance matrix ，initial weight 0 W equals 0.5 ，then the mean and the covariance of the state after UT can be calculated as: After revising the formulas of simplex sigma set UT, the new flow is shown in Figure 1.
4) Expand vector sequence for 2, j n =  , according to: × unit matrix， 0 0.5 W = ，we can calculate the mean and covariance after UT, which are: [ ] 1; 2;3; 4;5;6 However, this kind of UT method has the problem that the radius which bounds the sphere of the sigma points is /2 2 n .Therefore, at even relatively low dimensions there are potential problems with numerical stability.

Spherical Simplex Unscented Transformation
The spherical simplex unscented transformation was developed with the goal of rearranging the sigma points of the simplex algorithm in order to obtain better numerical stability [12].The sigma points are lie in the hypersphere with radius of 2) Choose weight sequence: 3) Initial vector sequence: [ ] 4) Expand vector sequence for 2, j n =  , according to: According to the statements of Menegaz [15], the spherical simplex UT cannot capture the mean and covariance of the state after UT.Actually, the formulas in Table I of [15] were not consistent with Julier's [12], the readers can simply find it by making a comparison between the formulas in Fig. 2 here with Table I in the corresponding paper of [15].Take the same example of his paper, let the mean value before UT be ，covariance be 1 0 we can calculate the mean and covariance after UT are, Actually, the mean and the covariance are all capture the mean and the covariance of the state after UT.
From the above statements, we can say that the conclusion only the minimum set proposed by Menegaz [16] matching the mean and the covariance matrices (CM) of the system state is not very accurate.

Minimum Sigma Set Unscented Transformation
Minimum sigma set unscented transformation was proposed by [16] which used only n +1 sigma points (both simplex UT and spherical simplex UT use n +2 sigma points) to match the mean and the covariance matrices of the system state.
The flow of minimum sigma set unscented transformation is stated in Figure 3.
3) Calculate Sigma points: which is the weight that associates with i-th sigma point 3) Time update equations： Propagate directly through state transition function： The state of the filter is the form of error, so the mean and covariance can be predicted directly without using the spreading sigma points.This may save the computation time greatly.
is the covariance matrix of the state.
( ) The column vectors of χ are the sigma point vectors, k is the iteration time starts from 1.

5) Measurement update equations：
Propagate through observation function (13)： ( ) Calculate the covariance value of the measurements: Update the Cholesky factors via rank-one Cholesky updates: , which will be used in the next step 3.

Pseudo Satellite Measurements
GNSS signals are vulnerable to disruption by environmental and man-made interference, so they would not meet all of the emerging performance requirements of navigation.The navigation error may be large even for the GPS/INS coupled system when there is no enough observation information.One simple way to solve this problem is to add more constraints for the observation equations.A pseudo-GPS mechanism was proposed by [30] for the loosely-coupled GPS/INS system.
The pseudo-GPS method can afford artificial pseudoranges and pseudorange rates for calculating the GPS receiver position and velocity.The extra pseudo measurements can be added to enhance the position and velocity solution when there is no enough observed satellites.
The pseudo-GPS satellites can be taken together with the true satellites, viewed by the receiver.
For example, the radio navigation station can work as a pseudo satellite.The signal sent by radio navigation station will be received by the moving receiver, and then the pseudoranges and pseudorange rates are created.
Assumed i-th pseudo-satellite is fixed at in ECEF frame, then the pseudorange updated by model can be written as: The pseudorange rate updated by model can be written as: (15) Furtherly, in order to be used in the SRUKFs, the observation equations set by pseudo-satellite is described as:  (16) In ( 16), is the original pseudorange directly from the observation file of the rover which is the distance between the rover and the pseudo satellite; T is troposphere propagation errors, which usually can be ignored, since when the rover is not far away from the pseudo satellite station, it's error is quite small ; T  is troposphere error range rate of T , which is also very small in general, so they can be ignored;

Experiments of UAV
Simulations below are based on the post-processing of the UAV field test data.In the UAV experiment, the reference result was obtained from difference GPS and INS coupled system.
The initial conditions of the GPS/INS tightly-coupled system here are set as below.
The frequency of INS is 200Hz.The pure INS calculation is based on double-sample algorithm, in which coning and sculling errors are compensated.
GPS updating frequency is 2Hz.
The initial weights 0 w of M-SRUKF, S-SRUKF and SS-SRUKF are set as 0.5.
The initial error state covariance matrix 0 P is set as： (0.5 ) (0.5 ) (0.5 ) (0.1 / ) (0.1 / ) (0.1 / ) (10 ) (10 ) (10 ) (10 / ) (10 / ) (10 / ) ; (300 ) (300 ) (300 ) (25 The spectral density of measurement noise matrix is, The spectral density of process noise matrix is, The discretization form of q is Q , which obeys  1 and Table 2, respectively.Figure 6 show the estimated biases of gyros and accelerometers.The second scenario is that from 800s to 1000s there are only three visible satellites available; the mean and the standard deviation of the absolute position errors and the absolute attitude errors are shown in Table 3 and Table 4, respectively. z(m)  It can be seen from Table 1, in terms of the absolute position errors, all the SRUKFs (including the reduced sigma point SRUKFs) have only slightly higher accuracy than EKF in the first scenario when GPS observation information is enough.Also, the SRUKFs and EKF show the similar accuracy in terms of the attitude estimation which is shown in Table 2.
In the second scenario, however, when GPS signal is deficient, the SRUKFs performed distinct higher accuracy than EKF in terms of the mean and the standard deviation of both the absolute position and attitude errors, which can be seen from Table 3 and Table 4.
All the results from Table 1 to Table 4 show that the filters SRUKF, S-SRUKF, SS-SRUKF and M-SRUKF have nearly the same accuracy in the GPS/INS tightly coupled system, because they use nearly the same mechanization except that the number of sigma points is different.M-SRUKF uses only n+1sigma points, while both S-SRUKF and SS-SRUKF use n+2 sigma points, the standard SRUKF uses 2n+1 sigma points.From this point, M-SRUKF is the best filter here, since it has only nearly half computational cost of the standard SRUKF but still has higher accuracy than EKF.
The estimation of gyro and accelerometer constant biases of EKF and SRUKFs in Fig. 6 show that, all of them can estimate the constant biases successfully.But all the SRUKFs have faster convergence rate than EKF, which is to say that the latter has longer estimation time, which can be seen explicitly from the y direction gyro, x, y and z direction accelerometers.
Meanwhile, comparisons between Fig. 7 (a) and Fig. EKF takes more than 100s longer than M-SRUKF to recover to normal navigation and location (positon errors less than 10m), which means that the M-SRUKF is more effective than EKF.Similar comparisons can also be seen in a spacecraft relative navigation example [28], which also showed that SRUKF has faster convergence rate than EKF.
Simulations above indicates that M-SRUKF is a better mean for the GPS/INS tightly-coupled system, considering both from the accuracy and computation cost.

. Pseudo-satellite Aided M-SRUKF
Figure 7 (b) displays that there is still maximum error 85m for the M-SRUKF tightly-coupled system position error, which is a large error of normal navigation.In this section, a pseudo-satellite is introduced to improve the navigation accuracy of M-SRUKF GPS/INS tightly-coupled system when there are only three visible satellites from 800s-1000s.
Assume the pseudo satellite , ) ( , x y z is fixed at ECEF frame, which is about 30km away from the rover in y direction.In order to avoid the mutual signal interference, the minimum range between each pseudo satellite is 54km [31].The problem of designing the whole pseudo satellite network is not in the scope of this paper; readers can reference the paper [24]. The observation equations driven by pseudo-satellites are as (16) describes.In theory, more accurate results can be obtained with more pseudo-satellites, especially when there is no enough observation information.As well as that, the geometry between pseudo-satellites and the rover also affects the navigation results, which is beyond the scope of this paper.

Conclusions
In this paper, some questionable formulas and conceptual comments of previous UT methods, which use less than 2n sigma points, are revised.Real UAV data was used for the performance comparison among EKF, SRUKF and S-SRUKF, SS-SRUKF and M-SRUKF based GPS/INS tightly-coupled systems.Results showed that all the SRUKFs had only slightly higher accuracy than EKF when GPS observation was enough.However, SRUKFs showed distinct higher position accuracy than EKF when GPS signal was deficient.All the SRUKFs had nearly the same accuracy for the GPS/INS tightly-coupled system.But the reduced sigma point SRUKFs' computation cost were greatly saved, since they had only nearly half the computational cost of standard SRUKF.All the SRUKFs have faster convergence rates than EKF when GPS signals were recovered from partial outages to normal level.Meanwhile, SRUKFs have faster convergence rates in estimating the constant biases of gyros and accelerators than EKF.To trade off between accuracy and computation cost, M-SRUKF is a better mean than other filters for the GPS/INS tightly-coupled system.Pseudo-satellite aided mechanism was introduced to improve the M-SRUKF GPS/INS tightly-coupled system when GPS outage occurs.Simulations showed that this aiding mechanism worked well.

vCδ
are the error vectors of attitude and velocity, L δ , δλ , , h δ are errors of latitude, longitude and height, respectively.n in ω is the angular rate vector of the navigation frame relative to the inertial frame; is the direction cosine matrix; b ε and b ∇ are the constant biases of gyros and accelerometers, respectively; g w and a w are the white noise vectors of gyros and accelerometers, respectively.GPS receiver clock error and clock drift are modeled as is receiver clock error, ru c t δ is clock drift, tu w and tru w are white noises.

cr
is the position vector of i-th satellite in ECEF frame, i,sat c v is the velocity vector of i-th satellite in ECEF frame.sins r and sins v are position and velocity vectors of INS updated by navigation computer, respectively.c δ r and c δv are unknown errors, which will be obtained by real-time Kalman filter.

Fig. 1
Fig. 1 The revised simplex unscented transformation flowThe revised simplex unscented transformation satisfies the sum of weighs is unit one, and the UT captures the mean and the covariance of the state after UT.For example, let

Fig. 2
Fig. 2 The spherical simplex unscented transformation flow , which is lower triangular matrix.W is the weight matrix with only diagonal values.

Fig. 3 1 )
Fig. 3 The minimum sigma set unscented transformation flow In order to be used in the SRUKFs based GPS/INS tightly-coupled system, the above UTs have to be added into the corresponding Kalman filter flow.Here we only give the Kalman filter flow of the M-SRUKF used in the GPS/INS tightly-coupled system, which is shown in Fig. 4. Actually, the difference among the filters (SRUKF, S-SRUKF, SS-SRUKF, M-SRUKF) are just lie in the UT procedure.1) Initial values of state and the state covariance of the 17 states in (7)： { }

Fig. 4 M
Fig. 4 M-SRUKF flow used in the GPS/INS tightly-coupled system known as the Sagnac or Earth-rotation correction of pseudorange and pseudorange rate, usually can be ignored since the rover is not far away from the pseudo satellite station; ( ) r t δ is the clock error of the pseudo satellite;

w
is the measurement noise of pseudorange rate; 1,...n ε and 1,...n ζ are errors that are not explicitly modeled or measured.

TQ=
Fig. 5. Trajectory of the UAV 5.1 .Comparisons among EKF , SR-UKFs Comparisons are made among EKF, standard SRUKF and reduced sigma point SRUKFs GPS/INS tightly-coupled systems in two scenarios.In the first scenario, seven satellites are visible during the whole flight of UAV.The mean and the standard deviation of the absolute position errors and the absolute attitude errors are shown in Table1 and Table 2, respectively.Figure6show the

Fig. 7 (
Fig. 7 (a).EKF GPS/INS tightly-coupled position errors (there are GPS lost from 800 to 1000s) ignored since the rover is not far away from the pseudo satellite stations; Assume the clocks between the rover and the pseudo satellites are synchronized; Troposphere propagation errors, which are quite small, are ignored, since the rover is not far away from the pseudo satellite station; the beginning of this chapter.Positioning results are shown in Figure8.It shows that the M-SRUKF GPS/INS tightly-coupled system goes back to normal navigation level (with only maximum height error 7.2m) with the aiding of pseudo-satellites.

Table 1
Mean values and standard deviation values of the absolute position errors when there are seven visible satellites during the whole time (m)

Table 3
Mean values and standard deviation values of the absolute position errors when there are only three visible