1
Introduction

1.1 Background and Motivation

With the rapid development of sensor technologies, and due to increased density in integrated circuits predicted by Moore's law, the autonomous vehicle has become a fruitful area blending robotics, automation, computer vision, and intelligent transportation technologies. It has been reported that traditional automobile companies and startups plan to get their autonomous driving systems ready in the 2020s [Ross, 2017].

The US Department of Transportation's National Highway Traffic Safety Administration (NHTSA) defined five levels of autonomous driving, from manual driving (level 0), to driver assistance (level 1), to fully autonomous driving (level 5) (https://www.sae.org/standards/content/j3016_202104). As an inspiring example, the Audi A8, launched in 2017, is claimed to be "the world's first production automobile conditional automated at level 3," according to Audi AG. Nevertheless, some pessimistic voices have emerged, claiming that fully autonomous cars will not be developed as quickly as expected or are even unlikely. One of the pacesetters in fully autonomous driving technologies, Waymo LLC, has received resident complaints due to conflicts in driving behaviors between humans and autonomous vehicles.

Although it is still a long way to level 5 autonomy, there is high demand for the development of autonomous vehicles so that tasks related to logistics, environmental cleanup, public security, and much more can be automated. Among all the functional blocks in autonomous vehicles, the navigation system plays an irreplaceable role since the vehicle needs to be literally "in motion" for any particular task. Multimodal perception and state estimation are two coadjutant modules for vehicle navigation. There have been extensive research outcomes on these two topics in autonomous vehicle navigation, but a few challenges still exist, motivated by which the in-depth studies in this book have been carried out:

• A modern pose estimation system contains multiple sensors to achieve accuracy and robustness. Appropriate sensor configurations, which combine the advantages of each sensor to benefit the whole estimation system, are distinct depending on the specific applications and requirements. Based on a particular sensor configuration, new theories and ideas are required for multi-sensor pose estimation, where states, measurements, and constraints are represented in a unified fusion framework.
• Due to the stealthiness of attacks, system operators usually cannot discover attacks in time, which may lead to severe economic damage and even the loss of human lives. Such incidents indicate that enhancing the security of the system is an urgent issue. Researchers have studied how we can securely estimate the state of a dynamical system from the controller's point of view based on a set of noisy and maliciously corrupted sensor measurements. In particular, researchers have focused on linear dynamical systems and have tried to understand how the system dynamics can be leveraged for security guarantees.

This book discusses the pose estimation problem for robotic mobility platforms using information from multiple sensors. The first part discusses different sensor configurations and introduces new sensor fusion algorithms and frameworks to minimize pose estimation errors. Those concepts and methods are extensively used in current state-of-the-art autonomous vehicles, and extensive experimental results have been provided to verify the algorithm performance on real robotic platforms. The second part focuses on the secure estimation problem in multi-sensor fusion, where attacks are considered and explicitly modeled in algorithm design. As this is a new topic that is at the primary stage of research, theoretical analysis and simulation results are shown in the related chapters.

1.2 Multimodal Pose Estimation for Vehicle Navigation

1.2.1 Multi-Senor Pose Estimation

Multi-sensor fusion is a typical solution where system dynamics, measurements, and constraints are fused consistently to increase estimation performance in terms of accuracy and robustness [Borges and Aldon, 2002, Ye et al., 2015, Teixeira et al., 2018]. Essentially, pose estimation can be considered as state estimation within a state space with a problem-dependent topological structure. Let us assume the following discrete state equation and output equation:

where , , denote the state, control input, and measurement, respectively; and are the state equation and output equation; and and represent process and measurement noise.

Filtering and optimization are two frequently used data fusion frameworks for pose estimation. Filtering approaches propagate state vectors with their joint probability distributions along with time. The Kalman filter models the state and noise as Gaussian, which is not suitable for non-Gaussian or multimodal distributions. The particle filter and its variants [Van Der Merwe et al., 2001, Nummiaro et al., 2003] have been proposed to deal with non-linear and non-Gaussian systems, and the computation load of updating particle states proliferates with the sample number. The optimization-based approaches retain historical measurement and estimation as a graph such that they can be used for bundle adjustment or simultaneous localization and mapping (SLAM) [Grisetti et al., 2010]. The two commonly used frameworks are elaborated here.

Filtering-Based Approaches As shown in related work [Janabi-Sharifi and Marey, 2010, Koval et al., 2015, Bloesch et al., 2017], filters provide a probabilistic solution on pose estimation, which can be divided into two steps. First, the "prediction" step predicts states without current measurement, according to the state equation

where denotes the conditional distribution, and specifically is obtained from (1.1). Then, the probability distribution of the update can be obtained in the "correction" step, based on the output equation

where is obtained from (1.2), and the constant denominator is

Optimization-Based Approaches Instead of using the filtering-based approaches, some other research [Leutenegger et al., 2015, Huang et al., 2017, Parisotto et al., 2018, Wang et al., 2018a] aims to minimize the user-defined cost function such that

where denotes the cost items to be considered; the information matrix indicates the degree of confidence in the corresponding measurement; and the error function measures the difference between the ideal and actual measurement.

1.2.2 Pose Estimation with Constraints

Constraints1 in pose estimation are helpful in increasing algorithm robustness and accuracy. For example, we may consider motion constraints (1.1), that limit the vehicle's pose change with time, and road constraints, which require the vehicle to stay on the road. Constraints in practical issues are mostly considered as soft to allow modeling errors and noise. We discuss constrained pose estimation from two perspectives.

Incorporating Constraints into Filtering Given the constraints , where is a constant vector, the augmented output equation can be obtained to incorporate the constraints into measurements [Mourikis and Roumeliotis, 2007, Simon, 2010, Boada et al., 2017, Ramezani et al., 2017, Yang et al., 2017a, Shen et al., 2017]:

where the covariance matrix of indicates the confidence in the soft constraints. With a prediction that remains the same, the correction step can be achieved by applying the augmented output equation.

In addition, we may first obtain the estimate without constraints and then project the unconstrained estimates toward the constraint states to get the final estimate

where is an operator indicating the difference between states and is a positive-definite weighting matrix. For linear systems under linear constraints, if , the ordinary vector subtraction is selected as , leading to analytical solutions. Numerical methods are required to generalize the projection method to non-linear systems or with non-linear constraints. For particle filters, particle weights can be adjusted to reduce the influence of estimation results that do not satisfy the constraints.

Incorporating Constraints into Optimization For hard constraints, the method of Lagrange multipliers can be used to construct the corresponding non-constrained optimization problem. For soft constraints, one naive but effective way is to add the penalty functions to the cost function , such that

where denotes the -th constraint to be considered; indicates the degree of confidence in the -th constraint. Examples of related work can be found in [Estrada et al., 2005, Levinson et al., 2007, Lu et al., 2017, Hoang et al.,...