ON A NEW FAMILY OF KALMAN FILTER ALGORITHMS FOR INTEGRATED NAVIGATION

Here we present a review on a new family of Kalman filter algorithms which recently developed for integrated navigation. In particular it is useful for vision based navigation due to the type of data. Here we mainly focus on three algorithms namely weighted Total Kalman filter (WTKF), integrated Kalman filter (IKF) and constrained integrated Kalman filter (CIKF). The common characteristic of these algorithms is that they can consider the neglected random observed quantities which may appear in the dynamic model. Moreover, our approach makes use of condition equations and straightforward variance propagation rules. The WTKF algorithm can deal with problems with arbitrary weight matrixes. Both of the observation equations and system equations can be dynamic-errors-in-variables (DEIV) models in the IKF algorithms. In some problems a quadratic constraint may exist. They can be solved by CIKF algorithm. Finally, we compare four algorithms WTKF, IKF, CIKF and EKF in numerical examples.


Introduction
Recently, there has been an explosion in the number, type and diversity of system designs and application areas of mobile sensors.The navigation of these systems is one of the main problems.In this problem, one aims to determine the position and attitude of a mobile sensor in a geo-referenced frame.When this information is attained directly by means of measurements from sensors on-board the vehicle the term direct geo-referencing is used (Skaloud (1999)).The integration of these data is done during a Kalman filter algorithm (Kalman (1960)).In the literature, the Kalman filter is derived as either a best predictor (BP) or a best linear predictor (BLP), see e.g.Kalman (1960), Gelb (1974), Teunissen and Khodabandeh (2013).The minimum mean squared error (MMSE) is the criterion which selects the best predictor or estimator.By now, one of the common solutions to this problem has been the traditional extended Kalman filter (EKF) (Jazwinski (1970)), in particular, when the navigation systems have relied mainly on the GPS receivers as the primary source of information to provide the position of the vehicle.GPS is able to provide precise positioning information to an unlimited number of users anywhere on 2 * the planet.However, a comprehensive study, referred to as the Volpe report (Center (2001)), indicates several vulnerabilities of GPS associated with signal disruptions.GPS can provide precise information only under ideal conditions which require an open environment (i.e.open space areas).In other words, the system doesn't work very well in urban, canopy areas due to signal blockage and can be totally blocked if the signal is jammed (see e. g.Sheta (2012) andStepanyan (2006)).Nevertheless, motivated by the new advances in remote sensing sensors solutions in combination with traditional navigation sensors, recently some new systems have been proposed based on fusing remote sensing measurements with GPS-INS measurements to achieve comprehensive, fast and real-time systems (Sheta (2012)).The linearized observation equations and/or system equations of these new developed systems may be DEIV models (Mahboub et al. (2016) and Schaffrin and Iz (2008)).Hence, the classic EKF algorithm or its alternatives such as Unscented Kalman Filter (UKF) (Julier et al. (1995)) may not be useful.Schaffrin and Iz (2008) considered the case which observation equations are DEIV model and proposed a Kalman filter algorithm namely total Kalman filter (TKF).Then Schaffrin and Uzun (2011) tried to impose a data snooping procedure to the TKF algorithm.As the characteristic of the noise of different sensors is often arbitrary and different, the structure of the noise supposed by Schaffrin and Iz (2008) may not be useful.Therefore Mahboub et al. (2016) presented an applicable TKF algorithm with general weight matrixes and named it weight total Kalman filter (WTKF).Then Mahboub et al. (2017a) solved the problem which both of the coefficient matrix of the observation equations and system equations are corrupted by random noise and named it integrated Kalman filter (IKF).Eventually Mahboub et al. (2017b) developed a constrained integrated total Kalman filter (CITKF) as a solution to DEIV model since a quadratic constraint may appear the navigation problem.

Section 2: Dynamic model of integrated navigation
In this section we introduce the concepts of integrated navigation in terms of the Kalman filter algorithms.In recent years, integrated Navigation systems have started to be applied to mobile systems.They can provide a selfcontained autonomous navigation solution and can be used as an alternative (or an addition) to the traditional sensors (GPS, INS and integrated GPS/INS).By now, the dynamic model of integrated direct geo-referencing problem in integrated navigation has been traditionally defined as follows (see e. g.Sheta ( 2012)): (1) It is also assumed that the state vector is observed at an initial (previous) epoch: Equation ( 1) represents observation equations and equation (2) is system equations which is also called dynamic model.It relates the unknown parameters at an epoch  to an earlier epoch  − 1.In the above equations   is the m × 1 random observation vector,   is the m × 1 vector of observational noise,   is the linear/linearized design matrix of the observation equations,   is the n × 1 random parameter vector (time dependent unknowns) which contains 3-D position and attitude of the mobile sensors i. e.   = [           ],   is the n × n transition matrix and   is the random system noise,   is an independent time variable function,  −1 0 is the random noise at the first epoch and underlining ( ) indicates random variables.
Equation ( 2) is usually produced by INS data and equation ( 1) is usually provided by other sensors e. g.GPS and remote sensed sensor in an integrated navigation system.This arrangement can be changed due to available sensors in a system.
Equations ( 1) to (3) usually represent the functional model of the dynamic model for the problem.We also define the corresponding stochastic model as follows: where    ,   and Σ −1 0 , are the corresponding dispersion matrixes of the observation vector, the system equations and the observed unknown parameters at an initial epoch.
Traditionally, the so called extended Kalman filter (EKF) is applied to solve the above problem.
Nevertheless, we can argue that the real dynamic model of the problem should be given by the following equations since some random observed quantities may still exist in   which may be neglected such noisy control points.Moreover, if INS is used to produce system equations, one encounters with a dynamic errors-in-variables (DEIV) model (Mahboub et al (2017a).Furthermore, one may need to impose the following quadratic constraint since it is well known that the Quaternion approach is sometimes used to solve the singularity problems of the Euler angles at the 90 degrees angle.This constraint may also be because of the fixed distance between two sensors.Thus we have Here    and    are the corresponding random noise of the   and   ,  is a symmetric matrix and  0 is a nonnegative scalar.In such a case the EKF algorithm needs to be improved.In the next section we present the WTKF algorithm (Mahboub et al. (2016)) and explain how WTKF, IKF and CIKF algorithms deal with the above dynamic model for integrated navigation.

Section 3: The new family of Kalman filter algorithms for integrated navigation
The weighted total Kalman filter (WTKF) algorithm which was proposed by Mahboub et al (2016) can deal with equations ( 5) to (8) if they are simplified as follows i. e.  = 0 and    = 0: =    −1 +   +   (10) In contrast to Schaffrin and Iz (2008), here an arbitrary dispersion matrix can be considered for the random observed quantities as follows: Where  the fully correlated dispersion matrix of size ( + ) × ( + ) and the random error vector   of size ( + ) × 1 is given by: (   ) =   (14)  In the next section we numerically compare four algorithms WTKF, IKF, CIKF and EKF.

Section 3: numerical comparison of EKF algorithm with WTKF, IKF and CIKF algorithms
In this section, three comparisons are made.Due to restriction for publication IN THE ISPRS, we avoid presenting big numerical matrixes which correspond to equations ( 5) to (8).

Section 3-1: WTKF versus EKF
Suppose that the dynamic equation is given in a local frame as follows: The observation equations of this example which may be produced by GPS and remote sensed data is given by 5 DEIV models at 5 epochs  = 1,2,3,4,5.Note that the chosen time interval is 5 seconds i. e.  = [ 5 10 15 20 25 ].
We apply three algorithms EKF, TKF (Schaffrin and Iz (2008)) and WTKF proposed here to this problem.The results are illustrated in figure 4 and compared with the true solution.The proposed WTKF approach can make the best improvement i.e. approximately 25% improvement in the solution of the predicted position in contrast to other algorithms.In particular this situation can be seen when the magnitude of the weights of the elements in the random design matrix are large.Although the TKF algorithm of Schaffrin and Iz (2008) is mathematically correct and improvement of WTKF algorithm relating to TKF solution is not so high especially for rotation angles, it cannot completely satisfy the given weight of this problem.Therefore, its predicted position differs from the position predicted by the WTKF algorithm and the true solution.This difference is increased when the magnitude of the weights of the observed quantities are lager which prove the important role of the weight matrixes.Furthermore, in both of the EKF algorithm and the TKF algorithm of Schaffrin and IZ (2008), the magnitude of the bias will be increased at the later epochs.The other major point refers to the predicted position and attitude of the classic EKF algorithm.Not only its solution considerably differs from the true solution, but also it has an irregular treatment since this algorithm does not consider the random property of the design matrix within the DEIV model.Moreover, it can be demonstrated that both of the TKF algorithm proposed by Schaffrin and Iz (2008) and the WTKF algorithm give the same results in the the homoscedastic case.Also, if the coefficient matrix of the observation model has no random error, the results of three algorithms EKF, TKF and WTKF are the same.Note that all of the positions are in meters and angles are in radians.Now suppose that for an integrated geo-referencing of a mobile sensor, we are going to determine the position and attitude of a mobile sensor at five epochs.The system equations are produced by IMU.Due to equations ( 5) to ( 8), the components of the DEIV model of the system equations at these epochs are determined.
The observation equations which can be produced by GPS and remote sensed data are given by 5 DEIV models at 5 epochs  = 1,2,3,4,5.
Three algorithms EKF, TKF and IKF were applied to this direct geo-referencing problem which both of the observation equations and system equations are in fact DEIV models and compare the result with true solution.The results are shown in figures 5 and 6 for 3-D position and attitude of the mobile sensor in a local frame respectively.Note that after computing the attitudes in quaternion representation, we converted them into three rotations about three axis in degrees.The results given by the figures 5 and 6 demonstrated that the proposed IKF approach can improve the solution of the predicted position and attitude in contrast to other algorithms particularly when the magnitude of the weights of the elements in the random design matrixes   and   cannot be neglected.The improvement of the predicted position is more considerable than the predicted attitude.However, the TKF solution has larger difference respect to true solution than the ITKF solution since it does not consider the random property of the random design matrix   .This situation gets worse for the EKF solution in which not only we neglect the random property of the noisy design matrix   but also the random design matrix   is considered deterministic i. e. with no noise.Furthermore, an irregular treatment can be seen in the solution of the EKF approach in contrast to other methods.Another point of interest is that the general treatment of the TKF and IKF approach are similar, however, we can see a significant bias in the TKF solution respect to the IKF solution which is because of inappropriate modeling of the system equations made by the TKF approach.

Section 3-3: IKF versus CIKF
In some numerical examples we see that considering a quadratic constraint may improve the solution of the ITKF algorithm for integrated direct geo-referencing.This situation can be seen when we want to impose the quadratic constraints of Quaternion angles for instance.In other words if we want to use the quaternion angles  1 ,  2 ,  3 and  4 , it is well-known that the following quadratic condition must hold: The matrix  and the scalar  0 in quadratic constraint given by equation ( 8) can be obtained as follows due to equation ( 25): = [ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 1 0 0 0 1 ] ;  0 = 1; Now suppose that for an integrated geo-referencing of a mobile sensor, we are going to determine the position and attitude of a mobile sensor at five epochs.We applied the CIKF and IKF i. e. unconstraint algorithms to the above example.Figures 7 and 8 illustrate the results for 3-D position and attitude of a mobile sensor.As it can be seen from figure 5, the CIKF algorithm shows approximately 2 degrees improvement for attitude in contrast to the IKF approach since the CIKF algorithm considered the quadratic constraint given by equation ( 24).Nevertheless, figure 4 demonstrates that the results of two algorithms are the same for 3-D position since the quadratic constraint ( 24) is not relevant to position.Otherwise it can also improve the solutions to the position of the sensor.

Conclusions
Here we introduced and compared a new family of Kalman filters including WTKF, IKF and CIKF with the traditional EKF algorithms for integrated navigation.We made three comparisons.First we examined the WTKF algorithm versus EKF algorithm.Then the IKF algorithm versus WTKF algorithm was investigated.Eventually the CIKF and IKF algorithms were compared.The results demonstrate that the proposed WTKF approach can make improvement compared to the existing EKF algorithms since not only it considers the random coefficient matrix in the DEIV model but also a general structure for the stochastic model can be satisfied with this algorithm.Moreover, for a more appropriate modeling of an integrated direct geo-referencing problem, we found that the IKF algorithm should be used The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLII-4/W4, 2017 Tehran's Joint ISPRS Conferences of GI Research, SMPR and EOEC 2017, 7-10 October 2017, Tehran, Iran since it consider the noise of the design matrix   in the system equation.Furthermore, the CTKF algorithm shows approximately 2 degrees improvement for attitude in contrast to the IKF approach since the CIKF algorithm considered the quadratic constraint given by equation

Figure 1 :
Figure 1: WTKF algorithm Later on Mahboub et al (2017a) proposed integrated Kalman filter (IKF) algorithm in order to solve the problem given by equations (5) to (7) which both of the coefficient matrix of

Figure 4
Figure 4-a: the solution of different algorithms for 3-D position of the sensor in a local frame

Figure 5 :
Figure 5: solutions of different algorithms for 3-D position of the mobile sensor in a local frame

Figure 7 :
Figure 7: solutions of the CIKF and IKF algorithms for 3-D position of the mobile sensor in a local frame