Stanford Lecture on Multi-Sensor Fusion: How Kalman Filters Combine Camera, Radar, Lidar and GPS for Vehicle Perception

Prof. Daniel Watzenig opened by saying the lecture would be a preparation for a more hands-on session on Friday, which would use Autoware, a software stack he described as one of the most important around, to get the code into a car.

He said the lecture would cover how sensor fusion works, what the different taxonomies in sensor fusion are, and how the Kalman filter and its variants can be used to fuse data from different sources. He also planned a simple example and a more advanced one, and recommended two references, Gustafsson's work on statistical sensor fusion and Dan Simon's book on optimal state estimation.

Watzenig showed footage of real sensor data from a car in Austria. The first layer was a camera, a Mobileye unit. The long-range radar, a commercial Continental sensor, was added next, and then the lidar data from a standard Velodyne sensor with up to 16 layers.

He explained that fusion is not only about combining lidar, radar and camera for perception. Additional sources such as GPS and the inertial measurement unit (IMU) also matter. In a car he identified two kinds of fusion: perception, which uses the data shown in the video, and pose estimation, which determines where the vehicle is so it can drive. For pose estimation he said GPS, IMU and lidar could be used, a point he would return to later.

Watzenig said the sensors on a vehicle typically observe behavior up to about 250 to 300 m. Using infrastructure-mounted cameras and radar, he said the range might reach almost 800 m, or nearly one mile.

He discussed the drawbacks of using only one sensor: each has a limited range and field of view, accuracy in estimating distance can be an issue, and false positives or false negatives can occur. A false positive, for example, could make the system brake when there is nothing in front of it. These problems are the reason for considering multi-sensor perception.

He listed the expected benefits of several sensors: better classification, better accuracy in detecting objects, improved state estimation, greater robustness and increased availability, since a backup sensor remains if one fails. He also said infrastructure data could help a vehicle see around a corner or through the next object.

Watzenig said fusion can be considered through four taxonomies. The first concerns the data. Measurements are never perfect; he listed issues such as cross-sensor correlations, inconsistency, outliers and disparateness, and noted that data must be aligned in time so that the wrong data is not fused. He also described uncertainty as taking several forms, including vagueness, ambiguity, incompleteness and degree of belief, and gave an example in which a measured distance to the nearest car was about 35 m but the reliability of that number still needed to be assessed.

The second taxonomy concerns the level at which fusion happens: on raw signals, on pre-processed features, or on object lists, where objects have already been classified as, for example, a car, a truck or a cyclist.

He said that from an information-theoretic point of view, the lowest level, the raw data, should be fused because all the information is there.

Watzenig contrasted the theoretical preference with practical car manufacturing. In the supply chain, an original equipment manufacturer (OEM) works with Tier 1 suppliers, and it would be good to have a clear interface. If an OEM wants to switch its sensor fusion block from one supplier to another, such as from Magna to Denso, Continental or Bosch, a fusion block placed at a higher level is easier to exchange.

Watzenig said that the Bayesian law lets prior information, prior densities and measurements be brought together intuitively through probability density functions. Within this framework everything is treated as a random variable.

He contrasted this with maximum a posteriori (MAP) or maximum likelihood (ML) estimation. He said that once one computes those estimates, one is solving an optimization problem, which takes the analysis outside the Bayesian formulation, since it requires optimality conditions such as a vanishing first derivative.

The advantage he stressed is that a full posterior distribution, obtained by combining the prior with the measurement likelihood, allows any statistics to be calculated from it. He noted that the Kalman filter and its variants belong to this Bayesian framework.

Watzenig worked through the product of the two measurement distributions. He said the first part of the expression is independent of X, so it can be moved out, and the remaining constant part can be grouped into a constant C, leaving the well-known formulation of the fused result.

He then examined the mean and the variance of the final result. For two Gaussian sensors, with D1 the more precise sensor, he said the fused curve is narrower than either input, so the combined estimate is more accurate than the better sensor alone.

To explain how variances combine, Watzenig compared the expression to two resistances in parallel. The overall resistance R of a parallel circuit has the form R1R2/(R1+R2), and the same structure appears in the combined variance. He noted that the overall resistance is smaller than the smallest individual resistance, so with 1 kΩ and 2 kΩ resistors the result is below 1 kΩ, and the combined variance behaves the same way and is smaller than the best sensor's variance.

He emphasized that sensor modeling is important, saying that if the sensor model is poor, then a common filter does not make much sense.

Watzenig described the Kalman filter's belief in the state estimate as Gaussian, normally distributed around a mean and a variance. He said many variants exist because the assumptions do not always hold: real systems are rarely linear, and linearization can seriously affect the result. This is the reason the unscented Kalman filter exists, along with the particle filter, or sampling from the posterior with Markov chain Monte Carlo.

He stressed that the Kalman filter handles a single hypothesis. With one object one filter is enough; with two objects two filters are needed, and for n objects, n Kalman filters. For multi-peaked distributions, a particle filter or Markov chain Monte Carlo could help. He also noted that with several dynamic objects, such as two pedestrians and a car, it becomes difficult to determine which measurement belongs to which object.

Watzenig described the prediction step, which uses no measurements and propagates the state through the model. When new measurements arrive, the correction step checks whether the prediction matches them, using the Kalman gain to scale the measurements against the model. He said the gain is recalculated at every time step.

He stated the rule plainly: if the Kalman gain is close to one, the filter believes the measurements, and if it is close to zero, it believes the model. The result is a belief in the state, expressed as a mean and a covariance, which he said gives a good estimate of the current state, and this is the basis for the pose estimation used in the car.

Watzenig said pose estimation is safety critical, and that redundant GPS and lidar systems make sense because a vehicle should not drive autonomously without knowing its pose to a certain accuracy. The state vector includes position, velocity and acceleration, and the model is a single-track model, which he said is widely used and fine for longitudinal dynamics. For lateral control, he said tire models would be needed.

Each entry of the state is subject to noise, and TS is the sampling time. He said the noise is assumed to be independent and identically distributed, with a fixed covariance. He then set up two cases, the first with lidar and IMU only and the second with three sensors: lidar, GPS and IMU. He said the lidar was of low precision.

Watzenig said that by tuning the process noise Q and the measurement noise R, one can improve the convergence of the common filter. Small process-noise values indicate higher confidence in the model, while large values indicate lower confidence. The same holds for the sensor model and its noise. He cautioned that one might lose the physics of the system if one tunes too much, and that when there is real access to a system, the noise should be measured rather than guessed.

The example used synthetic data to which zero-mean Gaussian noise had been added. The filter ran in a two-stage recursive procedure of prediction and correction. He said the basic code, implemented in MATLAB and Python, would be shared.

Watzenig said the earlier results were not satisfying, so he added GPS to lidar and IMU. In answer to a question about why the velocity estimate showed no noise, he explained that velocity was not measured directly; it was a pure estimate from the single-track model, so no large noise was assumed for velocity.

He said that augmenting the filter with another sensor is easy: only the measurement equation changes, while the state equation stays much the same. In this example, GPS was assumed to have the lowest measurement uncertainty and was therefore treated as the best sensor, while lidar remained the worst. He mentioned that differential GPS (DGPS) is another variant.

Watzenig showed that the estimate, the red curve, lay very close to the GPS data, which raised the question of why a fused filter is needed when GPS looks good. His answer was that if GPS fails, the fused solution is hopefully better, and that the fused variance is by definition smaller than the variance of the best sensor, which in the example was GPS. He added that this holds subject to the choice of measurement variances.

Watzenig asked how bias could be handled in a sensor such as the IMU. He said real noise is often not Gaussian and may have fat tails, so the distribution might be closer to a chi-squared or Student-t distribution, which could be checked by many samples and a histogram fit.

His suggested approach was to estimate the bias along with the other parameters by augmenting the state vector, for example by adding the IMU bias as another row. He said that if the bias changes over time, the estimate can also give feedback that the sensor is degrading over time, which is useful for condition monitoring.

Watzenig said the innovation sequence, the difference between measurements and the sensor model, helps a great deal for error detection. By defining a threshold and monitoring it, one can get an idea whether a sensor has broken down or become unusable. The same monitoring can be applied to actuators, so the Kalman filter can be used for system or condition monitoring of the whole system.

He summarized that multi-sensor fusion offers the benefits discussed earlier, and that the four taxonomies, covering data, architecture, raw data versus object lists, and complementary, cooperative and competitive fusion, provide different ways to think about sensor fusion.