An improved Kalman filter algorithm for tightly GNSS/INS integrated navigation system

: The Kalman filter based on singular value decomposition (SVD) can sufficiently reduce the accumulation of rounding errors and is widely used in various applications with numerical calculations. However, in order to improve the filtering performance and adaptability in a tightly GNSS/INS (Global Navigation Satellite System and Inertial Navigation System) integrated navigation system, we propose an improved robust method to satisfy the requirements. To solve the issue of large fluctuations in GNSS signals faced by the conventional method that uses a fixed noise covariance, the proposed method constructs a correction variable through the innovation and the new matrix which is obtained by performing SVD on the original matrix, dynamically correcting the noise covariance and has better robustness. In addition, the derived SVD form of the information filter (IF) extends its application. The proposed method has higher positioning accuracy and can be better applied to tightly coupled GNSS/INS navigation simulations and physical experiments. The experimental results show that, compared with the traditional Kalman algorithm based on SVD, the proposed algorithm’s maximum error is reduced by 45.77%. Compared with the traditional IF algorithm, the root mean squared error of the proposed IF algorithm in the form of SVD is also reduced by 4.7%.


Introduction
The global navigation satellite system (GNSS), as the primary outdoor positioning method, has always attracted attention from scholars.In recent years, autonomous vehicles and unmanned transport equipment, which are sensitive to positioning performance, have developed rapidly at home and abroad.They have higher navigation and positioning performance requirements, requiring more accurate positioning accuracy and more robust environmental adaptability.
With the development of differential positioning and precise single-point positioning [1][2][3], GNSS can already provide satisfactory positioning results.However, the problem is also apparent.For example, the receiver will fail to capture and output incorrect results or not output results when the satellite signal is blocked by buildings and vegetation in urban areas.As an independent closed-loop system, the inertial navigation system (INS) has the advantages of high autonomy and strong antiinterference ability.After initial alignment, it can provide users with more accurate positioning results for a while without external input.However, due to inertial devices' inherent characteristics, INS has an accumulation of errors, so the positioning accuracy decreases with time without external corrections.
The GNSS/INS integrated system is highly complementary to each other.GNSS can correct the INS to prevent the accumulation of INS positioning errors, and INS can compensate for the deficiency that GNSS cannot position under the obstruction.It dramatically improves the performance of the entire positioning system.In loosely coupled GNSS/INS navigation, the GNSS receiver and the INS system work independently of each other, and the respective results are fused through the fusion algorithm [4,5].There is a cascade between the filters, and the measurement noise is time-correlated.Therefore, the accuracy of the fusion result is limited [6].Compared with loosely coupled integration, tightly coupled GNSS/INS navigation has a higher degree of fusion.It uses satellite receiver pseudorange and pseudorange rate for fusion, which has higher positioning accuracy and anti-jamming performance [7][8][9].The deep coupled GNSS/INS has reached hardware-level integration.In addition to the correction of INS errors, it also involves assisting the code loop and carrier loop in the GNSS receiver's baseband signal processing for signal acquisition and tracking through INS information [10].Therefore, the entire system's dynamic performance and anti-interference ability are better than those of the above two combinations [11,12].
Information fusion is the primary method of information processing in GNSS/INS integrated navigation.It can be roughly divided into filtering and non-linear optimization methods.The most representative of the filtering algorithms is the Kalman filtering algorithm under the Gaussian assumption and its various extended forms.The nonlinear optimization method is global optimization based on factor graphs.Its processing methods include the classic sum-product algorithm [13] and the latest incremental smoothing algorithm isam, isam2 proposed by Kaess et al. [14,15].
For positioning algorithms used in vehicles, there are specific prior pieces of information as well.We can assume that under normal circumstances, a vehicle will not have lateral or vertical velocity.Furthermore, zero velocity is also crucial information.When the target is at zero velocity, various states should remain unchanged.This can effectively constrain sensors such as IMU (Inertial Measurement Unit) that have cumulative divergence.In GNSS/INS fusion systems, the most commonly used zero velocity detection algorithms are those based on IMU measurements.These methods include generalized likelihood ratio testing (GLRT) [16,17].Acceleration magnitude detection (MAG) [18] and angular rate magnitude detection (ARE) can also involve machine learning approaches or deep learning methods, such as support vector machines (SVM) [19,20] and neural networks [21][22][23].
To ensure calculation accuracy and prevent divergence, there are two main methods: (a) The factorization method, i.e., reducing rounding errors and enhancing the filter's numerical stability through factorization and (b) adaptive filtering algorithm, i.e., adaptively adjusting the covariance matrix and reducing the dependence on prior knowledge.
The Kalman filtering algorithm of factorization has two forms: the Cholesky decomposition form and the singular value decomposition form.The SVD method was first proposed by Wang et al. [24].The method decomposes the error covariance matrix and uses the decomposition factor to iterate.This method is also called the singular value decomposition-based square root Kalman filter (SVD-SRKF).Kulikova et al. [25] analyzed the shortcomings of the SVD-SRKF algorithm in a document published in 2017.The main shortcoming is that the error covariance matrix update formula is not used to reduce the accumulation of rounding errors.In addition, the noise matrix needs to be a Cholesky decomposition, which requires a rigorous matrix form.Kulikova et al. improved it and proposed the singular value decomposition-based Kalman filter (SVD-KF).
Mehra divided adaptive filtering methods into four categories: Bayesian, maximum likelihood, correlation and covariance matching [26].Among them, the covariance matching method is the most widely used.It uses the error between the current noise covariance and the theoretical covariance matrix to adaptively adjust the filtering process.In 1998, Mohamed et al. [27] proposed the innovationbased adaptive estimation Kalman filter (IAEAKF) algorithm, which is based on innovations and utilizes maximum likelihood estimation for correction.They derived the correction process and also presented an equivalent form based on residuals.By correcting the noise covariance using innovations or residuals dynamically and without simultaneously correcting state noise and observation noise together, this method mitigates the impact of initialization errors and exhibits stronger interference resistance [28][29][30].Bermudez et al. [31] used the standard deviation of an arithmetically differentiated small-signal segment to update the noise covariance matrix in SVD-based filter in 2020.It is also an adaptive method based on innovation.In 2017, Liu et al. [32] introduced a decay factor into IAEAKF for noise suppression in vehicle GNSS/INS integrated navigation systems.In 2019, Wei Sun proposed a new decay factor calculation method.In 2021, Pan et al. [33] added decay factors to the cubature Kalman filter (CKF) based on SVD to improve the robustness of the algorithm.The applications of decay factors enhance system stability and positioning accuracy and have practical value in certain scenarios.The Sage-Husa filter can be summarized as a Kalman filter based on covariance matching.It is constantly modified and adjusted in the iterative process to stabilize the filter.However, the main disadvantage is that the forgetting factor selection is difficult and the noise covariance is prone to be nonpositive definite before it becomes stable.The general solution is to adopt a suboptimal way to make corrections in the iterative process according to the actual situation [9,34].The method proposed in our paper belongs to the algorithm based on covariance matching.
The information filter (IF) algorithm is an equivalent form of the Kalman filtering algorithm, which has certain research value in some application scenarios.IF is concerned with the inverse matrix of the error covariance matrix rather than the matrix itself.Compared with the Kalman filter algorithm, the initialization of IF is more straightforward and the computational complexity of the iterative process is smaller.It is more appropriate to use IF for scenarios with sizeable initial error covariance [35].In addition, there has been no exploration of the SVD form of IF and its adaptive performance.Therefore, research on the IF algorithm's SVD form is also of great significance.
Figure 1 shows an overall block diagram of the tightly GNSS/INS system.The input consists of raw data from IMU, GNSS observations, and satellite positions and velocities.The IMU data undergo a low-pass filter (LPF) and are used as input for the inertial navigation system (INS) to predict the pseudorange and pseudorange rate.Additionally, a neural network is employed to classify the current motion state of the target.The fusion algorithm is Kalman filtering based on SVD.Since this algorithm is applied to ground vehicles, nonholonomic constraints (NHC) are applied to improve positioning accuracy.Zero velocity update (ZUPT) are also applied in stationary states.
In general, the filtering form based on SVD reduces the accumulation of rounding error and has good filtering performance.The main contributions of this paper are reflected as follows.The Sage-Husa filter method is used to improve the Kalman filter algorithm based on SVD.Then, the decomposed matrixes are used to derive the SVD form of the adaptive method and obtain a more robust filtering method.In response to the characteristics of commonly used filtering forms based on singular values, this paper employs various adaptive methods to enhance the stability of the filter.In addition, due to the performance improvement brought by SVD, this paper will also derive the SVD form of IF.This paper conducts comprehensive experiments to compare the impact of the proposed method and common methods on positioning accuracy.
The structure of the article is as follows.First, it introduces how to perform zero velocity detection using neural networks based on IMU raw data.Second, it introduces the GNSS and INS tightly coupled integrated navigation model and then proposes an improved algorithm.Then, the SVD form of the IF is derived.Finally, the performance of the algorithm is verified by simulation and physical experiments.

A zero-velocity detection based on a neural network
Using a neural network for zero velocity determination is a binary classification task.The input consists of raw data from IMU measurements, which include three-axis accelerometer readings and three-axis gyroscope readings.The output indicates the zero-velocity state, distinguishing between stationary and in-motion states.The details on the network can be found in reference [21].To emphasize the IMU features more effectively, a low-pass filter is applied to the data before feeding it into the network.This filtering aims to remove noise that could affect the network's classification.Here, a second-order digital low-pass filter is chosen, which allows for minimal delay while rapidly attenuating information outside the cutoff frequency.
In general, IMU data first go through the zero-velocity determination network.If the system is in a zero-velocity state at this moment, velocity and heading constraints are applied to maintain the current state.

Tightly coupled GNSS/INS navigation model
In the Kalman filter model of tightly coupled GNSS/INS integrated navigation, the state and measurement equation are: where ( 1) is the equation of state and the subscripts  and  − 1 represent moments  and  − 1 respectively.State parameter   = [, , , , ,   ]  ,  is the attitude error of the carrier coordinate system relative to the reference coordinate system. and  are the velocity error and position error, respectively. is the bias of the accelerometer,  is the bias of the gyroscope and   represents the clock offset and clock drift of the GNSS receiver.  is the state transition matrix, which is derived from the INS error equation.The continuous-to-discrete process adopts the first-order approximation.Eq (2) represents the measurement equation, and the measurement vector   is expressed as follows: where    and ̇  ( = 1,2 … ,  is the number of visible satellites) are the pseudorange and pseudorange rates of the i-th satellite, respectively. ̂ and İ are the pseudorange and pseudorange rates predicted by the INS results.  is the measurement matrix.  and   are the state noise vector and the measurement noise vector, respectively, and both are assumed to be Gaussian white noise with zero mean.Also, the covariances are represented by   and   [36].

Filter algorithm based on singular value decomposition
Section 4.1 first gives the algorithm procedure of SVD-SRKF and SVD-KF and then improves the above two algorithms, adds constraints and proposes a more robust form.Section 4.2 derives a new form of IF, the SVD form of IF.

Improved SVD-SRKF and SVD-KF
The SVD-SRKF algorithm procedure is given below, and the detailed derivation process can be found in reference [24].Initialization process: Iteration process: =   (7) where  is the estimated error covariance matrix.Eqs ( 9)-( 11) represent the error matrix prediction process, and Eqs. ( 12)-( 14) are the correction process.Subscript | − 1 is the filter estimated result, and | is the corrected result. 0|0 and  0|0 are the initial error covariance matrix and state vector, respectively. ̂ indicates the predicted value of the state vector.  and   are expressed as the SVD factors of the  matrix.  is the diagonal matrix. * ,   * and   * are the temporary factors generated during the SVD process, and  * is the diagonal matrix.  and   are the factors obtained after Cholesky decomposition of the inverse matrix of  and matrix . is the Kalman filter gain.
SVD-KF is an improvement of SVD-SRKF.It has some commonalities.The following only gives the key steps of the algorithm.For the specific process, please refer to reference [25].
The SVD-KF has the following improvements compared to SVD-SRKF: 1. Use singular value decomposition of  −1 and   instead of Cholesky decomposition in SVD-SRKF.2. The posterior estimation error covariance matrix  | is calculated by Eq (17).Compared with the calculation form in SVD-SRKF, it can guarantee symmetry and positive definiteness.
Werries et al. [29] pointed out that it is relatively easier to obtain accurate results for the observed noise covariance   in the navigation system.Based on this, assume that   is a constant matrix.To improve the robustness of the system, it is hoped to adjust   adaptively, so the above two algorithms will be improved throughout the rest of this section.
In order to obtain the form of correction, it is necessary to modify the classical forms.Mohamed et al. [27] proposed and derived the maximum likelihood estimation for the measurement noise covariance: To obtain the SVD form of the likelihood estimation, some transformations are required.Using the new matrix (,  and ) to replace  and  in the Eq (18): where  can be expressed as where   is shown in Eqs (15) and   is innovation.For the SVD-SRKF method, using  ̂ to correct the noise covariance: where and        are the Cholesky decomposition forms before and after correction.  = (1 − )/(1 −   ) ,  ∈ [0,1] is expressed as the forgetting factor.The general value is 0.95~0.99.diag( * ) means selecting the value on the diagonal of the matrix *.According to Eq (7) of the SVD-SRKF algorithm procedure, it involves Cholesky decomposition.Therefore, this paper uses a suboptimal method, which is not used  ̂ directly but only modifies the diagonal covariance data to ensure the feasibility of decomposition.If the  ̂ matrix is directly used, then it will result in non-positive definite.The corrected algorithm is called the singular value decomposition-based adaptive square root Kalman filter (SVD-ASRKF) in the rest of the article.
Similar to the SVD-ASRKF method, the following adaptive adjustments are made to   in the SVD-SRKF algorithm.
where  −1 and  ̂ have been mentioned above.Because SVD-KF directly performs SVD on   ,  ̂ is directly used for correction in Eq (22) and the suboptimal form of diagonal correction is not needed.The improved SVD-KF algorithm is called the singular value decomposition-based adaptive Kalman filter (SVD-AKF) in the rest of the article.Figure 2 shows the algorithm flowchart about the SVD-ASRKF and SVD-AKF.The main improved work is focused on the red box selection section and the black box represents the original algorithm flowchart (SVD-SRKF and SVD-KF).As shown above, our work focuses on dynamically correcting the covariance of measurement noise through new matrix ( ,  and  ) and innovation.In order to express the main flow of tightly GNSS/INS integrated navigation algorithm base on SVD-ASRKF, we summarize it in Algorithm 1.
(1 ) diag The algorithm flowchart about the comparison of SVD-ASRKF and SVD-AKF.The above is the improved algorithm of the SVD-SRKF and SVD-KF algorithms.In practical applications, to simplify the calculation, Eqs ( 21) and ( 22) are reanalyzed first.The main problems are the limitation of the number of iterations and the selection of the forgetting factor.For the first problem, when the number of iterations approaches infinity,   approaches 1 − .The effect of the innovation   decreases, and the current noise information is mainly determined by the past state.
The   in Eqs ( 21) and (22) do not need to be iterated all the time.There are two main reasons for this.First, we reduce the computational overhead.Second, it is affected by the limited word length.Therefore, the calculation can be stopped as long as   reaches an expected small range.Different forget factors might be used in various scenarios, so we use delta   as the criteria for assessment and the selected delta range is less than 5.0*10-4 in this paper.In addition, the forgetting factor can be selected by the following methods.First, choose a relatively large forgetting factor because the initial matrix  is generally accurate.Then, the forgetting factor can be adjusted according to certain criteria.The judgment criterion selects the magnitude of the change in the  square error.Calculate by the following formula: where  ̂ is the same as in (18) and  and  represent each element in the matrix.When error   is greater than the expected threshold, it indicates that the deviation of  is large, and the forgetting factor can be decreased to speed up the adjustment speed.The threshold can be selected three times the square error of the previous state, which is 3 −1 .
In general, the adaptive adjustment of the noise matrix makes the algorithm more robust.In addition, the constraints on Eqs ( 21) and ( 22) save computational overhead and are more suitable for engineering implementation.

IF based on singular value decomposition
IF is the equivalent form of the Kalman filter algorithm.Compared with the Kalman filter algorithm, it is more suitable to use IF in scenes with less initial information.Better filtering performance can be obtained by deducing the SVD form of IF.
The classical formula of IF is as follows [35]: where  −1 is the inverse matrix of error matrix which is traditionally called the information matrix, and Then, we analyze the prediction process of  −1 .Because Now, we want to solve the factors   |k-1 −1 and   |k-1 −1 from Eq (30), make an equivalent change to it and perform SVD on .
So, we can obtain that: Algorithm 3 The tightly coupled GNSS/INS navigation algorithm based on SVD-IF Input: 1:   −1 and   −1 matrixes from SVD of inverse error covariance matrix  0 −1 、initial state vector  0 、   −1 and   −1 matrixes from SVD of inverse noise covariance matrix  −1 、   and   matrix from SVD of noise covariance matrix  2: satellite position, pseudorange, pseudorange rate, IMU raw data Output: position, velocity and attitude 1: if new GNSS data comes then 2: using current state and satellite information to predict pseudorange and pseudorange rate and calculate residuals 3: update   −1 and   −1 matrixes from Eq (48) and(49) 4: correct state vector and obtain  ̂| 5: else 6: predict   −1 and   −1 matrixes using Eq (44) and (45) 7: using imu data to predict next state 8: end if 9: return position, velocity and attitude We summarize the GNSS/INS navigation algorithm based on SVD-IF in Algorithm 3. Through the SVD of the information matrix of IF, the accumulation of rounding error in the iterative calculation process can be alleviated in the case of limited word length, making the filtering algorithm more stable and the location result more accurate.

Experiment
To evaluate the performance of the method proposed in this article, this section first conducts a tightly coupled GNSS/INS integrated navigation simulation experiment.Then, it is applied to actual navigation scenarios.In the experiment, the variable's storage width is 32 bits, where the sign bit is 1 bit, the exponent is 8 bits and the mantissa is 23 bits.The methods that need to be simulated and compared are listed: In the tightly coupled GNSS/INS integrated navigation simulation, 50 Monte Carlo experiments were carried out.The GNSS data output frequency is 2 Hz, the IMU frequency is 100 Hz and the sampling time is 60 s.The simulation selects a low dynamic motion scene and the target velocity is 20 m/s.To increase the dynamic performance of the target and make the scene more general, two 90° turns are made at 15 s and 45 s.The random standard variances of the accelerometer and gyroscope are 3 mg and 1°/h, respectively.Other initial parameters are set as shown in Table 1

Results and discussion
Figure 3 shows the comparison results, selecting the local navigation frame (ENU).In the figure, the X-axis represents the sampling time in seconds and the Y-axis represents the position error in meters.The experimental results show that compared with the conventional Kalman filter, SVD-KF and SVD-SRKF both have better filtering performance.The SVD of the correlation matrix can reduce rounding errors and improve filtering performance.However, there is no significant difference between the filter performance of SVD-KF and SVD-SRKF in this simulation.4 is the simulation result of the improved filtering form proposed in this paper.The experimental results show that the method proposed in this paper, due to the real-time correction of the state noise covariance in the iterative process, has higher accuracy and better robustness than the above two methods.It also has a more stable performance in turning where the target's motion state changes quickly.

Experimental settings
To further prove the performance of the algorithm, a physical experiment is implemented.The GNSS receiver and INS use ProPak6 and IMU-FSAS, which are made by NovAtel.The GNSS receiver output frequency is 1 Hz, and the INS output frequency is 10 Hz.The accelerometer bias and scale factor of INS are 1 mg and 300 ppm, respectively.The gyro bias and scale factors are 0.75°/h and 400 ppm, respectively.The coordinate system uses ECEF, and the reference position adopts the position that has been accurately calibrated.In the static experiment, the ZUPT will be closed.
In addition to static experiments, dynamic experiments were also conducted.In this experiment, a GNSS reference station was set up at a fixed location.The trajectory obtained through real-time kinematic differential positioning is used as the reference trajectory for vehicle movement.The precise coordinates of the GNSS reference station are accurately known.The evaluation metrics are maximum and root mean squared error (RMSE).

Results and discussion
The static experiment results are as follows.Figure 6 compares the positioning error between the conventional Kalman filter and the SVD form filter.In this figure, the X-axis represents the sampling time in seconds, and the Y-axis represents the position error in the ECEF.Through the experimental results, it can be concluded that the Kalman filter based on SVD has better performance in position accuracy.The positioning results about the improved filtering form are shown in Figure 7. Table 2 shows the detailed positioning results.The RMSE of each method on each coordinate axis is listed.The experimental results show that compared with SVD-SRKF and SVD-KF, the improved filtering forms of SVD-ASRKF and SVD-AKF have smaller positioning errors on each coordinate axis.The Z-axis positioning error decreased the most, with a maximum reduction of 25.7% and a minimum reduction of 6.8%.There is also a 3.3% reduction in the X-axis error.Figure 8 shows the performance comparison of IF and SVD-IF under the above experimental conditions.The experimental results show that SVD-IF has higher accuracy and robustness than IF.The specific error results are listed in Table 2.The RMSE in the X direction decreases by 4.7%.The dynamic experiment results are as follows.Zero velocity detection on the target vehicle is performed using a neural network and the results are shown in Figure 9.In the figure, the detection result curve has only two values, "0" and "2".A value of "0" indicates the detection of zero velocity, while a value of "2" indicates that the target is in motion.Due to the presence of high-frequency environmental noise and vibration noise from the vehicle when it is stationary, a low-pass filter is applied before the detector to enhance the detection rate.The experimental results demonstrate that the detection rate reaches 94.73%.Analyzing the experimental results, it can be observed that when the target transitions from an open sky to a scene in which the GNSS signal is obstructed, the number of visible satellites decreases.In this case, if some of the visible satellites involved in the positioning solution exhibit significant observation errors, it can lead to fluctuations in the final positioning solution.The red rectangular box highlighted in Figures 10 and 11 represents the scenario described above, and the change in the number of satellites is shown in rectangular box in Figure 12.In Figure 10, the proposed methods have smaller fluctuation.In addition, due to the lack of spatial error compensation for ionosphere and troposphere delay, the trajectories do not align very well.However, absolute positioning accuracy is not the primary focus of the algorithm and does not affect performance comparisons.In Figure 11, it can be observed that the proposed method, compared to the Kalman filtering algorithms based on classical SVD methods, exhibits superior positioning performance in such complex environments.As errors in GNSS observations, the method proposed in this paper adaptively increases the covariance of the measurements.This, in turn, places greater trust in the pure IMU derived results, thereby reducing the impact of erroneous GNSS observations.

Figure 1 .
Figure 1.Block diagram of the tight GNSS/INS system.

Figure 3 .
Figure 3.Comparison of simulation results between the conventional KF and SVD algorithms.

Figure
Figure4is the simulation result of the improved filtering form proposed in this paper.The experimental results show that the method proposed in this paper, due to the real-time correction of the state noise covariance in the iterative process, has higher accuracy and better robustness than the above two methods.It also has a more stable performance in turning where the target's motion state changes quickly.

Figure 4 .
Figure 4. Comparison of simulation results for the improved filtering algorithm.

Figure 5 .
Figure 5. Simulation results of the SVD-IF approach.

Figure 6 .
Figure 6.Comparison of the experimental results between the conventional KF and SVD algorithms.

Figure 7 .
Figure 7.Comparison of the experimental results of the improved filtering algorithm.

Figure 8 .
Figure 8. Experimental results of the SVD-IF approach.

Figure 9 .
Figure 9.The results of the zero-velocity detection.

Figure 10 .
Figure 10.Comparison of the ground-truth trajectory and the estimated trajectory.

Figure 11 .
Figure 11.Comparison of the experimental results of the improved filtering algorithm.
and   matrixes from SVD of error covariance matrix  0 , initial state vector  0 ,   and   matrixes from Cholesky decomposition of inverse noise covariance matrix −1and noise matrix  2: satellite position, pseudorange, pseudorange rate, IMU raw data The main flow of the tightly coupled navigation algorithm based on SVD-AKF is shown in Algorithm 2. The main different steps compared to Algorithm 1 are steps 6-10.The tightly coupled GNSS/INS navigation algorithm based on SVD-AKF Input: 1:  and   matrixes from SVD of error covariance matrix  0 , initial state vector  0 ,   and   matrixes from SVD of noise covariance matrix  ,   and   matrixes from SVD of noise covariance matrix  2: satellite position, pseudorange, pseudorange rate, IMU raw data Output: position, velocity and attitude 1: if new GNSS data comes then 2: using current state and satellite information to predict pseudorange and pseudorange rate and calculate residuals from Eq (3) 3: obtain the Kalman gain using the SVD matrix 4: correct state vector and obtain  ̂| 5: calculate correction about measurement noise covariance by innovation   ,   and   matrix 6: update   and   matrix using Eq (22) 7: update   and   matrix 8: else 9: using imu data to predict next state 10: predict   and   matrix 11: end if 12: return position, velocity and attitude (11)rithm 1The tightly coupled GNSS/INS navigation algorithm based on SVD-ASRKF Input: 1: 9: using imu data to predict next state 10: predict   and   matrix using Eq(10)and(11)11: end if 12: return position, velocity and attitude

Table 2 .
Position RMSE comparison of different approaches in 32-bit.