SINGULAR VALUE DECOMPOSITION-BASED ROBUST CUBATURE KALMAN FILTER FOR AN INTEGRATED GPS / SINS NAVIGATION SYSTEM

A new nonlinear robust filter is proposed in this paper to deal with the outliers of an integrated GPS/SINS navigation system. The influence of different design parameters for H∞ cubature Kalman filter is analysed. It is found that when the design parameter is smaller, the robustness of the filter is stronger. However, the design parameter is easily out of step with the Riccati equation and the filter is easy to diverge. In this respect, the singular value decomposition algorithm is employed to replace Cholesky decomposition in the robust cubature Kalman filter. On the wider conditions for design parameter, the new filter is more robust. The testing results demonstrate that the proposed filter algorithm is more reliable and effective in dealing the data sets produced by the integrated GPS/SINS system. * Corresponding author.


INTRODUCTION
The integration of strap-down inertial navigation system (SINS) and the Global Positioning System (GPS) has been implemented for real-time navigation, mobile mapping, Location-based Services, transport and many other applications.The Kalman filtering (KF) is the most common technique for carrying out data fusing of GPS and SINS (Grejner-Brzezinska et al., 1998).However, the operation of the KF relies on the proper definition of dynamic and stochastic models and the standard KF can only be used to deal with linear model (Yi et al., 2006).Furthermore, due to the nonlinear characteristic of the low-cost SINS error model and the uncertainty of noise stochastic model, the KF estimation is not optimal and may produce an unreliable result, sometimes even leads to filtering divergence (Geng and Wang, 2008).Over the past few decades, nonlinear KF algorithms have been intensively investigated to deal with the nonlinear error model of low-cost SINS (Wendel et al., 2006, Gustafsson, 2010).Recently proposed cubature Kalman filtering (CKF), is a Gaussian approximation to Bayesian filtering, with more accurate filtering performance than traditional method and less computational cost (Arasaratnam and Haykin, 2009).CKF was introduced to deal with the data fusion of the integrated GPS/INS system (Sun and Tang, 2012).However, the standard CKF may still face difficulty in provision of stable results and cannot deal with the outlier data effectively.The robust cubature Kalman filtering (RCKF) based on a H ∞ filter was proposed for integrated GPS/INS navigation applications (Liu et al., 2010).The algorithm makes use of the H ∞ robust filter to overcome the interference of outliers.For the RCKF, the given parameter  is used to show the bound lever and decide the robustness for the uncertain interference of the H ∞ filter (Simon, 2010).The parameter  can be chosen appropriately according to the detailed performance index and there is a balance between system average accuracy and its robustness performance (Einicke and White, 1999).Certainly,  must be larger than a positive number to output a normal filtering result.The smaller the value of  the more strong the robustness of the filter is.However, a disproportionately small value can easily lead to a non-positive definite state covariance and cause filter divergence.Based on the error variance constraints or minimum variance principle, the modified robust filters were proposed (Hung andYang, 2003, Shaked andSouza, 1995).The nonlinear robust Kalman filtering problem with norm-bound parameter uncertainties also was studied by Xiong et al (Xiong et al, 2012).Furthermore, the optimal robust H ∞ estimator could be obtained by minimizing the H ∞ norm from uncertain disturbances to estimation errors (Shi et al., 2012).However, how to improve the performance of a robust filter under a small given parameter  was rarely investigated.
In this paper, the authors compare the performance of a robust cubature Kalman filter for the integrated GPS/SINS navigation applications under different given parameter  .In order to maintain a high level of numerical stability, a new filter algorithm is proposed.A singular value decomposition (SVD) algorithm is introduced to replace Cholesky decomposition in the RCKF.Land vehicle tests have been carried out to compare the performance of this algorithm with other cubature Kalman filter algorithms.The results show that the SVD based robust cubature Kalman filter (SVD-RCKF) algorithm can improve the filtering stability and has better robustness to the impact of outliers.The outline of this paper is as follows.Section 2 includes the nonlinear description of H ∞ filter and a description of the calculation steps of RCKF based on SVD is presented in Section 3. Section 4 lists the formulas of the system and observation equations of the GPS/SINS system.Two test results and data analysis are given in Section 5.The final part of the paper is the preliminary conclusions attained through this study.

Principle of an H ∞ filter
An H ∞ filter is a typical implementation of the robust filtering theory (Simon, 2010).It defines a cost function as follows: where and are the unrelated system noise and measurement noise, and and are their covariance matrices respectively.0 x is the system state vector and its covariance is 0 P , whilst 0 x is the estimated value of 0 x .In this equation, Other similar terms can be obtained recursively.
The goal of an H ∞ filter is to find an estimate k x that minimizes , under the condition J . Normally, it is difficult to get the analytical solution of an optimal H ∞ filter problem.Therefore, we need find a suboptimal iterative algorithm.We can set a threshold value  , which meets The threshold value is equivalent to the follow Riccati inequality (Chen, 2009): (2) For an H ∞ filter, the estimation error in the most unfavourable conditions is controlled by the threshold value  that is called designed restrict parameter.When the designed restrict parameter  is smaller, the robustness of filter is stronger.When  approaches infinity, the H ∞ filter is approaching to the standard Kalman filter.

An H ∞ cubature Kalman filter
In order to apply nonlinear filter to the H ∞ filter, the recursive Riccati equation for linear model is transformed to implement nonlinear filter.Due to et al., 2008), the formula for computing the state vector covariance matrix of the nonlinear H ∞ filter can be modified as follows: (3) For nonlinear models, we can calculate the mean and covariance of the state vectors by cubature point transformation instead of Taylor expansion, and then we can obtain the the nonlinear H ∞ robust filter based on cubature Kalman filter.

ROBUST CUBATURE KALMAN FILTER BASED ON SVD
Based on a cubature Kalman filter frame, we introduce a new filter algorithm by introducing an H ∞ robust filter.We can call it as a robust cubature Kalman filter (RCKF).However, after many times of iteration in RCKF, and / 1 k k P k P are very easy to lose their positive definiteness and this will consequently lead to the instability of the numerical value.Meanwhile a much smaller restrict parameter  may lead to non-positive definiteness of / 1 k k

P
and k P .Therefore singular value decomposition (SVD) (Gao et al., 2010) is applied in the calculation of the covariance matrix for RCKF instead of Cholesky decomposition in this paper.Considering the follow discrete nonlinear system 1 ( ) where and are the state and measurements of the dynamic system; h  are known nonlinear functions; and are the independent process and measurement Gaussian noise sequences with zero means and covariance and Then the procedure of the robust cubature Kalman filter based on singular value decomposition (SVD-RCKF) are expressed as follows: 1) Calculate the basic cubature sampling points and weights based on the cubature rule.
[1] 2 Where is the number of cubature points, and m x n is dimension of state vector.
represents the following set of points: is a diagonal matrix.Normally, the co-variance matrix  P is a symmetric one, so its eigenvalues are 2 ( 1,2, )  and  U V .Then the factorization formula can be described as follows: Evaluate the propagated cubature points , , ( Estimate the predicted state and error covariance Calculate the propagated cubature points , , / ( Calculate the predicted measurement, innovation covariance matrices and cross-covariance matrix Calculate the gain matrix, updated state and the corresponding covariance Considering the state covariance update formula of the H ∞ robust filter: Formulas ( 4)-( 16) and ( 18) constitute the calculation procedure of the robust cubature Kalman filter based on SVD (SVD-RCKF).

THE DYNAMIC AND OBSERVATION EQUATIONS OF A GPS/SINS SYSTEM
The loosely coupled GPS/SINS style is adopted.The state vectors are composed of the position and velocity error in frame, attitude error between computer frame and platform e e e  frame, gyros and accelerometers drift error in body frame, which can be expressed as (Petovello, 2003): The nonlinear error model for low-cost SINS is as follows: Where Generally, the measurement model can be expressed as Where This contribution has been peer-reviewed.doi:10.5194/isprsarchives-XL-3-W1-149-2014

TEST CASES AND DATA ANALYSIS
To demonstrate the performance of SVD-RCKF algorithm, data was collected under real world conditions with a probe vehicle.Two tests were performed.

Case 1
The first dataset was collected in China University of Mining and Technology (CUMT), China.The test had employed two GPS receivers and one low-cost IMU (SPAN-CPT).One of the GPS receivers was set on the rooftop as the reference station, and another one was placed on the top of the testing vehicle together with the IMU.The data was logged for post processing.The SPAN-CPT IMU consists of three-axis openloop fiber optic gyroscope and three-axis MEMS accelerometers.The technical data is shown in Table 1.The specified parameters were used in setting up the Q estimation in filtering process.Figure 1  The test results plotted in Figure2 are based on the cubature Kalman filter.It is worth noting here that the results contain some outlier values due to the vehicle driving over the speed bumps.These indicate that the robustness of CKF needed to be improved further.
Figure 3 shows the positioning error when using the robust cubature Kalman filter and the parameter  is set as 2.
Comparing Figures 3 and 2, it is apparent how effective the robust filter is.The error amplitude in Figure 3 is reduced by improving the robustness of the filter.The SVD based robust cubature Kalman filter (SVD-RCKF) shows the almost same result as the RCKF when the parameter  is set as 2. To keep the description concisely, the corresponding plot of SVD-RCKF is omitted.Table 2 shows the statistic information for the different filter algorithms.The performance of robust cubature Kalman filter with difference values of the parameter  was further analysed.
Table 3 presents the statistic information.For the RCKF, the larger value the parameter  is, the less the robustness of the filter.Compared with the result of RCKF on the value of 2500, the result on the value of 1.5 just improved the performance by 6.5%, 7.1% and 3.7% in three directions.This indicates that RCKF can get the jarless robustness performance if the different design parameters are within limits.From the fundamental of the H ∞ robust filter, we know that the smaller the restrict parameter  the stronger the robustness of the filter.To get more robust performance, the case studies with smaller  values were compared.The result is shown in Table 3.We can find that the RCKF cannot work well sometimes.The reason is that after many times of iteration in RCKF with a much smaller restrict parameter  , and / 1 k k P k P lose their positive definiteness and this consequently leads to the instability of the numerical value.In order to further improve the numerical stability of RCKF, the SVD-RCKF is proposed.Figure 4 gives the position errors when using the new filter algorithm with  set as 1.As expected, the results are better than the other algorithms previously mentioned.However, it was found that the relationship between  and robustness of a filter does not exist anymore if  is much smaller.That is because there is no solution of Riccati inequality if the  is much smaller.

Case 2
The second test was carried out in Nottingham, UK.The test setup was similar with the first one.A GNSS antenna, a GNSS receiver and a SPAN-LCI IMU were mounted in a van and data was logged from the receiver's serial ports to a laptop for storage and processing.The vector between the IMU centre and GPS antenna was accurately surveyed using a total station and is considered known to within 1cm.A base station was set up on the roof of the NGB building to provide DGPS and RTK corrections.The update rate of INS is 200Hz and the one of GPS is 1Hz.The average baseline length was less than 3 km for the test.Figure 5 is the test trajectory and Figure 6 is a photograph taken for the van.The high accuracy real-time output results of SPAN system are used as the reference value and the double difference code GPS position and velocity results are used as the input measurements.The position error of CKF is shown in Figure 7 and Figure 8 shows the position error when using the robust cubature kalman filter (RCKF) and the parameter  is set as 3.As we can see from Figure 7 and Figure 8, the error amplitude in Figure 8 is reduced by improving the robustness of the filter.As the same as case 1, the corresponding plot of SVD-RCKF is also omitted in case 2. Table 4 shows the statistic information of plots.
As in case 1, the performance of robust cubature Kalman filter with different values of the parameter  was also compared.
The result is very similar with case 1. Table 5 shows the corresponding statistic result.From Table 5, can know that the larger value the parameter  is, the filter can get the worse robustness.Compared with the result of RCKF on the value of 5000, the result on the value of 3 just improved the performance by 7.0%, 0.0% and 6.2% in three directions.Similar with case 1, this result demonstrates that RCKF can get the jarless robustness performance if the different design parameter is within limits.To get more robust performance, the case studies with smaller  were compared.The result of case 2 is shown in Table 5.
We can find that the RCKF cannot work when the parameter  is smaller than 2. The reason is that after many times of iteration in RCKF with a much smaller restrict parameter  , / 1 k k

P
and k P lose their positive definiteness and this consequently leads to the instability of the numerical value.As the same as case 1, the SVD-RCKF is introduced to further improve the numerical stability of RCKF. Figure 9 gives the position errors when using the new filter algorithm with  set as 1.44.As expected, the results are better than the other algorithms previously mentioned.However, the same problem is that the robust filter will diverge when the parameter  is set too small. .The position errors of different strict parameter in case 2

CONCLUSIONS
The robust cubature Kalman filter based on the H ∞ filter is very effective for detecting outlier data in GPS/SINS integration system.It has been found that the smaller restrict parameter  can improve the overall performance of RCKF.However, it is also apparent that RCKF is easy to diverge if the parameter is too smaller.The robust cubature Kalman filter based on SVD can maintain the system stability and robustness on the wider conditions for the design parameters.More work needs to carried out about how to set the optimal parameter.


accelerometers drift error in body frame.bThe dynamical model of a loosely coupled GPS/INS system can be expressed as follows , 1 ( )

Figure 3 .
Figure 2. The position error of CKF in case 1

Figure 8 .RMS
Figure 7.The position error of CKF in case 2

Figure 9 .
Figure 9.The position error of SVD-RCKF(  =1.414) in case 2 The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XL-3/W1, 2014EuroCOW 2014, the European Calibration and Orientation Workshop, 12-14 February 2014, Castelldefels, Spain ˆe INS R and ˆe INS V are computed position and velocity vectors by SINS in frame, e ˆe GPS R and are the ones output by GPS, and is the noise vector.ˆe GPS V k v shows the ground track of the test vehicle.The update rate of INS is 100Hz and the one of GPS is 1Hz.The high accuracy double difference carrier-phase GPS position results are used as reference value.

Table 3 .
The position errors of different strict parameter in case 1

Table 4 .
Position errors of different filters in case 2