Multi Sensor Fusion Based on Adaptive Kalman Filtering

Abstract

The optimal performance of the conventional Kalman filters is not guaranteed, when there is uncertainty in the process and measurement noise covariances. In this paper, in order to reduce the effect of noise covariance uncertainty, the Fuzzy Adaptive Iterated Extended Kalman Filter (FAIEKF) and Fuzzy Adaptive Unscented Kalman Filter (FAUKF) are proposed to overcome this drawback. The proposed FAIEKF and FAUKF have been applied to fuse signals from Global Positioning System (GPS) and Inertial Navigation Systems (INS) for the autonomous vehicles’ navigation. In order to validate the accuracy and convergence of the proposed approaches, results obtained by FAUKF and FAIEKF were compared to the Fuzzy Adaptive Extended Kalman Filter (FAEKF), Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), and Iterated Extended Kalman Filter (IEKF). The simulation results illustrate the superior performance of the AKUKF compared to the other filters.

1 Introduction

Data fusion techniques have been widely employed in multisensory environments to fuse data from multiple sensors in order to achieve lower detection errors and higher reliability. One of the most popular approaches for navigation sensor fusion is implementing the conventional Kalman filters [1,2,3]. The significant difficulties in designing the conventional Kalman filters (i.e. EKF, UKF, and IEKF) for sensor fusion is incomplete prior information on covariance matrices [4]. Poor knowledge of the models may seriously reduce the Kalman filters’ performance, and make the filters unstable. Numerous model proposals have been improved to address this problem over the last twenty years, including the adaptive Kalman filtering approach, which has proven to be an effective strategy for managing the limitations associated with the conventional Kalman filters. An adaptive Extended Kalman Filter for the localization of mobile robots were developed by Jetto and Longhi [5]. In their work, the data provided by sonar and odometrical sensors were fused through an adaptive EKF to provide online estimates of a robot’s position. An adaptive two stage EKF for estimating unknown fault bias in an INS/GPS loosely coupled system, was proposed by Kim and Lee [6]. An adaptive EKF using artificial neural networks was developed by Stubberu et al. [7], who designed a neuro-observer that can learn system uncertainties and improve the overall performance of an uncertain control system in the state-estimator model. An adaptive UKF algorithm for target tracking with unknown process noise statistics was introduced by Shi et al. [8]. In this algorithm, a modified Sage-Husa noise statistics estimator was introduced to assess the system process noise variance adaptively. An adaptive fading UKF with Q-Adaptation for attitude estimation was introduced by Soken et al. [9].

The adaptive tuning of the Kalman filter with the fuzzy logic has been very popular for managing systems with dynamical uncertainties, particularly in the field of adaptive control. A fuzzy adaptive strong tracking Kalman filter for ultra-tight GPS/INS integration was introduced by D. Jwo et al. [10]. In this work, the performance of the filter improved by adaptively modifying the sub-optimal fading factor via fuzzy logic. J.Z. Sasiadek et al. [11,12,13,14], proposed an adaptive filter known as the Fuzzy Adaptive Extended Kalman Filter (FAEKF), to adapt the process and measurement noise covariance matrices. The method was based on exponential data weighting, to protect the EKF from divergence. In this work, the EKF has been modified using the fuzzy logic.

This paper focuses on the development of new integration algorithms based on the combination of the Fuzzy Logic Controllers (FLCs) and conventional Kalman filters such as the Iterated Extended Kalman Filter (IEKF) and Unscented Kalman Filter (UKF) to provide reliable and accurate navigation solutions. The proposed algorithms are based on the correction of both the process noise covariance matrix Q, and the measurement error covariance R. The FLCs are implemented to adjust the exponential weighting of weighted UKF and IEKF and protect the filters from divergence. The rest of the paper is organized as follows. Section 2 describes two proposed adaptive Kalman filtering approaches. Section 3 introduces the Fuzzy Logic Controllers. In order to validate the effectiveness of the proposed adaptive algorithms, simulation results are discussed in Sect. 4 and finally, in Sect. 5, the conclusions of this work are given.

2 The Adaptive Estimation Algorithms

2.1 System Description

The non-linear dynamic and measurement models are given by:

$$\begin{aligned} \mathbf x _{k}= \mathbf f (\mathbf x _{k-1},\mathbf u _{k})+\mathbf w _{k} \end{aligned}$$

(1)

$$\begin{aligned} \mathbf y _{k}= \mathbf h (\mathbf x _{k})+\mathbf v _{k} \end{aligned}$$

(2)

where the vectors $\mathbf x _{k}\in \mathfrak {R}^{8}$, represents the state of the system at the time point k. The process noise is given by $\mathbf w _{k}$, and $\mathbf z _{k}\in \mathfrak {R}^{4}$, corresponds to the observed measurement signal, driven by Gaussian white noise $\mathbf v _{k}$.

In order to protect the conventional Kalman filters from divergence when there are uncertainties in the system noise covariances, exponential data weighting is applied. Two new adaptive formulations, weighted Iterated Extended Kalman Filter and weighted Unscented Kalman Filter can be described as follow.

2.2 Weighted Iterated Extended Kalman Filter

The model and implementation equations for the weighted Iterated Extended Kalman Filter are defined as the following recursive equations.

2.2.1 Initialization

$$\begin{aligned} \hat{\mathbf{x }}_{0}=E(\mathbf x _{0}),\quad \quad \quad \mathbf P _{0}= E[(\mathbf x _{0}-\hat{\mathbf{x }}_{0})(\mathbf x _{0}-\hat{\mathbf{x }}_{0})^{T} ] \end{aligned}$$

(3)

2.2.2 Prediction

The predicted state can be defined as:

$$\begin{aligned} {\hat{\mathbf{x }}}_{k|k-1}=\mathbf{f (\hat{\mathbf{x }}}_{k-1},\mathbf u _{k}) \end{aligned}$$

(4)

The covariance matrices of the adaptive IEKF defined as:

$$\begin{aligned} \mathbf R _{k}= \alpha ^{-2(k+1)}{} \mathbf R \end{aligned}$$

(5)

$$\begin{aligned} \mathbf Q _{k}= \alpha ^{-2(k+1)}{} \mathbf Q \end{aligned}$$

(6)

where, $\alpha \ge 1$. For $\alpha >1$, as the time increases the covariance matrices decrease which means the recent data has more credibility, due to the exponential decreased noise covariance with time. When $\alpha =1$, the filter is acting like a regular IEKF. It should be noted that $\alpha$ is the output of the fuzzy controllers.

$$\begin{aligned} \mathbf P _{k}^{-}= \mathbf F _{k-1}{} \mathbf P ^{-}_{{k-1}}{} \mathbf F ^{T}_{k-1}+\alpha ^{-2(k+1)}{} \mathbf Q \end{aligned}$$

(7)

where, $\mathbf F _{k-1}$, the linear approximation equation can be present in form of:

$$\begin{aligned} \mathbf F _{k-1}\approx \frac{\partial \mathbf{f }}{\partial \mathbf x }\mid _{{\hat{\mathbf{x }}}_{{k-1},\mathbf u _{k}}} \end{aligned}$$

(8)

By defining the weighted covariance as

$$\begin{aligned} \mathbf P ^{-}_{k}= \alpha ^{-2k}{} \mathbf P _{k|k-1} \end{aligned}$$

(9)

By calculating ${\hat{\mathbf{x }}}_{k}$, $\mathbf K _{k}$, $\mathbf P _{k}$ at each iteration about the most recent estimate. The Kalman gain can be computed as:

$$\begin{aligned} \mathbf K _{k,i}= \mathbf P ^{{-}}_{k}{} \mathbf H _{k,i}^{T}(\mathbf H _{k,i}{} \mathbf P _{k}^{-}{} \mathbf H _{k,i}^{T}+\alpha ^{-2(k+1)}{} \mathbf R )^{-1} \end{aligned}$$

(10)

The superscript i, $(i = 0,2,\ldots , \tau )$, is the number of iteration steps. By using (9), the Kalman gain can be rewritten as:

$$\begin{aligned} \mathbf K _{k,i}=\mathbf P _{k|k-1}{} \mathbf H _{k,i}^{T}(\mathbf H _{k,i}{} \mathbf P _{k|k-1}{} \mathbf H _{k,i}^{T}+ \mathbf R /\alpha ^{2})^{-1} \end{aligned}$$

(11)

where

$$\begin{aligned} \mathbf H _{k,i}\approx \frac{\partial \mathbf{h }}{\partial \mathbf x }\mid _{{\hat{\mathbf{x }}}_{k,i}} \end{aligned}$$

(12)

The predicted measurement estimation can be rewritten as:

$$\begin{aligned} {\hat{\mathbf{x }}}_{k,i+1}={\hat{\mathbf{x }}}_{k|k-1}+\mathbf K _{k,i}(\mathbf y _{k}-{\mathbf{h }({\hat{\mathbf{x }}}_{k,i})}-\mathbf H _{k,i}(\hat{\mathbf{x }}_{k|k-1}-{\hat{\mathbf{x }}}_{k,i})) \end{aligned}$$

(13)

where, ${\hat{\mathbf{x }}}_{k,i}$ presents the estimate at time point k and $i_{th}$ iteration. The iteration process is initialized with ${\hat{\mathbf{x }}}_{k,0}={{\hat{\mathbf{x }}}}_{k|k-1}$.

And the posterior covariance matrix defined as:

$$\begin{aligned} \mathbf P _{k,i}= (\mathbf I -\mathbf K _{k,i}{} \mathbf H _{k,i})\mathbf P _{k|k-1} \end{aligned}$$

(14)

The iterative process will not be stopped until a certain termination condition is met.

2.3 Weighted Unscented Kalman Filter

2.3.1 Initialization

$$\begin{aligned} {\hat{\mathbf{x }}}_{0}=E(\mathbf x _{0}),\quad \quad \quad \mathbf P _{0}= E[(\mathbf x _{0}-{\hat{\mathbf{x }}}_{0})(\mathbf x _{0}-{\hat{\mathbf{x }}}_{0})^{T} ] \end{aligned}$$

(15)

2.3.2 Prediction

For the L elements state vector, a set of $(2L+1)$ sigma-points are created according to the following:

$$ \varvec{\chi }_{k-1}=[\hat{\mathbf{x }}_{k-1} \quad \hat{\mathbf{x }}_{k-1}+{\sqrt{(L+\lambda }}){\sqrt{\mathbf {P}_{k-1}}}\quad \hat{\mathbf{x }}_{k-1}- $$

$$\begin{aligned} {\sqrt{(L+\lambda )}}{\sqrt{{\mathbf {P}_{k-1}}}} ] \end{aligned}$$

(16)

where each column of $\varvec{\chi }_{k-1}$, represents a sigma-point. ${\sqrt{\mathbf {P}_{k-1}}}=chol(\mathbf P _{k-1})$ is the square root of the state error covariance [15], and the scaling parameter $\lambda$ defined as:

$$\begin{aligned} \lambda =\sigma ^{2}(L+\kappa )-L \end{aligned}$$

(17)

where, $\sigma$, ${1e^{-4}}\le \sigma \le 1$, determines the size of the sigma-points distribution and $\kappa$ influences the accuracy of the approximation [16]. Once the sigma-points have been generated, each point is propagated through out the non-linear state equation as:

$$\begin{aligned} \varvec{\chi }^{(i)}_{k|k-1}=\mathbf f (\varvec{\chi }_{k-1},\mathbf u _{k}),\quad \quad i=0,\ldots ,2L \end{aligned}$$

(18)

The mean and covariance are approximated using a weighted mean and covariance of the transformed points as:

$$\begin{aligned} \hat{\mathbf{x }}_{k|k-1}=\sum ^{2L}_{i=0} \varvec{\eta }^{(m)}_{i}\varvec{\chi }^{(i)}_{k|k-1} \end{aligned}$$

(19)

$\mathbf P _{k|k-1}=$

$$\begin{aligned} \sum _{i=0}^{2L}\varvec{\eta }^{(c)}_{i}(\varvec{\chi }^{(i)}_{k|k-1}-\hat{\mathbf{x }}_{k|k-1})(\varvec{\chi }^{(i)}_{k|k-1}-\hat{\mathbf{x }}_{k|k-1})^{T}+\alpha ^{-2(k+1)}{} \mathbf Q \end{aligned}$$

(20)

where the mean weight vector $\varvec{\eta }^{(m)}_{i}$, and the covariance weight vector $\varvec{\eta }^{(c)}_{i}$ associated with the $i_{th}$ point are defined as:

$$\begin{aligned} \varvec{\eta }^{(c)}_{i}=\varvec{\eta }^{(m)}_{i}=1/(2(L+\lambda )), \quad i=1,\ldots ,2L \end{aligned}$$

(21)

$$\begin{aligned} \varvec{\gamma }^{(i)}_{k|k-1}=\mathbf h (\varvec{\chi }^{(i)}_{k|k-1}) \end{aligned}$$

(22)

Then the mean of the measurement vector is calculated as:

$$\begin{aligned} \hat{\mathbf{y }}_{k|k-1}=\sum ^{2L}_{i=0} \varvec{\eta }^{(m)}_{i}\varvec{\gamma }^{(i)}_{k|k-1} \end{aligned}$$

(23)

Covariance and cross-covariance matrices of the proposed adaptive UKF are defined as:

$$\begin{aligned} \mathbf P ^{yy}_{k}=\sum _{i=0}^{2L}\eta ^{(c)}_{i}(\varvec{\gamma }^{(i)}_{k|k-1}-\hat{\mathbf{y }}_{k|k-1})(\varvec{\gamma }^{(i)}_{k|k-1}-\hat{\mathbf{y }}_{k|k-1})^{T}+\alpha ^{-2}{} \mathbf R \end{aligned}$$

(24)

$$\begin{aligned} \mathbf P ^{xy}_{k}=\alpha ^{-2(k-1)}\sum _{i=0}^{2L}\eta ^{(c)}_{i}(\varvec{\chi }^{(i)}_{k|k-1}-\hat{\mathbf{x }}_{k|k-1})(\varvec{\gamma }^{(i)}_{k|k-1}-\hat{\mathbf{y }}_{k|k-1})^{T} \end{aligned}$$

(25)

Similar to the weighted IEKF, When $\alpha =1$, the filter is acting like a regular UKF. For $\alpha >1$, as the time increases the covariance matrices decrease.

2.3.3 Update

$$\begin{aligned} \mathbf K _{k}=\mathbf P ^{xy}_{k} (\mathbf P ^{yy}_{k})^{-1} \end{aligned}$$

(26)

The Kalman gain $\mathbf K _{k}$, is then used to update the state and covariance estimates as:

$$\begin{aligned} \hat{\mathbf{x }}_{k}=\hat{\mathbf{x }}_{k|k-1}+\mathbf K _{k}(\mathbf{y }_{k}-\hat{\mathbf{y }}_{k|k-1}) \end{aligned}$$

(27)

$$\begin{aligned} \mathbf P _{k}=\mathbf P _{k|k-1}-\mathbf K _{k}{} \mathbf P ^{yy}_{k}{} \mathbf K ^{T}_{k} \end{aligned}$$

(28)

3 Fuzzy Logic Controllers

The conventional Kalman filters provide an effective means of estimating the state of a system from noisy measurements when the covariances of the system are known, and the system is well defined. However, in some cases, there are uncertainties in the system noise covariances, which can cause filters to become unstable. Adaptive tuning of the conventional Kalman filters via fuzzy logic has been one of the promising strategies to protect the filters from divergence when dealing with parameter uncertainty and non-white process noise [13]. In order to adjust the filters, the FLCs continuously monitors and tunes the noise level in the filters, internal model. The residuals of Kalman filter should be zero mean white noise process; if not, divergence will happen. Hence, in this paper, the covariance and mean values of the residuals are used as inputs to the FLCs to decide the degrees of divergence.

The block diagram for the GPS/INS navigation sensor fusion using the FAUKF and FAIEKF is shown in Fig. 1. The proposed fuzzy adaptive Kalman filters have been validated for two different cases.

3.1 Fuzzy Logic Adaptive System for Parameter Uncertainty

The uncertain or time varying parameters in Q and R matrices make the conventional Kalman filters diverge or coverage to a large bound. When the filter does not work well, the FLCs would apply a suitable weighting factor to Enhance the accuracy of the filter. Two groups of fuzzy controllers have been defined for parameter uncertainty.

Fig. 1

Block diagram representation of fuzzy adaptive Kalman filter for GPS/INS integration

3.1.1 First Fuzzy Controller

In this controller, the mean values and covariance of the residuals are inputs to the FLC to determine the degree of divergence. The exponential weighting $\alpha$ is the first fuzzy controller output. The FLC will select the suitable $\alpha$ to optimally adapt the Kalman filter. The membership function of FLC inputs (i.e. the mean and covariance of residuals) and the output $\alpha$ are illustrated in Figs. 2, 3 and 4, respectively. The characteristics of a fuzzy system are highly dependent on the relevant rules. The proposed fuzzy logic controller used 9 rules as shown in Table 1.

3.1.2 Second Fuzzy Controller

The second controller was designed to detect the changes in the R matrix, and adapt the filter accordingly. The measurement covariance matrix, R, is related to the residual’s covariance; thus, any changes in the measurement covariance matrix can alter the covariance of the residuals. With this controller, the UKF and IEKF is adapted by selecting the appropriate scale. For example, the FLC applies a large scale to adjust the $\alpha$ if it determines that the covariance of the residual is greater than expected.

$$\begin{aligned} \alpha _{k+1}=(\alpha _{k}-1)*{ scale}+1 \end{aligned}$$

(29)

Fig. 2

Mean value membership functions for parameter uncertainty [J.Z. Sasiadek et al. [13]]

Fig. 3

Covariance membership functions for parameter uncertainty [J.Z. Sasiadek et al. [13]]

Fig. 4

$\alpha$ membership functions for parameter uncertainty [J.Z. Sasiadek et al. [13]]

Table 1 Rule table of $\alpha$ for parameter uncertainty

Table 2 Rule table of scale for parameter uncertainty

The 9 rules for this fuzzy controller are shown in Table 2.

3.2 Fuzzy Logic Adaptive System for Non-white Process Noise

The conventional Kalman filters require that both, the process noise $\mathbf w _{k}$, and the measurement noise $\mathbf v _k$ are zero-mean white noise with known covariance Q and R. In practice, sometimes the process noise could be correlated with itself. Implementing the FLC is one alternative to this problem

The membership functions for this fuzzy control are shown in Figs. 5, 6 and 7 respectively.

There are 9 rules for the FLC in the present of non-white noise, Table 3, hence, little computational time is required.

Table 3 Rule table of $\alpha$ for non-white noise

Fig. 5

Mean value membership functions for non-with process noise [J.Z. Sasiadek et al. [13]]

Fig. 6

Covariance membership functions for non-with process noise [J.Z. Sasiadek et al. [13]]

Fig. 7

$\alpha$ membership functions for non-with process noise [J.Z. Sasiadek et al. [13]]

4 Simulation Results

Numerical simulation has been done to validate the performance of the proposed FAUKF and FAIEKF in comparison with FAEKF and the conventional EKF, UKF and IEKF approaches for INS/GPS integration when dealing with non-white noise and uncertain or time varying parameters are considered to exist in the process and measurement noise matrices. More detailed descriptions of weighted EKF and standard Kalman filters approaches for GPS/INS integration are given in J.Z. Sasiadek et al. [13] and S. Yazdkhasti et al. [17, 18], respectively (Fig. 5).

Table 4 The position errors with the non-white noise inputs

The state vectors used in the simulation included three states for INS position errors, three for INS velocity errors and two states for GPS range drift and range bias as:

$$\begin{aligned} \mathbf x _{k}=[\textit{x}_{k},\textit{y}_{k},\textit{z}_{k},\dot{x_{k}},\dot{y_{k}},\dot{z_{k}},C\varDelta t,C\dot{\varDelta }t] \end{aligned}$$

(30)

where, x points east, y points north and z is the attitude, $C\varDelta t$, $C\dot{\varDelta }t$ represent GPS range and drift states (Fig. 6).

Fig. 8

Inovation sequence (residual)

Fig. 9

Variance of the Adaptive Filters for 5Q and 4R

Fig. 10

Variance of the convential filters for 5Q and 4R

Fig. 11

Variance of the adaptive filters for 5Q and R

Fig. 12

Variance of the convential filters for 5Q and R

Fig. 13

Variance of the adaptive filters for Q and 4R

Fig. 14

Variance of convential filters for Q and 4R

For the first part of the simulation, the process noise $\mathbf w _{k}$ assumed as a non-zero mean process noise. The white noise with a standard deviation of 4 m was applied to the GPS measurements. The simulation was done for different INS mean values $(\mathbf w _{k})$, for the East (x), North (y), and Altitude (z) respectively (Fig. 7).

The summary of root mean square (RMS) errors of the six GPS/INS configurations provided in Table 4. As it is indicated, the fuzzy adaptive filters clearly improved the performance of the conventional Kalman filters. The position errors of FAEKF, FAUKF and FAIEKF are much smaller than that of EKF, UKF, and IEKF. Among the six approaches, the FAUKF demonstrates superior navigation accuracy performance in comparison with other filters.

The residuals of the six filters are shown in Fig. 8. The residual is the difference between the best measurement prediction based on the filter’s internal mode and the actual measurement; hence, it can be used to evaluate filter’s performance. From Fig. 8 it could be noticed that the residual of the EKF, UKF, and IEKF have a large drift, while the residual mean value of FAEKF, FAUKF, and FAIEKF are smaller than that of EKF, UKF, IEKF.

For the second part of the simulation, the developed algorithms have been tested and evaluated for various parameter uncertainties. The white noise with a standard deviation of 5 m was applied to the GPS measurements. The INS standard deviations are 0.006, 0.006, and 0.0006 m for the East (x), North (y), and Altitude (z), respectively. Figures 9 and 10 illustrates the covariance of Fuzzy adaptive filters and standard Kalman filters when high uncertainty, 5Q and 4R, exists. 5Q and 4R mean the real time parameters are 5 and 4 times as large as the designed Q and R. The simulation was repeated for different covariance values (5Q, R), Figs. 11 and 12 and (Q, 4R), Figs. 13 and 14.

Due to better treatment of parameter uncertainties, the FAUKF and FAIEKF has shown performance improvement when compared to the conventional UKF and IEKF, respectively.

5 Conclusion

This paper develops new data fusion techniques for GPS/INS integration by incorporating Fuzzy Logic controller (FLC) and the conventional Kalman filters. The Fuzzy Adaptive Unscented Kalman Filter (FAUKF) and a Fuzzy Adaptive Iterated Extended Kalman Filter (FAIEKF) have been developed to improve the Unscented Kalman Filter and Iterated Extended Kalman filter Performance, respectively. By monitoring the residual, the FLC can detect uncertainty or time varying Parameters in both, the process, and the measurement noise covariance matrices. And then adapt the Kalman gain in real time to improve the IEKF and UKF performance and avoid the filters from divergence. Performance comparisons on FAUKF, FAIEKF, FAEKF, UKF, EKF, and IEKF have been conducted. The simulation results show the proposed FAUKF leads to very accurate results.

Appendices

Appendix

GPS Satellite Geometry

Four pseudo range measurements are used as a measurement model of the Kalman filter.

$$ h_{1}=\sqrt{(X_{1}-x)^{2}+(Y_{1}-y)^{2}+(Z_{1}-z)^{2}}+C\varDelta t_{1} $$

$$ h_{2}=\sqrt{(X_{2}-x)^{2}+(Y_{2}-y)^{2}+(Z_{2}-z)^{2}}+C\varDelta t_{2} $$

$$ h_{3}=\sqrt{(X_{3}-x)^{2}+(Y_{3}-y)^{2}+(Z_{3}-z)^{2}}+C\varDelta t_{3} $$

$$\begin{aligned} \quad h_{4}=\sqrt{(X_{4}-x)^{2}+(Y_{4}-y)^{2}+(Z_{4}-z)^{2}}+C\varDelta t_{4} \end{aligned}$$

(31)

where, $(X_{1},Y_{1},Z_{1})$, $(X_{2},Y_{2},Z_{2})$, $(X_{3},Y_{3},Z_{3})$, $(X_{4},Y_{4},Z_{4})$ are the positions of the four GPS satellites respectively, and (x, y, z) are the position of the vehicle. The GPS satellite assumed to be in circular orbits.

$$\begin{aligned} X_{j}=R[cos \theta _{j} cos \varOmega _{i}-sin\theta _{j} sin\varOmega _{j} cos55^{\circ }] \end{aligned}$$

(32)

$$\begin{aligned} Y_{j}=R[cos \theta _{j} sin \varOmega _{j}+sin\theta _{j} sin\varOmega _{j} cos55^{\circ }] \end{aligned}$$

(33)

$$\begin{aligned} Z_{j}=R[sin \theta _{j} sin55^{\circ }] \end{aligned}$$

(34)

where

$$ \theta _{j}= \theta _{0}+T \dfrac{360}{43082}\,{\mathrm {deg}} \quad j=1,\ldots ,4 $$

$$ \varOmega _{j}=\varOmega _{0}-T \dfrac{360}{86164}\,{\mathrm {deg}} $$

$$\begin{aligned} R=26560000\,{\mathrm m} \end{aligned}$$

(35)

$$\begin{aligned} \mathbf P _{0}=\begin{bmatrix} 100&0&0&0&0&0&0&0 \\0&10&0&0&0&0&0&0\\0&0&100&0&0&0&0&0\\ 0&0&0&10&0&0&0&0\\ 0&0&0&0&100&0&0&0\\ 0&0&0&0&0&10&0&0\\ 0&0&0&0&0&0&100&0\\ 0&0&0&0&0&0&0&10\end{bmatrix} \end{aligned}$$

(36)

$$\begin{aligned} \mathbf Q = \begin{bmatrix} {{\sigma } }_{{{ x}}}^{{2}}{t}^{{3}}/{3 }&\sigma _{x}^{2}t^{2}/2&0&0&0&0&0&0 \\ {\sigma } _{x}^{2}{t}^{2}/2&{\sigma } _{x}^{2}t^{2}&0&0&0&0&0&0 \\ 0&0&{\sigma }_{{y}}^{2}t^{3}/3&\sigma _{y}^{2}t^{2}/2&0&0&0&0 \\ 0&0&{\sigma } _{y}^{2}t^{2}/2&{\sigma } _{y}^{2}t^{2}&0&0&0&0&\\ 0&0&0&0&{\sigma } _{z}^{2}t^{3}/3&{\sigma } _{z}^{2}t^{2}/2&0&0 \\ 0&0&0&0&{\sigma } _{z}^{2}t^{2}/2&{\sigma } _{z}^{2}t^{2}&0&{2}/{3}\\ 0&0&0&0&0&0&({S}_{a}t+\frac{{S}_{b}t^{3}}{3})c^{2}&\frac{S_{b}t^{2}c^{2}}{2} \\ 0&0&0&0&0&0&\frac{{S}_{b}t^{3}c^{2}}{2}&{S_{b}} \end{bmatrix} \end{aligned}$$

(37)

where, $\sigma _{x}$, $\sigma _{y}$, and $\sigma _{z}$ represent standard deviations associated with x, y, and z, respectively. t is the sample time, c is the speed of light $S_{a}=0.4(10)^{-18}$, standard deviation of clock offset, and $S_{b}=1.58(10)^{-18}$, is the standard derivation associated with velocity (Table 5).

Table 5 Satellite parameters