The Linear Kalman Filter and the Extended Kalman Filter

Get Complete Project Material File(s) Now! »

Industrial context

Inertial Navigation Systems since the 1950’s

With the progress of transportation systems came the need of an ever increasing localisation accuracy. In this regard, the 1960s and the Apollo program were major milestones both on the software and the hardware sides with the emergence of the Kalman filter and the use of an inertial navigation system to guide the Saturn V rockets.
The Kalman filter [74] is a major tool in state estimation, which allows for unbiased sensor fusion minimum variance in the case of linear systems with Gaussian noises, ensuring powerful convergence properties. Its extension to non-linear cases, such as inertial navigation, known as the Extended Kalman Filter (EKF), became a de facto standard in navigation system up until today, although it is known to lose the optimality properties of the linear case in general.
Inertial Measurement Units (IMU) are navigation tools usually composed of several gyroscopes and accelerometers, measuring the body’s orientation and specific acceleration (ie without the gravity) re-spectively. Given an initial position, speed and orientation, they allow tracking the evolution of this navigation triplet through time. Since the estimation is based on a triple integration of measurements subject to noise, the output will drift with time, with an amplitude depending on the sensor’s quality. Therefore, inertial navigation progressed both by improving the inertial sensors, and by coupling them with other sensors, such as GNSS receivers (e.g., GPS antenna), to correct the accumulated drift. In this context, Safran, through its Electronics and Defense branch, is the No. 1 company in Europe and No. 3 worldwide for Inertial Navigation Systems (INS). It offers a range of IMUs of various accuracy, up until the Marine grade which needs almost no correction.
Historically, INS heavily relied on the EKF, fully aware of its shortcomings, because of its ease of use, and its practical reliability in a number of aerospace applications thanks to the accumulated experience. That is why Safran developed, in collaboration with Mines Paristech, a novel version of the EKF, namely the Invariant EKF (IEKF) based on the theory of Lie groups. Its theoretical and practical advantages were brought to light by the PhD thesis of Axel Barrau [9], in particular with high-grade IMUs, which serves as one of the starting points of this work.
The other motivation for this thesis is the recent evolution of the complimentary sensors INS are made of. Indeed, while GPS measurements have been at the heart of these systems for decades, the rise of applications needing localisation in places where the GPS is either unavailable or unreliable (e.g., tunnels and urban canyons) lead to the use of other complimentary sensors for local navigation. In particular, the fusion of inertia and vision, known as visual-inertial odometry or sometimes visual INS (VINS), recently became highly popular thanks to the decrease of inertial sensors’ cost. Lidar-inertial navigation is also on the rise. GPS and visual measurements fundamentally differ, as the first one is a global observation of the system at a single time, i.e., of a single state, while the second often represents a relative observation between the states at two different times, for example a displacement. In this context the EKF is not sufficient anymore, and this lead to the development of new algorithmic tools to deal with these systems, in particular the Multi-State Constrained Kalman Filter (MSCKF) [86] and smoothing [59,80]. Both are based on the same idea, keeping old states in the estimation to be able to handle the relative measurements.
For those reasons, the present work focuses on the extension of the invariant framework to smoothing, and the ensuing computational issues which might appear when dealing with high-grade IMUs which were disregarded up until now.

Safrantech’s experimental autonomous car

Inside Safrantech, Safran’s Research & Development center, an autonomous vehicle team was created roughly along the beginning of this thesis. They set up their own autonomous car prototype, illustrated on Figure 1.1, equipped with a number of navigation sensors, including a precise IMU. The creation of this prototype was an opportunity, both from the research and industrial points of view. Indeed, along with the hardware setting, the car prototype needs a navigation toolbox. It was decided to develop it mostly internally at Safrantech. Therefore, in order to effectively test and assess the interest of the different algorithms, this work also helped in the making of the toolbox, with the objective that it should be easily usable by Safran’s navigation teams. In turn, having this toolbox allowed the prototype to serve as a test bench for the various estimation algorithms considered throughout these three years.
Figure 1.1: Autonomous vehicle prototype developed at Safrantech, which was used in the experiments.

Sensor fusion: global vs. local

For centuries, navigation was mostly thought as a way to localise oneself in a global frame, that is, compared to an identified reference position, e.g., the North pole. To this end, star tracking devices and compasses have been fundamental tools, which are still used today. The emergence of Global Navigation Satellite Systems (GNSS), such as the GPS, was a revolution in this field, as it allows getting position information on almost the whole planet. In the same time, local navigation became increasingly popular, in particular in the automation field, which required precise relative position information in places where no global positioning was available, or was not accurate enough, such as in buildings or tunnels. The problem of localising itself in a map which is being created, known as Simultaneous Localisation and Mapping (SLAM), is now a major research domain.

 Inertial-aided navigation with global sensors

Navigation based on the fusion of inertial measurements and global observations have been used for decades in the guidance of spacecrafts, starting with the Apollo program during which the astronauts manually tracked stars to correct the IMU’s drift. If the IMU is precise enough, usually from tactical grade, it can even bring some global information by measuring the Earth’s rotation. A particular property of global navigation, is that the state gradually loses its dependency to the past ones, i.e., it tends to ”forget” where it comes from. This is intuitive when thinking about GPS measurements: if we have position information on ten minutes of navigation, knowing the first position becomes almost irrelevant. Mathematically, this comes from the assumption that the last GPS measurement received only depends on the current state, not the other ones, which is called the Markovian assumption and will be detailed in Section 4.1.2. This is illustrated on Figure 1.2(a).

Inertial-aided navigation with local perception

Local navigation can not forget its past
Navigation based on local sensors, such as cameras, or laser sensors (LiDar) is fundamentally different from the global case, because of two reasons:
• There is information which will remain unknown
• The past is never forgotten
Think of a building that is being discovered. Knowing that it lies in Paris or New-York has no impact on the fact that one is currently at the second floor. Therefore, there will be no way to discover where one is globally, or in which direction north is. The whole navigation is done with respect to the building’s entrance. A more radical example is the follwing: put a robot inside a cube with a target for it to see. Even if you rotate or move the cube, the robot will see the exact same thing. The problem is called symmetric (a more rigorous definition will be given in Section 5.2), as the cube’s position and orientation cannot be deduced from observations. This symmetry is at the heart of the problem, and drove a significant amount of work in this field [49]. In general, local navigation is thus carried out with respect to its initial state, that is, it is fixed and defines the reference frame in which the mobile will be located. For convenience, it is usually fixed at the origin. For instance, saying the robot is at point (2,1,0) at time t = 10s, means that it is 2 meters away along the first axis (say forward), and 1 meter along the second (say left), of its initial position. Using inertial sensors slightly tempers this, since it is sensitive to gravity and therefore helps finding the vertical, yet the global heading remains unknown. This naturally leads to the fact that the system never forgets its past. Indeed, knowing that the hall the system goes through is the building’s entrance is crucial in navigation, as it allows correcting some drift that the estimation suffered, a process called ”loop closing” [3]. This is illustrated on Figure 1.2(b).
Local navigation exists under various forms, depending on the sensors used and the chosen framework. Concerning the sensors, it ranges from mono sensor (usually visual or LiDar) to multi-sensor navigation, with or without an IMU. There are mainly two different frameworks, the ones explicitly estimating the local map through features, and those implicitly converting them into relative measurements of the state. Looking at Figure 1.2(b), the former would explicitly seek the orange cube, while the latter would turn the measurements of the cube from times 0 and 3 to a relative measurement between the corresponding states. Table 1.1 gives an overview of the zoology of methods
Note that the nomenclature between odometry and SLAM is not yet stabilised, as some papers relying on implicit relative measurements (here denoted as odometry) use the term SLAM, e.g., [94]. Some authors proposed simplified names, for instance in [78] the terms bundle-adjustment is used to cover visual-inertial odometry.
Nomenclature used in this thesis: To avoid confusion, SLAM is used as a general term to cover the whole local navigation framework. When more specific problems are addressed, the involved sensors will be given, and the methods will be either called pose-graph based or features based, depending on whether or not the features are explicitly estimated. Note that it will mostly be the former, and that this name comes from the notion of factor graph, tightly bound to SLAM, which will be detailed in Section 4.1.

The particular case of inertia-odometer fusion

The fusion of IMU and odometers does not perfectly follow the classification given above. Indeed, although it is a local navigation system with symmetries, it resembles much more the inertia-GPS fusion , as it is also based on unary measurements. This is due to the fact that it does not use perceptive sensors such as cameras or lasers.

Table of contents :

1 Introduction 
1.1 Highlights of the thesis
1.2 Points marquants de la th`ese
1.3 Industrial context
1.3.1 Inertial Navigation Systems since the 1950’s
1.3.2 Safrantech’s experimental autonomous car
1.4 Sensor fusion: global vs. local
1.4.1 Inertial-aided navigation with global sensors
1.4.2 Inertial-aided navigation with local perception
1.4.3 Keeping memory of the past in the presence of non-linearities
1.4.4 The smoothing framework, as studied in this thesis
1.5 Contributions and organisation of the thesis
1.5.1 Contributions
1.5.2 Organisation of the document
1.5.3 Publications
I From Invariant Filtering to Invariant Smoothing
2 Autonomous errors on Lie groups 
2.1 Mathematical preliminaries
2.1.1 Lie Groups
2.1.2 Uncertainties on Lie Groups
2.1.3 Uncertainties of group actions
2.2 The Lie groups of navigation
2.2.1 The group of 2D rotations SO(2)
2.2.2 The group of rotations SO(3)
2.2.3 The group of 2D direct spatial isometries SE(2)
2.2.4 The group of 3D direct spatial isometries SE(3)
2.2.5 The group of double direct spatial isometries SE2(3)
2.3 Group-affine dynamics and group actions lead to autonomous errors
2.4 Examples of exactly and imperfect group-affine dynamics in navigation
2.4.1 Attitude estimation
2.4.2 Pose estimation
2.4.3 Unbiased inertial navigation
2.4.4 Adding noise and the biases
2.5 Conclusion
3 Invariant Filtering and geometric constraints 
3.1 The Linear Kalman Filter and the Extended Kalman Filter
3.1.1 The linear Kalman filter and its optimality properties
3.1.2 The Extended Kalman Filter
3.2 The Invariant Extended Kalman Filter
3.2.1 Considered noisy systems
3.2.2 Invariant filtering equations, stability and consistency properties
3.3 Kalman filtering and geometric constraints
3.3.1 The Linear Kalman Filter may preserve side information
3.3.2 Non-linear case
3.4 Invariant Kalman filtering with geometric constraints
3.4.1 Geometric constraints
3.4.2 An example: equivariant constraints
3.4.3 Illustration in terms of attitude estimation
3.4.4 The L-IEKF naturally respects the constraints
3.4.5 Direct corollary for the R-IEKF
3.4.6 Graphical illustration of the theorem and discussion
3.5 Examples
3.5.1 Car position and heading estimation
3.5.2 Attitude estimation
3.5.3 Unbiased inertial navigation on flat-earth
3.6 Conclusion
4 Invariant smoothing
4.1 An introduction to the smoothing framework
4.1.1 From filtering to smoothing
4.1.2 Smoothing as a non-linear least-squares problem
4.1.3 Solving the optimisation with iterative methods
4.1.4 Smoothing inside a sliding window
4.1.5 Smoothing on manifold
4.2 Left-invariant smoothing
4.2.1 Considered systems
4.2.2 Factors definition
4.2.3 Final algorithm and benefits of the proposed approach
4.2.4 L-IS and the iterated IEKF
4.3 Application of L-IS to mobile robot localisation
4.3.1 Considered problem: robot localisation
4.3.2 Compared smoothing frameworks
4.3.3 Simulation results
4.3.4 Experimental setting
4.3.5 Experimental results
4.3.6 Discussion
4.3.7 Invariant Smoothing for biased inertial navigation: Extension for imperfect groupaffine
dynamics
4.4 IMU preintegration in the context of invariant smoothing
4.4.1 Classical preintegration theory
4.4.2 Preintegration as a group-affine property: summary of [17,18]
4.4.3 Defining the preintegrated IMU factors
4.4.4 Jacobians computation
4.4.5 Summary of the differences between preintegrated IMU factors
4.4.6 Simulation results in inertial navigation: L-IS vs IEKF
4.5 Other Invariant Smoothings: Right-Invariant and Hybrid Invariant Smoothing
4.5.1 Using the right-invariant parametrisation: Right-Invariant Smoothing
4.5.2 Considered systems
4.5.3 Linearised system of R-IS
4.5.4 An alternative to L-IS: Hybrid Invariant Smoothing
4.6 Conclusion
5 Invariant smoothing in the face of geometric constraints and unobservability related inconsistency 
5.1 Invariant Smoothing also respects geometric constraints
5.1.1 A 2D localisation illustrative example
5.1.2 Invariant Smoothing respects deterministic group affine dynamics
5.1.3 Conclusion
5.2 Unobservability from a smoothing point of view
5.2.1 Standard observability theory: The observability matrix and false observability
5.2.2 Unobservability seen through a smoothing update
5.2.3 The impact of the prior covariance illustrated on a toy example
5.2.4 Studying a reduced example for SLAM and inertia-aided pose-SLAM
5.2.5 Conclusion
5.3 Conclusion
6 Real-world experiments 
6.1 Experimental setup
6.1.1 Safrantech’s autonomous vehicle prototype
6.1.2 Experimental tracks
6.2 Inertia-GPS fusion experiment
6.2.1 Experiments using the actual IMU increments
6.2.2 Stronger biases could degrade performances
6.3 Fusing inertia and relative translations
6.3.1 Filtering vs Smoothing
6.3.2 Assessing the importance of the right-invariant parametrisation
6.4 Conclusions of the experiments
II Navigation with highly precise sensors
7 The shortcomings of standard smoothing with highly precise sensors 
7.1 Standard factor graph based smoothing through MAP
7.1.1 Main considered problem
7.1.2 Resolution of the linearised optimisation problem
7.2 The limits of factor graphs and MAP
7.2.1 Potential issue n1: computational complexity
7.2.2 Potential issue n2: ill-conditioned information matrix
7.3 A solution without matrix inversion: the robust batch solver
8 SC-BIFM : A new solver based on the Kalman smoother and stochastic cloning
8.1 The linear Kalman smoother as a least-squares solver
8.1.1 The standard Kalman smoother
8.1.2 The Backward Information Forward Marginal (BIFM) variant of the Kalman smoother100
8.1.3 Summary of the approach
8.2 Proposed method: the BIFM Kalman smoother with stochastic cloning
8.2.1 Stochastic cloning for filtering
8.2.2 Proposed “SC-BIFM” (stochastic cloning for smoothing)
8.2.3 Current limits of the proposed SC-BIFM
8.3 Generalisation to all linear least-squares
8.3.1 Dealing with spanning-trees of invertible factors
8.3.2 Regrouping factors and variables to propagate
8.4 Numerical illustration of the methodology
8.4.1 Ideal case numerical experiment
8.4.2 Numerical experiment with low noise
9 Real-world experiments 
9.1 Experimental setup
9.1.1 Considered models and parameters
9.1.2 Implementation
9.2 Experimental comparison of linear solvers illustrating the decay of standard methods
9.3 Experimental comparison of corresponding localisation algorithms
9.4 Conclusions regarding SC-BIFM
10 Invariant SC-BIFM : a non-linear Kalman smoother for precise navigation 
10.1 Information filter on Lie groups: exactly transposing the IEKF
10.2 Rauch-Tung-Striebel Kalman smoother on Lie groups
10.3 Adapting the Information form of Kalman filtering on Lie groups for BIFM
10.4 Numerical validation of non-linear BIFM
10.5 Conclusion
11 Conclusion of the thesis 
A Right-Invariant Smoothing factors
A.1 Prior factor
A.2 Group-affine factors
A.3 Measurement factors .

GET THE COMPLETE PROJECT