Robust Kalman Filtering Based on Chi-square Increment and Its Application

Abstract

In Global Navigation Satellite System (GNSS) positioning, gross errors seriously limit the validity of Kalman filtering and make the final positioning solutions untrustworthy. Thus, the detection and correction of gross errors have become indispensable parts of Kalman filtering. Starting by defining an incremental Chi-square method of recursive least squares, this paper extends this definition to Kalman filtering to detect gross errors, explains its nature and its relation with the currently adopted Chi-square variables of Kalman filtering in model and data spaces, and compares them with the predictive residual statistics. Two robust Kalman filtering models based on an incremental Chi-square method (CI-RKF) were established, and the one with a better incremental Chi-square component was selected based on a static accuracy evaluation experiment. We applied the selected robust model to the GNSS positioning and the GNSS/inertial measurement unit (IMU)/visual odometry (VO) integrated navigation experiment in an occluded urban area at the East China Normal University. We compared the results for conventional Kalman filtering (CKF) with a robust Kalman filtering constructed using predictive residual statistics and an Institute of Geodesy and Geophysics (IGGШ) weight factor, abbreviated as “PRS-IGG-RKF”. The results show that the overall accuracy of CI-RKF in GNSS positioning was improved by 22.68%, 54.33%, and 72.45% in the static experiment, and 12.30%, 7.50%, and 16.15% in the kinematic experiment. The integrated navigation results indicate that the CI-RKF fusion method increased the system positioning accuracy by 66.73%, 59.59%, and 59.62% in one of the severe occlusion areas, and 42.04%, 59.04%, and 52.41% in the other.

1. Introduction

Kalman filtering is a recursive algorithm widely used in positioning and navigation. It dynamically updates the state variables of a system and recursively estimates the state variables by assigning proper weights to the observations and state predictions. The weights of conventional Kalman filtering are defined based on the observation signal-to-noise ratio and satellite elevation angles [1]. When the real observations do not satisfy the mathematical model or the statistical properties of the measurement noise and the dynamic state process (in particular, when signals undergo severe interference from multipath, no-line-of-sight (NLOS) and gross errors), such a recursive process of conventional Kalman filtering not only generates a poor solution for this epoch, but also propagates poor quality solutions for subsequent epochs. To remedy such a vulnerability, scholars have proposed various methods to mitigate the impact of these abnormal situations, such as function model compensation [2], adaptive Kalman filtering [3], and robust Kalman filtering [4]. Both the function model compensation and adaptive Kalman filtering methods primarily examine the reasonableness of the underlying mathematical model and noise statistics. When a violation is detected, these methods revise the original inaccurate dynamic model and process noise statistics to improve the performance of the recursive filtering. These methods usually consume extra computational resources to detect abnormal observations and gross errors, and hence are not effective or conducive for civil real-time positioning in cases where the impact of gross errors is overwhelming. On the other hand, robust Kalman filtering is very effective for gross error detection. This paper focuses on the detection and elimination of abnormal observations and gross errors of Kalman filtering, in order to ensure its stability and reliability. Hence, for the rest of the paper our attention is confined to robust Kalman filtering.

Current robust algorithms are mainly composed of two parts: gross error detection and robust estimation. The main idea of these robust Kalman filtering algorithms is to use learning statistics for judging errors [5] to detect gross errors in observations, then eliminate them or assign proper reduced weights to minimize their contamination [6]. Commonly used algorithms such as the Huber scheme [7], Institute of Geodesy and Geophysics (IGGIII) scheme [8], two-factor equivalent weights [9], robust Bayesian estimation [10], Danish function [11], and so on all rely on the learning statistics from the discrepancy between the predictive residuals and posterior residuals of observations [12]. A commonly adopted approach is to construct a Chi-square variable with a Chi-square test to detect gross errors and monitor system failure. One type of Chi-square variable is based on the departure between the propagated state vector and the state vector estimated from Kalman filtering; its corresponding statistical test is called the state Chi-square test (SCST) [13,14]. Another type of Chi-square variable is based on the residuals between observations and predicted values from the estimated state; its corresponding statistic test is called the innovation Chi-square test (ICST) [15]. In ordinary least squares (LS), the Chi-square variable is defined based on the Sum of Squares of Errors (SSE) [16]. In particular, the increment of Chi-square ($\Delta {\chi }^2$) between two adjacent epochs is sensitive to abnormal observations and gross errors at this epoch [17]. The concept of the increment of $\Delta {\chi }^2$ also can be extended to Kalman filtering [18]. However, the connection between the $\Delta {\chi }^2$ extending from the recursive LS (RLS) and Chi-square variables of Kalman filtering that are defined directly in the model and data spaces has not yet been discussed. In this paper, we reveal that the $\Delta {\chi }^2$ extending from the RLS is equivalent to the ICST and, hence, that its contributors are from both model and data spaces. We then deploy the $\Delta {\chi }^2$-based robust Kalman filtering in real observation scenarios by defining the corresponding robust weight function.

The emerging integrated positioning of multi-constellation satellite navigation systems enlarges spatio-temporal coverage while enhancing positioning accuracy and stability. Meanwhile, it increases the probability of observation anomalies and blunders [19]. In occluded urban areas, the ratio of abnormal gross errors becomes much higher due to complex multipath environments and limited sky visibility [20]. Thus, the gross error detection and on-the-spot contamination reduction appear critical for real-time Global Navigation Satellite System (GNSS) positioning, an inertial measurement unit (IMU), and a visual odometry (VO)-assisted GNSS (GNSS/IMU/VO) integrated navigation system [21]. We tested our $\Delta {\chi }^2$-based robust Kalman filtering scheme in real-time static and kinematic experiments of both GNSS positioning and GNSS/IMU/VO integrated navigation, analyzed its positioning accuracy improvement, and compared its performance with the robust Kalman filtering based on predictive residual statistics. The observations were obtained from instrumental measurements, and their contents were related to their practical applications, such as the observation results from various sensors [22]. In this paper, the observations represent the pseudo-range measurements from the receiver in GNSS positioning, and refer to the positioning parameters obtained by GPS, IMU, and VO in integrated navigation. The validity of this model verifies that $\Delta {\chi }^2$-based robust Kalman filtering does improve the accuracy and stability of real-time kinematic positioning, and has an advantage over the other schemes of Kalman filtering that are compared in the paper.

2. Methodology

2.1. Gross Error Detection

The SSE is the sum of squared deviations of observed values from predicted values. It stems from the regression analysis and is used to measure discrepancies between observed data and an estimation model. It is also an important statistic in LS positioning [16], which is equal (in numerals) to the weighted sum of the squares of errors (WSSE) [23]. Since the LS positioning solution minimizes the WSSE components [24], the magnitude of the SSE of the LS reflects the degree of consistency between the observed values, and can be used to evaluate the fitting goodness of the estimated parameters to the actual observations.

Differently from the ordinary LS, where the model prediction uses full observations, the recursive LS (RLS) builds its model based on a subset of observations (called as “past history”). It has been shown that such derived recursive residuals are very sensitive to such a point that the model is not supported by real data [25]. Later, it was suggested that the recursive residuals were more sensitive to data outliers than the residuals from the ordinary LS [26]. Scientists have proposed the routine calculation of recursive residuals in an RLS analysis to diagnose both model and data outliers [27].

The observation equation at epoch k is expressed in forms of vector and matrix:

$$z_k=h_k{x}_k+{\nu }_k$$

(1)

where $z_k,h_k,{\nu }_k$ are the observation vector, design matrix, and noise vector measurements, respectively, and their subscript k represents the kth epoch; the observation error covariance matrix at this epoch is $r_k$; $x_k$ denotes the state vector at epoch k. In our positioning applications, we assume that observations and the error covariance matrix at each epoch are independent, and that the design matrix has a full ranking. After this is established, the solution and its covariance matrix can be expressed in a recursive form [28]:

$$\begin{array}{l}{\hat{x}}_k={\hat{x}}_{k-1}+C_{{\hat{x}}_{k-1}}{h}_k^T{(h_k{C}_{{\hat{x}}_{k-1}}{h}_k^T+r_k)}^{-1}(z_k-h_k{\hat{x}}_{k-1})\\ C_{{\hat{x}}_k}=C_{{\hat{x}}_{k-1}}-C_{{\hat{x}}_{k-1}}{h}_k^T{(h_k{C}_{{\hat{x}}_{k-1}}{h}_k^T+r_k)}^{-1}{h}_k{C}_{{\hat{x}}_{k-1}}\end{array}$$

(2)

where the symbol ^ is the estimated solution vector and the superscript T denotes matrix transposal. The estimated solution covariance matrix at epoch k is represented by C.

The SSE (Chi-square) of such a recursive LS from epoch 1 to epoch k is defined as

$${\chi }_k^2={\displaystyle \sum _{i=1}^k(z_i-h_i{\hat{x}}_k)}{r}_i^{-1}(z_i-h_i{\hat{x}}_k)$$

(3)

The increment of Chi-square at epoch k is defined as

$$\Delta {\chi }_k^2={\chi }_k^2-{\chi }_{k-1}^2$$

(4)

It was proven that $\Delta {\chi }_k^2$ can also be expressed in a recursive form [18]:

$$\begin{array}{c}\Delta {\chi }_k^2=\Delta {\hat{x}}_k^T{C}_{{\hat{x}}_{k-1}}^{-1}\Delta {\hat{x}}_k+(z_k-h_k{\hat{x}}_k{)}^T{r}_k^{-1}(z_k-h_k{\hat{x}}_k)\\ \Delta {\hat{x}}_k={\hat{x}}_k-{\hat{x}}_{k-1}\end{array}$$

(5)

This concept of $\Delta {\chi }_k^2$ from RLS can be extended to Kalman filtering. Suppose that the state equation and observation equation of Kalman filtering at epoch k are [29,30]

$$x_k=A_k{x}_{k-1}+B_k{u}_{k-1}+{\Gamma }_k{\omega }_k$$

(6)

$$z_k=h_k{x}_k+v_k$$

(7)

where subscript k still represents the epoch index; $x_k$ is the state vector; $u_k$ is the control function; $z_k$ and $h_k$ represent the observation vector and design matrix; $A_k,B_k$ are the transition coefficient matrix of the state and the control function transition; ${\Gamma }_k$ is the input matrix of the state disturbance noise; ${\omega }_k~N(0,q)$ is the state disturbance noise; $v_k~N(0,r)$ is the observation noise. Suppose that ${\omega }_k,v_k$ are independent of each other and obey a normal distribution of white noise.

The state transition and update equations are as follows:

$$\begin{array}{l}{\hat{x}}_{k,k-1}=A_k{\hat{x}}_{k-1}+B_k{u}_{k-1}\\ C_{{\hat{x}}_{k,k-1}}=A_k{C}_{{\hat{x}}_{k-1}}{A}_k^T+{\Gamma }_k{q}_k{\Gamma }_k^T\\ K_k=C_{{\hat{x}}_{k,k-1}}{h}_k^T(h_k{C}_{{\hat{x}}_{k,k-1}}{h}_k^T+r_k{)}^{-1}\\ {\hat{x}}_k={\hat{x}}_{k,k-1}+K_k(z_k-h_k{\hat{x}}_{k,k-1})\\ C_{{\widehat{x}}_k}=(I-K_k{h}_k)C_{{\widehat{x}}_{k,k-1}}\end{array}$$

(8)

where ${\hat{x}}_k$ represents the estimated state variable at epoch k and ${\hat{x}}_{k,k-1}$ denotes the predicted state vector at epoch k from epoch k − 1; I is the identity matrix and $K_k$ is the Kalman gain matrix at epoch k. According to the $\Delta {\chi }_k^2$ of the recursive LS, the $\Delta {\chi }_k^2$ of the k epoch in Kalman filtering is defined as [31]

$$\Delta {\chi }_k^2=\Delta {\hat{x}}_k^T{C}_{{\hat{x}}_{k,k-1}}^{-1}\Delta {\hat{x}}_k+(z_k-h_k{\hat{x}}_k{)}^T{r}_k^{-1}(z_k-h_k{\hat{x}}_k)$$

(9)

where $\Delta {\hat{x}}_k={\hat{x}}_k-{\hat{x}}_{k,k-1}$. It can be seen from the above equation that the $\Delta {\chi }_k^2$ of the k epoch is composed of two parts, with the first part being the increment caused by the change of fitting solution, and the second part being the increment caused by the observation residual of the k epoch.

The $\Delta {\chi }_k^2$ derived from Equation (9) acts as an indicator of gross error detection. Although it demonstrates its nature clearly, it is inconvenient in practical use because it requires an updated state variable ${\hat{x}}_k$. Once a gross error occurs, it indicates that the solution and its covariance matrix of the Kalman filtering have already been distorted and can no longer be used recursively to get the correct solution of the next epoch [32]. In order to obtain a normal solution, the solution and its covariance matrix of the previous epoch must first be saved. If no gross error is found, the solution and covariance matrix will be updated. Otherwise, it is necessary to remove the gross error of the k epoch and restore the previously saved uncontaminated solution and its covariance matrix, then continue the operation. Such a procedure not only occupies more computational memory, but also reduces the operation speed greatly, which is not conducive to real-time positioning. To obtain an estimated value of $\Delta {\chi }_k^2$ before updating the solution, we express the $\Delta {\chi }_k^2$ and the observation residuals at k epoch by the form of their propagated values from the k − 1 epoch:

$$\Delta {\hat{x}}_k=C_{{\hat{x}}_{k,k-1}}{h}_k^T{(h_k{C}_{{\hat{x}}_{k,k-1}}{h}_k^T+r_k)}^{-1}(z_k-h_k{\hat{x}}_{k,k-1})$$

(10)

$$z_k-h_k{\hat{x}}_k=r_k{(h_k{C}_{{\hat{x}}_{k,k-1}}{h}_k^T+r_k)}^{-1}(z_k-h_k{\hat{x}}_{k,k-1})$$

(11)

Bring Equations (10) and (11) into Equation (9):

$$\Delta {\chi }_k^2={(z_k-h_k{\widehat{x}}_{k,k-1})}^T{(h_k{C}_{{\widehat{x}}_{k,k-1}}{h}_k^T+r_k)}^{-1}(z_k-h_k{\widehat{x}}_{k,k-1})$$

(12)

Equation (12) is equivalent to Equation (9), but Equation (9) uses the updated ${\hat{x}}_k$, while Equation (12) uses the propagated ${\hat{x}}_{k,k-1}$ and $C_{{\hat{x}}_{k,k-1}}$. Therefore, Formula (12) can judge whether the epoch is normal or not through the prediction stage only. If the raw observation causes the $\Delta {\chi }_k^2$ anomaly, it will be regarded as a gross error, which can be further processed by eliminating data or reducing the Kalman filtering weight to get the normal positioning results.

For comparison, the SCST-adopted Chi-square variable is defined as [14]

$$\Delta {\chi }_k^2=\Delta {\hat{x}}_k^T{(C_{{\hat{x}}_{k,k-1}}-C_{{\hat{x}}_k})}^{-1}\Delta {\hat{x}}_k$$

(13)

which is different from our Equation (9) and is confined to state variables only in a parameter space. Meanwhile the ICST-adopted Chi-square variable is defined as [15]

$$\Delta {\chi }_k^2={(z_k-h_k{\hat{x}}_{k,k-1})}^T{(h_k{C}_{{\hat{x}}_{k,k-1}}{h}_k^T+r_k)}^{-1}(z_k-h_k{\hat{x}}_{k,k-1})$$

(14)

which is exactly the same as Equation (12). Thus, the nature of the ICST-adopted Chi-square variable is equivalent to the extension of the RLS-defined $\Delta {\chi }_k^2$, which actually covers the variance caused by the new data in both the data and model spaces according to Equation (9). The matrix $h_k{C}_{{\hat{x}}_{k,k-1}}{h}_k^T+r_k$ is called the covariance matrix of innovation, and is introduced by the new observation.

The learning statistics for judging errors, such as predictive residual statistics, are express as [33,34,35]

$$\Delta {\hat{V}}_k={(\frac{{\hat{v}}_k^T{\hat{v}}_k}{tr(C_{{\hat{v}}_k})})}^{\frac{1}{2}}$$

(15)

where ${\hat{v}}_k=h_k{\hat{x}}_{k,k-1}-z_k$ is the predictive residual vector, and $C_{{\hat{v}}_k}=h_k{C}_{{}_{{\hat{x}}_{k,k-1}}}{h}_k^T+r_k$ is the covariance matrix of innovation. Equation (14) can be converted to

$$\Delta {\hat{\chi }}_k^2={\hat{v}}_k^T{C}_{{\hat{\nu }}_k}^{-1}{\hat{v}}_k$$

(16)

A comparison shows that Equation (15) is a simplified version of Equation (16), which uses a trace of the covariance matrix of innovation to replace the full covariance matrix. Hence, Equation (16) is more rigorous, as all correlations among the predictive residuals are considered. Therefore, compared with learning statistics, $\Delta {\chi }^2$ reflects the impact of gross errors more accurately.

2.2. Robust Kalman Filtering Based on Chi-Square Increment

Ideally, $z_k-h_k{\hat{x}}_{k,k-1}$ should obey the normal distribution with a zero mean value [36]:

$$z_k-h_k{\hat{x}}_{k,k-1}~N(0,h_k{C}_{{\hat{x}}_{k,k-1}}{h}_k^T+r_k)$$

(17)

When a gross error occurs, the predictive residual no longer obeys the above normal distribution. The Chi-square test for $\Delta {\chi }_k^2$ provides useful statistics to identify gross errors in observation.

Method 1 is used to test the components of the vector $z_k-h_k{\widehat{x}}_{k,k-1}$ totally:

$${(z_k-h_k{\hat{x}}_{k,k-1})}^T{(h_k{C}_{{\hat{x}}_{k,k-1}}{h}_k^T+r_k)}^{-1}(z_k-h_k{\hat{x}}_{k,k-1})~{\chi }^2(n)$$

(18)

Set the following hypothesis test:

$$\{\begin{array}{c}{H}_0:\Delta {\chi }_k^2~{\chi }^2(n,0)\\ H_1:\Delta {\chi }_k^2~{\chi }^2(n,\lambda )\end{array}$$

(19)

where H₀ denotes that there is no gross error in the system and H₁ denotes that there is a gross error in the system; n is the degree of freedom of Chi-square distribution; and $\lambda$ is a centralized parameter matrix.

For observations with gross errors, robust estimation is usually achieved by enlarging the observation covariance noise matrix [37,38,39]. According to the above hypothesis, the robust factors are constructed with $\Delta {\chi }_k^2$:

$${\beta }_k=\{\begin{array}{cc}1& \left|\frac{\Delta {\chi }_k^2}{{\chi }_{\alpha }^2(n)}\right|<c_0\\ \left|\frac{\Delta {\chi }_k^2}{{\chi }_{\alpha }^2(n)}\right|& c_0<\left|\frac{\Delta {\chi }_k^2}{{\chi }_{\alpha }^2(n)}\right|<c_1\\ {\left|\frac{\Delta {\chi }_k^2}{{\chi }_{\alpha }^2(n)}\right|}^2& \left|\frac{\Delta {\chi }_k^2}{{\chi }_{\alpha }^2(n)}\right|>c_1\end{array}$$

(20)

where α is the confidence level, and $c_0,c_1$ are two pre-defined constants. Then, the observation covariance matrix is updated to ${\overline{r}}_k={\beta }_k{r}_k$.

Method 2 is used to test the components of the vector $z_k-h_k{\hat{x}}_{k,k-1}$ individually.

From Equation (18), it can be deduced that an individual $\Delta {\chi }_{k,i}^2$ should obey the Chi-square distribution with a degree of freedom equal to 1:

$${(z_k-h_k{\hat{x}}_{k,k-1})}_{{}^i}^T{(h_k{C}_{{\hat{x}}_{k,k-1}}{h}_k^T+r_k)}_{i,i}^{-1}{(z_k-h_k{\hat{x}}_{k,k-1})}_i~{\chi }^2(1)$$

(21)

where the subscript i denotes the i-th component. Then, the hypothesis test is as follows:

$$\{\begin{array}{c}{H}_0:\Delta {\chi }_{k,i}^2~{\chi }^2(1,0)\\ H_1:\Delta {\chi }_{k,i}^2~{\chi }^2(1,\lambda )\end{array}$$

(22)

Similarly, the robust factors can be constructed as

$${\beta }_i=\{\begin{array}{cc}1& \left|\frac{\Delta {\chi }_{k,i}^2}{{\chi }_{\alpha }^2(1)}\right|<c_0\\ \left|\frac{\Delta {\chi }_{k,i}^2}{{\chi }_{\alpha }^2(1)}\right|& c_0<\left|\frac{\Delta {\chi }_{k,i}^2}{{\chi }_{\alpha }^2(1)}\right|<c_1\\ {\left|\frac{\Delta {\chi }_{k,i}^2}{{\chi }_{\alpha }^2(1)}\right|}^2& \left|\frac{\Delta {\chi }_{k,i}^2}{{\chi }_{\alpha }^2(1)}\right|>c_1\end{array}$$

(23)

The observation covariance matrix is updated to ${\overline{r}}_{k,i}={\beta }_i{r}_{k,i},{\overline{r}}_k=\left[\begin{array}{ccccc}{\overline{r}}_{k,1}& \dots & {\overline{r}}_{k,i}& \dots & {\overline{r}}_{k,m}\end{array}\right]$.

The updating process of robust Kalman filtering is

$$\begin{array}{l}{\overline{r}}_k=C_{{\hat{x}}_{k,k-1}}{h}_k^T(h_k{C}_{{}_{{\hat{x}}_{k,k-1}}}{h}_k^T+{\overline{r}}_k{)}^{-1}\\ {\hat{x}}_k={\hat{x}}_{k,k-1}+{\overline{K}}_k(z_k-h_k{\hat{x}}_{k,k-1})\\ C_{{\widehat{x}}_x}=(I-{\overline{K}}_K{h}_k)C_{{\widehat{x}}_{k,k-1}}\end{array}$$

(24)

2.3. Mathematical Model of CI-RKF in GNSS Positioning

The observation equation of the pseudo-range [39] is

$${\rho }_k^i=r_k^i+\delta {\tau }_k-\delta t_k^i+I_k^i+T_k^i+{\epsilon }_{\rho }^i$$

(25)

where subscript k still denotes the epoch and superscript i is the observation index or satellite index at each epoch; $\rho$ is the pseudo-range measurement; $r$ is the geometric distance between the satellite $i$ and the receiver; $\delta {\tau }_k$ is the receiver error; I is the ionosphere delay; T is the atmosphere delay; and ${\epsilon }_{\rho }$ is the pseudo-range measurement noise. In order to establish the pseudo-range positioning equation, Equation (25) can be simplified as the following:

$$r_k^i+\delta {\tau }_k={\overline{\rho }}_k^i-{\epsilon }_{\rho }^i$$

(26)

where ${\overline{\rho }}_k^i={\rho }_k^i+\delta t_k^i-I_k^i-T_k^i$ is the pseudo-range measurement value after correction.

The initial position ${\hat{x}}_0$ is determined by LS positioning [40], $\Gamma =eye(8)$, and $B=zeros(8)$, where eye indicates the unit diagonal matrix, zeros indicates the zero matrix, and the number in brackets is the dimension of the matrix. In the static experiment of GNSS positioning, the state transition coefficient matrix is defined as $A=\left[\begin{array}{cc}eye(4)& diag(\left[\begin{array}{cccc}0& 0& 0& T_s\end{array}\right])\\ zeros(4,4)& diag(\left[\begin{array}{cccc}0& 0& 0& T_s\end{array}\right])\end{array}\right]$, where $T_s$ is the sampling interval. In the kinematic experiment, $A=\left[\begin{array}{cc}eye(4)& T_s\ast eye(4)\\ zeros(4,4)& eye(4)\end{array}\right]$ [41].

In Figure 1, all calculations except $\Delta {\chi }_k^2$ are the routine Kalman filtering formula. For the gross error checking, the only increased computational burden is the $\Delta {\chi }_k^2$ calculation, in which all matrix values are ready from routine analysis and the dimension of the matrix inversion is the observation number at epoch k. Such an additional computation is tolerable for real-time positioning. In our GNSS positioning static experiment, the central processing unit (CPU) time is increased from 90.643 to 93.284 seconds (only a 2.91% hike), then increased from 4.431 to 4.561 seconds in the GNSS positioning kinematic experiment (only a 2.96% hike) due to the gross checking.

2.4. Mathematical Model of CI-RKF in GNSS/IMU/VO Integrated Positioning

An IMU is a commonly used auxiliary navigation device [42]. It has the advantages of being independent from external information, freedom from electromagnetic interference, and good short-term accuracy and stability [43]. However, an IMU requires sporadic external positioning information to set up its initial position and calibrate its solutions in order to avoid cumulative positioning errors. VO is a key module of lidar sensor navigation technology based on simultaneous localization and mapping (SLAM) [44]. It locates its position autonomously through the surrounding environment, independent of satellite signals [45,46]. Due to the image blur, lens blocking, and other issues, it is difficult to perform positioning applications using VO technology on its own. As a result, the combination of an IMU and VO can improve the reliability of the system. An IMU and VO provide the relative location, while GNSS provides the absolute location. This system can provide short-term and relatively accurate position information by using a VO/IMU when the GNSS signal is temporarily missing, and for this reason the GNSS/IMU/VIO has become a trend in current integrated navigation.

The construction of a combined GNSS and VO/IMU is based on the loose coupling method [47] in which the VO and IMU are tightly coupled (the combined process can be found in [48,49]). The velocity information from the VO/IMU and the position information of GNSS are used as inputs [50], and the system positions are estimated via filtering [51]. The system coupling mode is shown in Figure 2.

The state equation and the observation equation of the system are

$$X_k=A{X}_{k-1}+Q$$

(27)

$$Z_k=H{X}_k+R$$

(28)

where $A=\left[\begin{array}{cc}{I}_{3\ast 3}& T_s\times I_{3\ast 3}\\ 0_{3\ast 3}& I_{3\ast 3}\end{array}\right]$ is the state transition matrix, and $I$ is the identity matrix; $H=\left[I_{6\ast 6}\right]$ is the design matrix; $X_k={\left[\begin{array}{cccccc}{x}_k& y_k& z_k& v_x& v_y& v_z\end{array}\right]}^T$ is the matrix of six variables to be estimated, $x_k$, $y_k$, and $z_k$ are the positional variables; $v_x$, $v_y$, and $v_z$ are the velocity variables; Q is the state noise; and R is the observation noise.

The system state prediction and state estimation equations are

$${\hat{X}}_{k,k-1}=A{\hat{X}}_{k-1}$$

(29)

$${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})$$

(30)

The system status update process is as shown in Equation (24).

3. Data and Experiments

Experiment 1: A corridor between Building A and Building B of the Estuary and Coastal Research Institute in East China Normal University was selected as our platform, as shown in Figure 3. The experimental data were collected by a U-blox9 receiver. The sampling frequency was 1 Hz, and total data covered 5522 epochs. The positioning results from the Trimble R8 were taken as the reference solutions.

Experiment 2: A route was planned around the campus as the kinematic experiment environment, with the occlusion caused mainly by the surrounding trees and buildings. The experimental equipment was placed on the roof of a car to receive the satellite signal (Figure 4). The data were also collected by both U-blox9 and Trimble BD982 receivers with a 1-Hz acquisition frequency. The solutions were derived from the data of the Trimble BD982 receiver and were solved using Inertial Explorer 8.60, a high-precision post-processing software that served as the ground truth. In order to reduce the influence of instability in the attitude and route, the vehicle tried to keep running in the middle of road at a near-constant speed.

Experiment 3: Selecting part of the route from Experiment 2, the vehicle moved around the building- dense area of the campus at a relatively constant speed. A HUAWEI HonorV9 mobile phone was placed in the carriage to collect the raw GPS observations through software named rinexOn [52], and the receiving frequency was set to 1 Hz. The IMU (MIT-G-710 of xsen series) and VO (ZED Stereo Camera) were fixed on the roof of the car to collect the data, and the sampling frequency and frame rate were 100 Hz and 30 fps, respectively. At the same time, the Trimble BD982 receiver was placed on the roof with a receiving frequency of 1 Hz, and its data were processed by Inertial Explorer 8.60. The vehicle’s experimental devices are shown in Figure 5.

4. Results and Discussion

Two robust methods were constructed by a Chi-square increment in Section 2. In this section, we first compare their performances based on the data collected through Experiment 1, select the better one, and record it as robust Kalman filtering based on a Chi-square increment (CI-RKF). The results are shown in Section 4.1. Next, the CI-RKF is compared with conventional Kalman filtering (CKF) and the robust Kalman filtering constructed using predictive residual statistics and IGGШ weight factor (PRS-IGG-RKF) based on the data from Experiments 1 and 2, respectively. The results are discussed in Section 4.2 and Section 4.3. Finally, the three schemes are applied to the GNSS/IMU/VO integrated navigation, and the results are obtained by comparing the fusion positioning accuracy in the shelter zone based on Experiment 3, which is shown in Section 4.4.

4.1. Comparison of Chi-Square Increment Robust Methods

The experiment results of two robust incremental Chi-Square methods are shown in Figure 6. The α is set to 0.15 in both methods, and $c_0,c_1$ is set to 3.7 and 2.3 in methods 1 and 2, respectively.

It can be seen in Figure 6 that the deviations between the final positioning results and the true values of Method 2 were less than those of Method 1, and the convergence speed of Method 2 was faster than that of Method 1, meaning the detection index in Method 2 was more sensitive to the gross error. Because the vector $z_k-H_k{\hat{x}}_{k,k-1}$ was tested as a whole in Method 1, the entire observed noise covariance matrix will be enlarged if the sum of errors per epoch is greater than the detection threshold, and the magnification of each covariance will also be the same. However, in Method 2, the components in vector $z_k-H_k{\hat{x}}_{k,k-1}$ were tested individually. If a satellite fails, the corresponding noise variance term will be amplified, and the magnification of each variance varies according to the deviation degree of the residual. The final positioning accuracies of Method 2 were 21.02%, 60.69%, and 85.57% higher than that of Method 1 in the east, north and up (E/N/U) direction. Thus, Method 2 was selected for discussion in the following section.

4.2. Comparison of GNSS Positioning Schemes in a Static Experiment

In order to evaluate the quality of the raw observations, the experimental data were solved by the CKF using the incremental Chi-square method from Section 2.1. The change of the Chi-square increment in the static experiment is shown in Figure 7a. The normalized Chi-square increment was obtained by dividing the Chi-square increment by the total number of satellites per unit epoch, as shown in Figure 7b. It can be seen from Figure 8 that there are gross errors in the observation at the 500–600 and the 2000–2500 epochs. In Figure 3, the experimental equipment was placed in the area similar to the urban canyon [53], so that as the satellites were moving, some signals (called no-line-of sight (NLOS) signals) could only be received by reflections off the building [53], and this caused the outline (the huge leaps in 500–600 and the 2000–2500 epochs) in the incremental Chi-square. The delayed effect and the calculation model of the NLOS are discussed in [54], and here we used the incremental Chi-square to detect them, mitigating the influence of this kind of gross error via the robust Kalman filtering in this section.

In PRS-IGG-RKF, the constants $c_0,c_1$ were set to 1 and 5, respectively. In CI-RKF, the constants $c_0,c_1$ were set to 2 and 3, respectively, and $\alpha$ was set to 0.15. Figure 8 illustrates the time series of positioning errors from the three schemes in the E/N/U direction, and

Table 1. Position errors at the final epoch in the E/N/U direction in the static experiment.

Direction	Position Error (m)			Improve (%)
	CKF	PRS-IGG-RKF	CI-RKF	PRS-IGG-RKF	CI-RKF
East	−3.012	−2.830	−2.147	6.04	28.72
North	1.305	0.762	−0.053	41.61	95.94
Up	3.226	2.472	0.135	23.37	95.82

shows their position errors at the final epoch and the improved accuracy of the other two schemes relative to the CKF.

In Figure 8, the positioning errors from the three schemes coincided during the first 500 epochs, as no gross errors occurred (Figure 7). When the outliers emerged in the 500–600 and 2000–2500 epochs, the positioning results of the PRS-IGG-RKF fluctuated more significantly at these points, and the positioning results of the CI-RKF were relatively smooth as the numbers of iterations increased. The errors of the PRS-IGG-RKF tended to converge after about 4000 epochs, while those of the CI-RKF tended to converge after about 3500 epochs. Finally, the positioning errors of CI-RKF in the E/N/U direction were lower than those of the PRS-IGG-RKF. Both the PRS-IGG-RKF and CI-RKF were able to improve positioning accuracy, but the final positioning accuracies of CI-RKF were 22.68%, 54.33%, and 72.45% higher than those of the PRS-IGG-RKF in the E/N/U direction. The static experiment using GNSS positioning confirms that the Chi-square increment could detect outliers effectively, and the CI-RKF could resist the influence of gross errors in real time by enlarging the observation covariance matrix, providing a better performance compared with the PRS-IGG-RKF.

4.3. Comparison of GNSS Positioning Schemes in the Kinematic Experiment

The incremental Chi-square and its normalized form of the kinematic data from Experiment 2 are shown in Figure 9. It can be seen that gross errors occurred in the intervals within the 100–150 and the 200–300 epochs.

In the PRS-IGG-RKF, the constants $c_0,c_1$ were 1.5 and 5, respectively; in the CI-RKF, the constants $c_0,c_1$ were 1 and 4, respectively, and $\alpha$ was set to 0.15. The positioning errors in the E/N/U direction are shown in Figure 10.

During the abnormal epochs (Figure 9), the filtering solutions of CKF fluctuated noticeably, and there were large deviations between the positioning results and the real values. The maximum error of the CKF was about 15 meters at the 100–150 epoch, and 10 meters in the 200–300 epoch. The accuracy of the subsequent solutions was influenced by these outliers. Although there was continuous GNSS navigation, it can be seen that gross errors still had a significant impact on positioning in the area with poor observation. However, the overall performance of the PRS-IGG-RKF and CI-RKF were relatively stable, especially at these epochs, which greatly reduced the errors and caused them to fluctuate near zero in the east and north direction.

Table 2. The RMS of position errors in the E/N/U direction in the kinematic experiment.

		Position Error (m)			Improve (%)
		CKF	PRS-IGG-RKF	CI-RKF	PRS-IGG-RKF	CI-RKF
Overall	East	2.334	1.659	1.372	28.92	41.22
	North	2.860	1.511	1.297	47.15	54.65
	Up	4.547	4.435	3.701	2.46	18.61
Shelter Zones	East	3.585	2.390	1.956	33.33	45.44
	North	3.888	2.024	1.675	47.94	56.93
	Up	6.428	3.311	3.687	48.50	42.64

shows the positioning errors of the total region and occluded areas from three schemes. For the total region, the performances of both the PRS-IGG-RKF and CI-RKF were better than of the CKF, and the positioning accuracies of the CI-RKF were about 12.3%, 7.5%, and 16.15% is higher in the E/N/U direction than those of the PRS-IGG-RKF. For the occlusion area, the positioning accuracies of the CI-RKF were 12.11% and 8.99% higher than those of the PRS-IGG-RKF in the east and north directions, but they underperformed in the up direction. Overall, the CI-RKF performed better, but more attention should be paid to the accuracy of the horizontal direction.

4.4. Integrated Navigation Experiment in an Occluded Urban Area

In heavily occluded areas, crowded buildings limit the sky visibility and increase interference from multipath and NLOS signals. The GNSS/IMU/VO integrated navigation system is commonly deployed to improve accuracy and stability of the positioning, while filling in GNSS observation gaps caused by eclipsed signals and huge outliers (in particular for low-cost GNSS receivers). The critical issue then is how to detect and identify outliers effectively when the true trajectory remains unknown. In this experiment, the robust Kalman filtering was an effective method, and three schemes were applied to a GNSS/IMU/VO integrated navigation system. Figure 11 compares the trajectories from three schemes. Fusion combined the GNSS raw observations with the positioning results from the VO/IMU data in a loosely coupled way via the CKF. The robust fusion trajectories of the integrated navigation system were obtained by the PRS-IGG-RKF (Robust Fusion 1) or CI-RKF (Robust Fusion 2). In this part, the robust weighting factors of the CI-RKF were adjusted slightly, according to the IMU’s ability to provide positioning information without GNSS signals over a short time. The GPS information from the mobile phone will be disused, as it provided very poor observations (that is, the corresponding observation noise variance was amplified to infinity).

The results from different Kalman filtering schemes in occluded areas were also projected into Google Earth, as shown in Figure 12. The respective position errors in the E/N/U directions of shelter zones 1 and 2 were calculated (

Table 3. The RMS of position errors in the E/N/U direction for shelter zones in the integrated navigation experiment.

		Position Error (m)			Improve (%)
		Fusion	Robust Fusion 1	Robust Fusion 2	Robust Fusion 1	Robust Fusion 2
Shelter Zone 1	East	4.939	2.889	1.643	41.51	66.73
	North	2.029	1.442	0.820	28.93	59.59
	Up	2.365	1.680	0.955	28.96	59.62
Shelter Zone 2	East	25.782	16.030	14.944	37.85	42.04
	North	12.946	6.094	5.303	52.93	59.04
	Up	15.089	9.615	7.181	36.28	52.41

).

Figure 11 indicates that the results of mobile phone positioning by GPS only had a large deviation, even interruption. In the GNSS/IMU/VO integrated navigation, when the receiver had no sufficient GPS satellite signals, or the observed GPS signals contained severe gross errors (and hence needed to be eliminated), the IMU/VO was able to assist navigation within a short interval, so that users could obtain a sustained positioning trajectory with good stability. The upper left and lower right corner of Figure 11a show the areas where the GPS-only solutions deviated seriously, and the integrated navigation trajectory of GNSS/IMU/VO also fluctuated significantly. This shows that in areas with serious occlusion or where GPS signals experienced gross errors, the CKF integrated navigation was not completely immune and the positioning results still suffered. Figure 11b shows the PRS-IGG-RKF results for data fusion, and that it improved positioning to a certain extent and mitigates *but not completely eliminated) the influence of gross errors. Figure 11c demonstrates the ability of the CI-RKF results to fuse with GNSS/IMU/VO integrated navigation data. This robust fusion method generated a smoother trajectory than in Figure 11a,b, and was closer to the ground truth. Figure 12 zooms the solution trajectories in on the two sheltered areas, displaying that the trajectory of the CI-RKF approach was significantly better than the other two schemes. Table 3 shows the statistics of the position errors for the two occluded areas, and that accuracy was greatly improved when the CI-RKF was applied.

5. Conclusions

This paper derives an incremental Chi-square using Kalman filtering by extending the definition of RLS and comparing its expression with currently adopted Chi-square variables. The comparison indicates that the definitions of $\Delta {\chi }_k^2$ from the extension of RLS and ICST are equivalent, and reveals that $\Delta {\chi }_k^2$ actually comes from both the change of fitting solutions and the residuals of new observations. In this paper, two methods were proposed for detecting anomalous observations via an incremental Chi-square. The experiment results show that the second method (called the CI-RKF), which provided an adaptive magnification to each term of the observation covariance, was superior. We applied the CI-RKF to the environment with an obvious NLOS effect, and its final positioning accuracies were 28.72%, 95.94%, and 95.82% higher than the CKF, and 22.68%, 54.33%, and 72.45% higher than the PRS-IGG-RKF, respectively. The static experiment indicates that the CI-RKF could detect the outlines and obtain satisfactory positioning solutions. Subsequently, we applied the CI-RKF to two kinematic experiments. First, the CI-RKF was tested using the GNSS positioning model, and the results show that its accuracies were 28.72%, 95.94%, and 95.82% higher than the CKF, and 22.68%, 54.33%, and 72.45% higher than the PRS-IGG-RKF. The overall positioning accuracies were 41.22%, 54.65%, and 18.61% higher than the CKF, and 12.3%, 7.5%, and 16.15% higher than the PRS-IGG-RKF. In next experiment, we replaced the U-blox by a mobile phone with an internal low-cost GNSS antenna, whose observation quality was relatively poorer than the U-blox and easily influenced by surrounding environment. We use its output as the satellite signal for an integrated navigation combining an IMU and VO. Compared with the positioning results obtained from the CKF fusion method, the positioning accuracy of the CI-RKF fusion method increased by 66.73%, 59.59%, and 59.62% in shelter zone 1, and by 42.04%, 59.04%, and 52.41% in shelter zone 2. When compared with the PRS-IGG-RKF fusion method, the CI-RKF was 25.22%, 30.66%, 30.66% for shelter zone 1, and 4.19%, 6.11%, and 16.13% higher for shelter zone 2. These kinematic experiments illustrate that the CI-RKF is able to improve the positioning accuracy of the GNSS and the integrated navigation system, while also enhancing accuracy and stability for real-time positioning in occluded urban areas.

The robust Kalman filtering approach using $\Delta {\chi }_k^2$ is general. This paper demonstrates its usage in GNSS time of arrival (TOA) positioning. It can also be used in other positioning approaches, such as time difference of arrival (TDOA), angle of arrival (AOA), received signal strength (RSS), among others.