Watzenig Says Autonomous Cars Require Multi-Sensor Fusion for Safety

Watzenig表示自动驾驶汽车必须采用多传感器融合以确保安全 由于单体传感器存在快速漂移和环境失效的问题，自动驾驶汽车必须融合互补数据，以保障运行安全并将状态不确定性降至低于任何单个传感器的水平。 Daniel Watzenig教授介绍了自动驾驶汽车多传感器融合的基本原理、分类体系以及数学基础。在对比了原始数据与目标级架构并分析了ISO 26262标准下的功能安全要求后，该讲座通过解析推导一维高斯融合，展示了组合多噪声传感器如何严格降低不确定性。随后，Watzenig过渡到离散时间线性卡尔曼滤波器，探讨了其状态传播方程、预测-校正递推过程以及卡尔曼增益权重。通过结合激光雷达、IMU和GPS的仿真双传感器与三传感器系统，他展示了多传感器冗余如何解决IMU漂移和未观测状态的问题，最后介绍了状态增广和新息序列监测等高级运行策略，以减轻传感器偏差并检测硬件故障。 自动驾驶汽车的多传感器感知融合了互补的感知模态（例如雷达和摄像头），将联合状态误差协方差椭圆缩小至远低于任何单个传感器的不确定性水平。 (3:08) 在数学上，独立高斯测量值的概率融合能够确保融合后的方差严格低于精度最高的单个传感器，其规律类似于并联电阻。 (25:48) 单体IMU航位推算由于加速度偏差的双重积分，会在10到20秒内发生灾难性漂移，因此必须与GPS或激光雷达进行多传感器融合，以锚定车辆状态估计。 (46:54) 标准卡尔曼滤波器可以消除零均值随机高斯噪声，但若不进行显式校准或状态向量增广，则无法处理系统性传感器偏差。 (66:02)

In this Stanford AA274A guest lecture, Dr. Daniel Watzenig argues that robust autonomous vehicle perception requires multi-sensor fusion to overcome directional uncertainties, systematic biases, and hardware failure modes that single-sensor pipelines cannot resolve. Taxonomies and Architectures for Sensor Integration (9:26) Watzenig organizes multi-sensor fusion into four core taxonomies covering data properties, fusion levels (signal, feature, and object), operational classes (competitive, complementary, and cooperative), and hardware architectures. In production automotive manufacturing, original equipment manufacturers often favor object-level fusion because standardized object lists make it straightforward to exchange hardware blocks from Tier 1 suppliers like Bosch, Continental, Denso, or Magna, even though raw signal fusion theoretically preserves the most information. Mathematical Optimality of Multi-Sensor Gaussian Fusion (17:47) Using an analytical univariate Gaussian example, Watzenig proves that multiplying two independent normal likelihoods yields a joint posterior distribution whose variance is strictly lower than that of either constituent sensor. He likens this behavior to the equivalent resistance of parallel electrical resistors: because measurements provide alternate routes to estimate ground truth, total uncertainty contracts beneath the variance of the most precise individual sensor. This principle translates directly into dynamic state estimation using linear Kalman filters. In simulated vehicle tracking, integrating a noisy lidar, an IMU, and a GPS produces a position variance bounded below the GPS error variance alone. Watzenig cautions against treating the process covariance and measurement covariance matrices as arbitrary tuning knobs; instead, engineers should empirically profile sensor noise directly on test benches or road trials to avoid artificially masking underlying physics. Kinematic Dead-Reckoning Limits and Observability Autonomous vehicles cannot rely solely on IMU dead-reckoning for localization. Because an IMU measures linear acceleration, deriving position requires double integration, which causes sensor biases to accumulate quadratically. Watzenig notes that empirical testing shows pure IMU integration drifts significantly off course within 10 to 20 seconds, demanding external references such as GPS or lidar odometry. When evaluating a baseline estimator combining a noisy lidar with an IMU, the resulting velocity estimate appears smooth despite the absence of a direct velocity sensor like a wheel encoder or Doppler radar. Watzenig clarifies that velocity is an unmeasured internal state synthesized entirely through the discrete-time kinematic single-track model. While this insulates the channel from high-frequency sensor noise, it leaves it susceptible to steady-state offsets when measurements carry systematic errors. Handling Sensor Biases and System Faults (67:52) Standard linear Kalman filters are optimal for mitigating zero-mean Gaussian random noise, but they cannot inherently resolve systematic errors such as sensor bias. To correct for static or time-varying IMU drift without breaking the estimation framework, Watzenig recommends state vector augmentation, expanding the kinematic state vector to jointly estimate bias parameters alongside position, velocity, and acceleration. Similarly, non-white colored noise can be managed through state augmentation or by introducing pre-whitening shaping filters. Beyond continuous drift, the Kalman filter's innovation sequence—the residual difference between raw sensor observations and model-based predictions—serves as an automated health monitor. Comparing these residuals against fixed diagnostic thresholds allows an autonomous platform to rapidly detect hardware faults, sensor outages, and actuator failures before they compromise vehicle control. Watzenig concludes that multi-sensor redundancy is fundamentally an operational safety requirement rather than merely an exercise in variance reduction. While single-sensor or GPS-dependent stacks may function in mild testing environments like California, full autonomy demands architectures resilient to prolonged signal loss, severe weather, and tunnel travel across global operating domains.

客座教授Daniel Watzenig在斯坦福大学课程中指出，自动驾驶系统的多传感器融合不仅能通过贝叶斯和卡尔曼滤波在数学上实现方差缩小，更是系统应对硬件故障、传感器物理缺陷及恶劣环境的基础安全防线。 多传感器互补与方差缩小机制 (3:08) 单一车载传感器存在不可避免的局限：单目相机测量纵向绝对距离精度差，长距毫米波雷达角分辨率粗糙，且均受限于视场遮挡与天气干扰，容易产生误报或漏报。将具有各向异性噪声特征的异构传感器结合，能使状态估计的协方差椭圆在重叠视场内迅速收缩。Watzenig通过一维高斯分布相乘的解析推导证明，融合后的方差倒数等于各传感器方差倒数之和，类似于并联电路的总阻值计算，其结果严格小于任意单个最优传感器的方差。这意味着只要模型准确，引入额外独立观测必然能够降低整体估计的不确定性。 融合层级分类与硬件架构安全考量 (10:59) 传感器融合可从数据层级、功能关系及硬件拓扑三个维度进行解构。在处理层级上，原始信号层融合保留了最完整的信息量；目标层融合虽然丢弃了底层细节，但因标准化接口便于在不同一级供应商（如Bosch、Continental、Denso、Magna）间替换模块，深受整车厂青睐。在功能关系上，系统分为冗余表决的竞争型、优势互补的互补型以及如双目立体视觉般的协同型。Watzenig强调，在硬件拓扑层面，尽管集中式计算能提供全局最优观测，但为了满足汽车功能安全标准ISO 26262的故障运行（fail-operational）与故障静默（fail-silent）要求，工程部署更倾向于采用分布式或去中心化多核架构，以消除单点失效风险。 线性卡尔曼滤波在位姿估计中的工程实践 在基于单轨车辆模型的位姿估计中，卡尔曼增益动态权衡运动学预测与传感器测量。纯惯性测量单元（IMU）由于加速度的双重数值积分，其测量偏差随时间呈二次方累积，Watzenig指出实测中其航位推算在10至20秒内便会彻底漂移发散。引入低精度LiDAR和IMU的两传感器组合虽然能缩小估计方差，但在车辆静止阶段仍会出现显著稳态偏差；而进一步引入GPS（特别是厘米级精度的差分GPS）后，状态估计的方差不仅被严格限制在最优传感器误差之内，还能滤除高频抖动。在参数配置上，Watzenig告诫不可将过程噪声协方差Q和测量噪声协方差R当作单纯的平滑调谐旋钮，过度调整会脱离真实物理特性，测量协方差应通过台架或实车采样标定获取。 应对传感器偏差、非高斯噪声与系统故障 (67:52) 经典线性卡尔曼滤波本质上是基于单峰高斯分布的单假设追踪器，无法自行消除具有固定或缓慢漂移特性的系统性误差（偏差）。针对IMU零偏和厚尾分布等真实非理想噪声，Watzenig提出将未知偏差和噪声参数直接扩展到状态向量中进行联合估计，或采用预白化整形滤波器消除有色噪声。此外，测量残差（新息序列）不仅用于更新状态，还可作为在线状态监测与故障诊断的关键指标。一旦新息超出预设阈值，系统即可迅速捕获传感器失真或执行器故障，从而触发安全降级机制。 Watzenig总结指出，多传感器感知方案绝不能仅针对硅谷的晴好天气进行算法优化，工程设计必须覆盖阿尔卑斯山区长隧道等全球极端环境下的GNSS长时间丢失与严苛气候挑战，在系统层面建立起高容错与环境适应能力。

[51:41] Prof. Daniel Watzenig: You might lose the physics if you just tune too much in Q and R. [32:50] Prof. Daniel Watzenig: The Kalman filter is perfectly suited for doing fusion.

[0:05] Good morning everybody. So actually it's [0:07] a pleasure to be here. [0:09] And we will have let's say the [0:11] opportunity to have today's lecture on [0:13] Thursday and Friday. On Friday we'll be [0:15] a bit more of a hands-on session when it [0:18] comes to Autoware [0:19] which is meanwhile one of the most [0:21] important software stacks around to get [0:24] the code to your learning in in in that [0:26] lecture in principles of robot autonomy [0:28] one actually in the car. [0:31] But today we actually do a bit of a [0:32] let's say a preparation in terms of when [0:35] it comes to data fusion, multi-sensor [0:37] perception which is actually quite [0:40] important. You got all the ingredients [0:42] over the last couple of weeks. [0:45] Marco did a great job so far so you are [0:47] aware of a lot of filters and [0:49] algorithms. [0:50] Before we start so Boris already [0:51] mentioned where I'm from so I'm actually [0:53] from Europe from Austria. [0:56] Yeah. [0:57] Austria is a pretty small country. [0:59] I think the [1:00] the [1:01] let's say the [1:03] the most well-known person still alive [1:06] is I think has a good link to to [1:08] California or so Arnold Schwarzenegger. [1:10] He was two times governor or two periods [1:12] governor here in California. He actually [1:14] came or he's coming from Graz so not far [1:17] away where where the where the [1:18] universities where where [1:20] I'm currently [1:22] working and where I'm teaching. So [1:24] Arnold is of course he's [1:27] he's he's a different accent because I'm [1:29] from a different part of Austria so you [1:31] will not hear the accent Arnold is [1:34] usually having so it will be a bit [1:36] different. [1:36] >> [laughter] [1:38] >> But now back to work. Um [1:40] today multi-sensor perception and [1:43] data fusion. [1:44] We actually will go through how sensor [1:47] fusion works, what are the different [1:49] taxonomies in sensor fusion, how can we [1:51] use the Kalman filter and its variants [1:56] to do data fusion to fuse data from [1:58] different sources. [1:59] We um actually will go through a simple [2:02] example and a bit more advanced example [2:04] so that you get a feeling and you get to [2:06] know how you can use Kalman filtering [2:09] for pose estimation and what are the [2:11] let's say the pitfalls there. [2:14] Just um two books which I personally [2:17] think they are pretty good. The one is a [2:19] Fredrik Gustafsson, a Swedish professor. [2:21] Um he has a quite nice book on [2:23] statistical sensor fusion. It's a bit [2:25] more advanced book. [2:27] But basically it's giving you a broad [2:29] overview how to deal with let's say data [2:32] fusion, how to deal with all the issues [2:34] of temporal synchronization, um spatial [2:38] localization, which also is quite uh [2:40] quite important uh when it comes to real [2:42] data from a car. [2:44] And second one is from Dan Simon, which [2:47] is basically an optimal state [2:48] estimation. [2:50] Um it starts from the basic Bayesian [2:52] theory, goes over um to Kalman [2:55] filtering, extended Kalman filtering, [2:57] unscented Kalman filtering, particle [2:59] filtering, and H infinity filtering, [3:01] which is also quite quite a nice nice [3:03] algorithm. [3:04] So, those two uh ones are pretty good. [3:08] Let's start with let's say a nice video. [3:11] Uh first we see just the camera. Um [3:13] that's uh actually it's real data from [3:15] our car in in in Austria um using a [3:18] Mobileye camera. Now we see in addition [3:21] the radar sensor, long-range radar [3:23] sensor [3:24] um of a commercial one from Continental, [3:26] and in a in a few seconds we'll also see [3:28] the lidar data [3:29] of one of our um just a standard [3:32] Velodyne lidar sensor. [3:34] Here we go. [3:35] With up to 16 layers. And so that's [3:38] actually the data which we can use for [3:41] for fusion uh when it comes to the [3:43] perception. As you know, fusion it's not [3:46] only only a thing of uh fusion fusing [3:48] lidar uh with radar and camera. Uh we [3:52] also can think about additional sensors [3:54] or additional sources like like um GPS [3:57] or IMU, the inertial measurement unit, [4:00] uh which also are important for fusion. [4:02] And in the end, we have two kinds of [4:04] data fusion in a car. The one thing is [4:06] when it comes to the perception, [4:08] um then we use those data we just have [4:10] seen. And that's the one thing, and the [4:12] other thing, of course, is pose [4:13] estimation or estimating the position [4:16] because you're all aware [4:17] we need to exactly know where we are [4:19] before we we are allowed to drive. [4:21] Uh be um and that's why we can also [4:24] think about using GPS, IMU, and lidar [4:27] for pose estimation. We will come to [4:29] that a bit later on. [4:31] Yes, uh fusion does make sense because [4:34] in the end, uh our idea is to get [4:37] every single sensor comes to every every [4:40] measurement in general has certain [4:42] uncertainties. And our goal is, of [4:44] course, to decrease the uncertainty. And [4:47] if you look to the, let's say, the red [4:49] one and the blue one, [4:51] we'll see that they there is a different [4:52] behavior. That actually means uh for a [4:55] radar sensor, we have, let's say, this [4:57] kind of, let's say, um [5:00] uh field of view. [5:02] And that means just using the radar [5:03] would actually give us a very good [5:05] resolution or accuracy and and the set [5:09] direction, so in the direction of uh of [5:10] the distance, while the camera is a bit [5:13] different. And our idea is when we can [5:16] bring together those [5:18] red behavior and the blue behavior, we [5:20] hopefully end up in this, let's say, in [5:22] this cross section. And that actually [5:24] means [5:26] so the those [5:27] shapes are reflecting our certainty that [5:29] we can actually decrease the [5:31] uncertainty. [5:34] And let's say one of the, let's say, [5:35] very uh intuitive examples is that one [5:38] where long-range radar and camera is [5:40] used. We actually see here um the [5:43] estimate of the Kalman filter. We see [5:44] huge variance. And once you're entering [5:48] let's say the field uh where both [5:50] sensors are active and we do a proper [5:53] fusion, we see that the covariance is [5:55] just shrinking and we have a better [5:56] estimate because we are always [5:58] interested in getting a [5:59] very small variance. [6:02] And if you're just to the left or to the [6:04] right of the car, the variant the [6:06] variance increases because only one [6:07] sensor is taking care of that object. [6:12] And [6:16] Um [6:17] one additional motivation is we just [6:20] talked about the car itself, the ego [6:22] vehicle, [6:23] but actually a lot of people are [6:25] meanwhile working [6:27] to extend the field of view. That means [6:29] how to use stationary sensors [6:32] which are mounted actually in the [6:34] infrastructure to extend the field of [6:36] view. In here at the Highway 101, BMW is [6:39] currently [6:41] doing that. So there is a camera and the [6:42] radar installed. [6:44] Um you might figure out where it is when [6:45] you next time drive [6:48] around the 101, but you will see that [6:50] currently BMW is doing let's say [6:53] collecting measurement and doing some [6:55] industrial research [6:57] to figure out how a cut-in or emerging [6:59] scenario can be optimized or improved [7:02] when using stationary sensors. And the [7:04] idea is because usually we have these [7:06] these let's say behavior up to 250 m, [7:10] 300 m [7:11] where long-range radar and lidar are [7:13] operating. [7:15] But using let's say infrastructure-based [7:17] sensors, cameras, radar sensors, we [7:20] might end up almost with these kind of [7:23] same 800 m. So up to let's say almost 1 [7:26] 1 mile. [7:27] Which is pretty good. [7:29] Um just [7:32] figuring out [7:34] what drawbacks do we have if we just use [7:36] one sensor. Just keep in mind the video [7:38] we just have seen. We have a for a [7:40] range. We have a limited field of view, [7:43] um [7:44] performance has some issues, [7:46] um we could actually have some issues in [7:48] the accuracy in in estimating the [7:50] distance. Distance is pretty important [7:52] if you would like to figure out dynamic [7:54] objects and to do the right decisions in [7:57] terms of evading or braking. And we also [8:00] have this these so-called false [8:02] positives or false negatives. Those are [8:06] um very welcome because um you would [8:08] like to get rid of sensor false positive [8:11] and false negative. That actually means [8:13] false positive, um your system is [8:15] braking although there is nothing. That [8:17] means just uh your sensor uh perceives [8:20] or perceived some sort of an artifact [8:22] and um the system just made the wrong [8:24] decision. [8:26] So, that's [8:27] why we have to think beyond that, um to [8:29] think about multi-sensor perception, and [8:32] um how to compensate those issues. [8:35] And basically, what we expect when using [8:38] couple of sensors is that we [8:41] get a lot of plus points in terms of [8:44] better classification, uh better [8:46] accuracy uh in terms of detecting [8:48] objects, improved state estimation. [8:51] We'll talk about that uh during our [8:53] today's lecture. [8:54] Better robustness, [8:56] increased availability. That means if [8:58] one sensor breaks down, if you have [9:00] issues with one sensor, um there is [9:03] still a plan B or even a plan C. [9:05] And hopefully, like the example with the [9:07] infrastructure that we think about, [9:09] let's say up to 1 mile um of um [9:12] seeing, let's say sometimes people say [9:15] something like looking around the corner [9:17] or looking looking through the next [9:19] object. Um and that's why it makes sense [9:21] to um [9:23] to include uh infrastructure data. [9:26] When it comes to fusion, we actually can [9:29] distinguish between four different [9:32] classes or four different taxonomies. [9:35] The first one is the data related one. [9:37] Second one is the diffusion level [9:39] taxonomy, diffusion classes and [9:42] more on the hardware [9:44] point of view, the architectural [9:47] taxonomy. I will briefly give you an [9:49] idea what those four taxonomies [9:52] taxonomies are. First one, data related. [9:55] That's [9:56] the original idea of doing fusion. [9:59] That actually means we have when we get [10:02] measurements, when we have to work with [10:03] real data, we have a lot of data related [10:06] issues. [10:07] And that actually is we have imperfect [10:09] data. [10:11] There might be some correlations between [10:12] the data of different sensors, [10:14] inconsistency, disparateness. [10:16] Those kind of issues are in the data and [10:18] we have to figure out how can we get rid [10:20] of those? [10:22] It's for sure that data is always [10:24] uncertain and we have to do the best out [10:27] of it. That means we have to actually [10:29] think about how to deal just looking at [10:32] the next next layer when it comes to [10:34] inconsistency or [10:36] conflicting data, [10:38] outliers in data. So think about robust [10:40] estimation, but this order data. [10:43] So all of those kind of things could [10:44] happen. [10:46] Think about you have two different [10:47] sensors in your car. [10:49] The sensors are acquiring data, but you [10:52] have to figure out what is the what are [10:54] the timestamps of the data in order to [10:55] avoid that the wrong data is is used. [10:58] Um [10:59] So we we have to take care of those kind [11:02] of issues and that's actually the idea [11:03] of [11:04] of data related a fusion taxonomy. [11:08] Um because you have to just think about [11:09] if we talk about estimating the distance [11:12] or measuring the distance, [11:14] then of course the distance to the [11:15] nearest car is some 35 m in the metric [11:19] system, [11:20] but actually that's [11:22] not where [11:24] where we in measurement usually do a [11:26] full stop. We have to think about how [11:28] good is the data, what's the degree of [11:30] belief in the data, talking about the [11:32] uncertainty of the data, vagueness, [11:34] ambiguity, incompleteness. So, a lot of [11:36] things which could happen just [11:38] ambiguity, but because of deal with all [11:40] four [11:41] issues or even more issues. [11:44] So, that's the data related one. [11:46] The next one which might be basically [11:49] one which you already heard about is you [11:51] have different sensors and people are [11:53] usually talking about fusing raw data, [11:55] fusing [11:56] object data, object lists, and that's [11:59] what what is here about. Uh we could [12:01] actually fuse data on signal level, [12:03] um digital signal level. [12:05] Um we could already do some [12:07] pre-processing, [12:08] um and work out features and [12:10] characteristics and do the fusion on [12:12] that level. Or we already have basic [12:14] decisions made, uh that means we already [12:16] decided this is a car, this is a truck, [12:19] this is a bicyclist, and do the the [12:21] fusion on object level data. [12:23] And [12:25] what is the best one? Of course, from [12:27] the information theoretical point of [12:29] view, we always should fuse data on on [12:32] the lowest level, the raw data on the [12:33] signal, because there is all the [12:35] information in. [12:36] Um [12:37] from let's say from the practical point [12:39] of view, just think about um how cars [12:42] are manufactured. There is an OEM and [12:44] there are tiers, tier one, and actually [12:47] it would always be good to have an [12:48] interface. So, an OEM would like to [12:50] exchange the sensor fusion block, [12:53] go for I don't know, for for Magna, then [12:55] for Denso, then for Continental or [12:57] Bosch. That means having it on a higher [12:59] level is easier uh in terms of [13:01] exchangeability. [13:03] Next one is actually um [13:05] talking about, let's say, classes. And [13:07] here we have three classes. Um the first [13:10] one is competitive fusion, second one [13:13] complementary fusion, and last one is [13:14] the cooperative fusion. [13:16] Um just showing this picture. If you [13:18] talk about, let's say, the green the [13:20] green part, uh the competitive fusion, [13:22] we're just having two sensors [13:26] um measuring the same thing, redundant [13:28] sensors, and the idea is to be fail [13:30] operational. That means if one sensor [13:32] breaks down, then we actually can the [13:34] second sensor takes over. [13:36] Uh, we can do some majority voting at [13:37] the outside, something like that. That's [13:39] competitive fusion. [13:41] Uh, second one is complementary fusion. [13:44] We use [13:45] lay a lighter, for instance, and radar [13:47] for distance estimation. [13:49] On the the third one is [13:52] usually called emerging fuse is when we [13:54] go Let's say when one single sensor is [13:56] not able to do that, we need a second [13:58] source. [13:59] Um, for instance, if you have to do [14:00] triangulation, combining with stereo [14:02] vision. So, those those kind of classes [14:05] are existing, competitive fusion, [14:08] complementary fusion, and cooperative [14:09] fusion. [14:11] And the last part, as I've mentioned [14:12] right before, is a bit more related to [14:14] the hardware, to the implementation [14:16] later on. We typically talk about the [14:18] centralized version, the decentralized [14:20] version, and the distributed version, [14:23] which means [14:24] if you just think about it, could be [14:26] that you just look for it as at the [14:27] decentralized architecture, that we have [14:29] different control units or at least [14:30] different cores behind. That means we [14:33] have a kind of, let's say, [14:34] um, [14:35] fail silent behavior there. [14:38] Um, [14:39] and [14:40] in fact, there are of course pros and [14:42] cons for all solutions, but of course [14:44] the, let's say, the decentralized or [14:47] distributed architectures, they are just [14:49] looking in terms of fail operational [14:50] behavior, um, just being [14:54] or let's say, compliant to the ISO [14:56] 26262, the functional safety norm, it [14:59] makes sense to have something like the [15:01] the distributed architecture in place. [15:06] Right. So, that's more or less the [15:08] overview of, um, of fusion and the [15:10] different taxonomies. [15:12] And one of the important [15:14] things [15:16] which you already heard throughout the [15:19] course, Marco, I think, gave you a lot [15:21] of motivation why actually using [15:24] Bayesian law [15:25] in order to solve those kind of issues. [15:28] You mean by now that use using base law [15:30] is is a great thing because it gives you [15:33] the opportunity [15:34] to [15:36] intuitively bring together prior [15:38] information, prior densities, and and [15:41] actually measurements in terms of [15:43] dealing with probability density [15:44] functions. [15:46] That means within the Bayesian framework [15:48] everything is treated as a random [15:49] variable. [15:51] And as soon as you're let's say [15:55] leaving that concept of random [15:57] variables, you're leaving the concept of [15:59] of the let's say the Bayesian [16:01] formulation that whenever you're [16:02] calculating a maximum a posteriori [16:04] estimate or a maximum likelihood [16:06] estimate, that's actually a optimization [16:09] problem, maximum something. [16:11] That means then you're already out of [16:13] the Bayesian formulation because then [16:15] you're actually looking for for an [16:18] optimized solution. That means [16:20] um complying to the optimality [16:23] conditions, [16:24] first derivative has to vanish, second [16:26] one should be greater [16:28] or [16:30] uh less than zero for a maximum or [16:31] minimum. Um that's an optimization [16:33] problem. Then we're already let's say [16:35] out of the Bayesian race. [16:37] Uh as long as we are dealing or treating [16:39] everything as random variables, then we [16:42] are [16:43] um in this [16:44] let's say framework. And the good thing [16:46] is [16:48] it allows us to calculate any [16:50] statistics. So, if you really get the [16:51] posterior distribution out of let's say [16:54] the combination of the prior [16:55] distribution and [16:57] the likelihood, the measurement [16:58] likelihood, we get [17:00] posterior distribution. And as you know, [17:03] dealing with distributions, we can work [17:05] out as I said any mode like the the [17:07] maximum likelihood or the conditional [17:09] mean. We could uh work out marginal [17:11] densities. So, we can get a lot of more [17:15] in terms of confidence, is degree of [17:17] belief, then when just running in [17:19] classical, let's say Gauss-Newton type [17:21] optimization, straightforward. [17:25] Right. [17:27] So, that's basically um the motivation [17:30] and you know the the common filter and [17:33] it's [17:34] variants are belonging um let's say to [17:37] the Bayesian framework and [17:40] in order to continue with let's say the [17:43] sensor data fusion, let's think about [17:45] the very simple example. [17:47] Um [17:48] This very simple example [17:50] is [17:52] basically [17:53] um like this. We Let us consider two [17:56] sensors. [17:57] And we have let's say an object around [18:00] us uh and would like to work out the [18:02] distance to those objects. Uh and we [18:05] have two sensors. [18:08] First, let us assume that both sensors [18:10] have the same field of view. [18:12] Um [18:13] We secondly assume that the first sensor [18:17] is just better than the second one. So, [18:18] we have a spend a bit of more money um [18:20] for the first sensor, the higher [18:22] precision, and uh the second sensor is [18:25] just a cheap one [18:26] um or an older version um that is in [18:29] place. So, [18:31] that's it. And finally, for this very [18:34] simple example to show you how fusion [18:36] works, um let's consider that we have n [18:40] equals one, that means just one object [18:42] around us. And now question is how to [18:46] fuse these measurements in a proper [18:49] sense. [18:52] If we [18:53] just think about So, in the end, we are [18:55] talking about [18:56] Gaussian distributed [18:59] measurements. So, d1 of x and d2 of x [19:02] are uh assumed to be Gaussian. Um that's [19:05] the classical formulation of a Gaussian [19:07] distribution as you know it. Um we have [19:10] in the exponent um our x minus the mean [19:14] the first measurement scaled by the [19:15] variance and for the second measurement [19:17] we have mu two [19:19] so the mean of the second measurement [19:21] and also scaled by the variance of the [19:23] second sensor. [19:25] And we are normally distributed that [19:27] means we are we have a mu one sigma one [19:30] squared so the variance one and the [19:33] variance two for for the second for the [19:35] second measurement and we assume we have [19:38] univariate Gaussian distributions. [19:41] Normally distributed so that's that's [19:43] our basic assumption. [19:45] And if we [19:47] now [19:48] um just [19:51] just recall that we more or less [19:54] decided that [19:55] that our sensor one is better than the [19:57] sensor two so we have this assumption so [19:59] we have to keep that one in mind and [20:01] actually would like to see if we fuse [20:02] that data [20:04] what is actually the outcome is the [20:05] variance then close to sigma one or is [20:08] it even better than than sigma one or [20:12] what's going on? [20:13] And [20:15] our problem formulation is now figure [20:17] out the distance DX given that we have [20:19] two measurements from two different [20:21] sensors with the same field of view [20:24] but with [20:25] different [20:26] let's say different uncertainties [20:28] connected or attached to those sensors [20:31] and if you just [20:33] do this first basic formulation we [20:35] actually see [20:37] we have to deal with how to [20:39] combine multi [20:41] how to combine Gaussian distributions so [20:42] we have to deal with Gaussian [20:43] distributions just do the product of D1X [20:46] and D2X [20:48] what we're doing is just combining those [20:50] two ones uh [20:52] uh as a product and we end up of course [20:54] the scaling factor which is independent [20:56] of X but the variances are going in [20:58] there. [20:59] And we have in the exponent uh first [21:02] sensor and second sensor so that's let's [21:04] say Um, not very very difficult so far. [21:07] Um, [21:08] that's let's say our first [21:11] first approach. [21:14] Pretty straightforward. [21:16] So, let's think about um, the exponent. [21:21] Um, [21:23] just just let's say looking a bit deeper [21:26] in in that one and it's a bit of mass. [21:28] Um, [21:29] if we start [21:30] we could do that on on the on the [21:31] whiteboard, but I think it's it's just [21:33] basic mass. That's why I'm just um, [21:36] giving you a few hints, but it's it's [21:37] not very very um, [21:39] very complex. So, that's our exponent [21:42] and what we're actually doing is just [21:44] multiplying that exponent out. So, we [21:46] have the let's see x minus mu one uh, [21:49] and bracket squared and the same thing [21:51] here and if you just multiply everything [21:53] out, [21:54] um, we actually [21:57] um, [21:58] get this expression and we introduce a [22:00] bit of a trick. So, we we just divide [22:03] numerator and denominator by the sum of [22:06] the variances of the two sensors. [22:10] That's why we have um, [22:11] sigma one as a [22:13] variance sensor one and variance sensor [22:14] two right in here and right in here. So, [22:17] that's the division. So, we're not So, [22:19] we're not adding new information. So, [22:20] it's just a mathematical trick and we [22:22] end up in in that formulation. [22:25] And [22:27] in the end we would like to find [22:28] something [22:30] in in our final expression which reminds [22:32] us then just going back which reminds us [22:35] in that formulation. [22:37] That's why we have to find let's say a a [22:39] way to express our fused data in terms [22:42] of this expression and that's actually [22:44] what we're doing right right here is [22:47] let's find the way to to get this let's [22:50] say final fused information in in that [22:52] way that we have x minus mu squared. [22:56] And it's again pretty standard [22:59] mathematical trick. We just do the [23:02] following. We have to [23:04] Just looking at this expression, we have [23:06] this x squared here. Uh there is already [23:08] an x here, so that looks like the mixed [23:10] term in in between. [23:12] Um so, actually that one. [23:15] So, right here. And what's missing is is [23:18] that term. And what we're usually doing [23:20] in maths is just add something and and [23:23] also subtract something so that we're [23:24] not adding additional information. And [23:27] to and more or less to work out that we [23:28] get something which is then in the form [23:30] of x minus mu squared. So, that's what [23:33] we're going to do here. [23:35] And basically, that's exactly that step. [23:39] We are just adding a term and we're [23:41] subtracting the same term again. So, no [23:43] new information added. [23:45] And we're now in this let's say in this [23:47] form uh we had before um right here that [23:50] we can actually [23:52] um just look at this first part, the [23:54] first three terms, and bring them [23:57] together [23:58] in x minus mu squared. So, that's the [24:00] first step. Of course, we have this [24:02] minus mu squared to write here. That's [24:04] over there. And the last part, uh let's [24:08] just substitute it by s because just [24:10] looking here, there is no no x in there, [24:12] so it's independent of x. So, we just [24:14] can [24:15] substitute it by [24:17] whatever [24:18] uh variable. Just decided to go for s. [24:21] And let's find an expression for this [24:24] term, and we just call it sigma squared. [24:27] Because then you can already see we have [24:28] some something which looks like um what [24:31] we usually would expect for a for the [24:33] exponent of the Gaussian distribution. [24:36] Next step is just to rearrange a bit. We [24:38] have exactly that term right here, and [24:40] plus the mu squared plus s. [24:43] Um one and a half over there goes in [24:46] there in this plus term. So, we get this [24:48] this information [24:50] right here. [24:52] Looks good. Final step basically is [24:55] bringing everything together, putting [24:56] everything together. and what we [24:58] actually see here is [25:00] just going back to this first [25:02] expression. [25:04] So, right here, we just So, sorry. Uh [25:07] just coming back to exactly that [25:09] expression. Um and [25:12] So, [25:13] and we are right here, bringing [25:14] everything together. Here is the Let's [25:17] see our expression we just worked out. [25:20] We um [25:22] could actually split that because the [25:24] first part [25:25] is again independent of X, so we can [25:28] move it out. Um we end up with this very [25:30] well-known formulation over there. [25:32] Um [25:33] the final step is just to say this is a [25:35] constant part uh [25:37] regarding regarding with respect to X, [25:40] that one as well. So, we just put [25:41] capital C in there, and we finally end [25:43] up with that expression. [25:48] Right. Nice thing, [25:50] but [25:53] what should we do with it? [25:54] Um [25:56] We actually [25:59] kept um [26:00] just summarize it. First thing is in the [26:02] end we have now some sort of a data [26:04] fusion done. [26:06] Um and let's have a closer look to mu, [26:08] to the mean, and to um to the variance [26:12] sigma of our final result. Um what you [26:15] see here is basically [26:17] the [26:18] um [26:20] the conclusion of doing data fusion um [26:23] when combining So, for this special [26:24] example of Gaussian distributions, [26:27] that we actually [26:29] um can see the following result. [26:32] Look here. Or what What's that? [26:35] D1 was our first sensor, um D2, the red [26:38] curve, our second sensor. [26:40] Uh we assume that D1 is more precise, so [26:43] it's better than D2. You could easily [26:45] see that the the variance, so the [26:48] the width of the peak is much smaller [26:50] than the red one. So, that the sensor is [26:52] more accurate than the other than than [26:54] the red one. [26:55] Uh that was our basic assumption. And [26:57] after doing the fusion, so D1X combining [27:01] with D2X, so doing the fusion, you [27:04] actually end up uh with the yellow [27:06] curve, which is basically [27:08] uh even better than the blue one. So, [27:11] actually uh we end up in a better [27:13] result, so in a more accurate result [27:16] than by just using the first sensor with [27:18] the high precision. So, even for um [27:21] for that case, we can actually show the [27:23] data fusion is of is of great value [27:25] because in the end the overall [27:27] uncertainty decreases. That actually [27:30] means [27:31] um [27:32] we [27:34] we do a different weighting, so the [27:35] weighting by variances because you could [27:37] actually figure out [27:39] the [27:40] let's say the the simplest [27:43] way of doing data fusion [27:45] um is just working on the arithmetic [27:47] m- so, on the mean arithmetic mean. That [27:50] actually means if you have something [27:51] like, let's say a result [27:54] with our two two measurements um [27:57] D1 plus [28:01] So, for a scalar case, um we just assume [28:03] we're just measuring the distance from [28:05] right from my position to the wall, and [28:07] we get um two measurements, D1 and D2, [28:11] and usually um [28:12] in in measurement techniques what you're [28:14] doing, you're just building the mean [28:16] actually. You're just going for for for [28:18] the mean operation. [28:20] And you're just doing [28:22] D1 plus D2 and [28:24] hopefully we get and even if you go for [28:27] of course they are just two samples. [28:29] Uh usually you should sample me 10 times [28:31] or 20 times. Or the same thing if you [28:33] work out the temperature of the room [28:35] here, um we just measure 10 times and um [28:38] then do the mean over those uh n n [28:41] equals 10 samples. And you can actually [28:43] see that's the the easiest way of doing [28:45] fusion because what we're doing here is [28:47] we fuse [28:52] actually D1 and D2. It's pretty much the [28:54] same what we did over there. [28:56] Um the only difference is there is a [28:58] let's say a very not very smart fusion [29:00] because we're just doing the same [29:02] waiting. Um there is no variance in [29:04] there. It's just the um [29:06] let's say doing the fusion of the means. [29:09] And that actually is is a nice thing, [29:11] but we have if D1 has some issues or D2 [29:14] has some issues, it goes in the same way [29:16] and so we are very in that case very [29:18] non-robust because um and that's the the [29:21] thing when you're working with means um [29:23] even data outliers uh so poor data is [29:27] weighted the same way than the good data [29:30] and that's why the mean doesn't make [29:32] sense that much um if you're just uh [29:35] working out measurements. And usually, [29:37] but I think you're aware of that, [29:40] makes sense instead of using the mean to [29:43] go for the median. Uh it's the first and [29:46] easiest way of doing robust estimation [29:49] because the median is able to treat [29:50] outliers and uh that's pretty good. [29:53] Because if you just work out [29:56] let's say an outlier, um the [30:00] the mean is just not penalizing outliers [30:02] while the median is doing that. [30:05] So that's basically the simplest the [30:06] simplest way of doing data fusion and if [30:08] we now look back to our to our result, [30:11] we see that the variances are going in [30:13] there and um the weighting of the [30:15] different sensors of different data is [30:17] done in a way how the variances are and [30:19] that's the most important conclusion of [30:21] that that um [30:24] the variances are quite important to [30:26] treat. And if you look at the final [30:28] result, um [30:30] of [30:33] um [30:35] of our let's say of our um [30:39] worked out [30:41] um [30:42] let's say, [30:43] variance and standard deviation. It's [30:45] actually comparable to think you have, [30:47] let's say, a lot of basic education in [30:50] electrical engineering. [30:51] Um, it's pretty much the same if you're [30:53] going for [30:55] for a parallel circuit, which is [30:57] something like [31:01] So, we just assume that we have [31:04] resistance one and resistance two. So, [31:06] we have some sort of a parallel circuit. [31:10] Some driving voltage [31:13] and [31:15] some electric current. And if you now [31:17] would like to work out [31:20] just looking from from that the notes [31:22] inside the [31:24] the sum, or let's say, the the overall [31:26] resistance, it's a parallel version. [31:28] That means we have R1 parallel R2, and [31:31] that actually is [31:33] 1 over [31:35] 1 plus 1 over [31:37] So, I'll just [31:39] do it in the American way, 1 over [31:43] R2 in order to avoid conflicts with the [31:45] with them. Um, [31:48] and we actually end up [31:50] exactly that way. It looked like like [31:52] the, uh, the variance over there, [31:55] um, but we end up in R [31:57] 2 [31:59] R1 plus R2. And the same thing happened. [32:03] So. [32:04] And we all know that [32:07] the [32:09] the overall resistance in a parallel [32:11] circuit, [32:12] uh, is smaller than the smallest [32:14] resistance in that network. That means [32:16] if R1 [32:18] is [32:20] R1 is 1K, [32:23] R1 is 1K, [32:25] and R2 [32:26] is 2K's, [32:28] then we actually would expect that the [32:30] overall resistance R is smaller than 1K. [32:33] And that happens to the variance [32:35] expression pretty much the same, but you [32:37] have the best sensor has variance sigma [32:40] one then the overall variance of the [32:43] fusion is smaller than that one. [32:45] Which is a pretty cool result. [32:50] Right. So that's basically our intro [32:53] example. Now let's go for a bit more [32:56] more advanced one. [32:58] Um [32:59] let's go back [33:01] what you already heard over the last [33:02] last weeks the common filter. [33:05] Um common filter actually is [33:10] It's a [33:11] Just a second. [33:14] Here we go. [33:16] The common filter is perfectly suited [33:18] for doing fusion. Um [33:21] Before running the common filter um just [33:24] let's do a bit of a recall what you have [33:26] already heard. [33:27] First thing is running the common filter [33:29] implies a couple of assumptions. [33:32] Uh the first one is [33:35] we have to deal with the linear [33:36] dynamics. [33:38] And we would like so just looking at our [33:41] dynamic system XT which [33:45] um let's say is driven by the dynamics [33:49] of the system AT. We have let's see a [33:51] control input UT scaled by some capital [33:54] BT and we have some noise attached to [33:57] it. [33:58] Uh we assume that this noise is [34:00] independent and identically distributed [34:04] around let's say zero mean and with a [34:07] certain covariance covariance RT [34:10] which might also depend over T and might [34:13] not be constant um over the different [34:16] time steps. [34:18] And that actually implies [34:21] that we deal with Gaussian distributions [34:23] because for linear systems if we if you [34:25] have linear systems you know uh Gaussian [34:28] distributed [34:30] signals running through a linear system [34:32] are still Gaussian distributed. [34:34] It only gets critical if you have [34:36] nonlinear systems. And we actually just [34:38] look at this. Um the probability of um [34:42] our state XT conditioned on the control [34:45] input and our previous state is actually [34:48] the Gaussian distribution again with our [34:50] scaling factor where let's say our [34:52] process noise uh goes in and we have in [34:55] the exponent [34:57] what we actually see right here. So, we [34:59] just have to rearrange it and we get the [35:01] quadratic expression. So, in terms of [35:04] matrix this uh matrix expression, but we [35:07] are still quadratic here right in there. [35:12] That's assumption number one. For [35:14] assumption number two, um let's have a [35:16] look at the at the the measurement [35:18] equation or the measurement model, which [35:20] is actually the sensor model. So, um [35:22] when we talk about the measurement [35:24] equation, that's actually where we have [35:26] to model the sensor, where we have to [35:27] find the lidar model, where we have to [35:29] find the radar model or a camera model. [35:31] So, that's going in there. [35:33] And we again assume a linear behavior um [35:37] and [35:38] we also are aware that we have some [35:40] measurement noise, some uncertainties [35:42] attached to the measurements, which are [35:44] um expressed by delta. Uh again, we [35:47] assume zero mean Gaussian distribution [35:50] uh identically distributed and [35:51] independent with a certain measurement [35:54] covariance [35:55] uh QT. [35:57] And our measurement set T conditioned on [36:02] uh our state XT are exactly that [36:05] expression, which directly comes out [36:07] from let's say our measurements and here [36:09] is the sensor model or measurement [36:10] model. And that's that's actually one of [36:12] the things as you already know, which [36:14] are quite important and that's the [36:16] original idea of Kalman um that Kalman [36:19] more or less had the idea, let's think [36:22] about this so-called he called it [36:24] innovation sequence, [36:26] uh which actually means the let's say [36:27] difference between my measurements and [36:30] my model, and how to, let's say, how to [36:33] make sure, [36:34] do I believe more in my measurements in [36:36] that time stand, time instant, or do I [36:39] believe more in the model? [36:42] And that's one of the basic assumptions [36:44] that we have a model in there, and [36:47] that's also a bit of, let's say, the [36:48] usual brain work we have to spend to [36:51] find a proper C, a proper model for for [36:55] for for a sensor. Because if the the [36:57] sensor model is poor, then of course [37:00] common filter doesn't make sense that [37:02] much. The same thing for the [37:03] measurements are poor, because then you [37:06] always believe either the measurements [37:08] or [37:09] or the model. And the the, let's say, [37:12] the work we have to spend here is very [37:15] important to find a proper model in [37:16] there. [37:17] But that's basically assumption number [37:19] two. [37:20] Our [37:21] um [37:23] let's say, assumption that our sensor [37:25] model, our measurement equation, is [37:27] linear. Assumption number three three is [37:30] basically [37:32] what about the starting, the initial [37:34] state X zero, and the belief [37:38] of X zero, [37:39] which is basically can be described by [37:41] the probability of the initial state, [37:44] and [37:45] we also [37:46] assume [37:48] that this initial belief [37:51] is also Gaussian distributed [37:53] with zero mean. [37:56] And all of the first three, let's say, [37:59] or those three assumptions, they [38:01] basically ensure [38:04] that the posterior distribution, so the [38:05] a posteriori distribution, [38:08] so the belief in our state estimate XT [38:12] is actually Gaussian [38:14] and normally distributed around [38:18] a mean mu T and [38:20] with a variance sigma T. [38:23] So that's the the basic thing and of [38:25] course there exists a lot of variants of [38:27] the common filter because not always [38:29] those assumptions are valid. [38:32] Uh linear systems are usually [38:35] not happening in reality. [38:37] Um first thing is always to start with [38:38] some kind of linearization. You could [38:40] actually work out that linearization has [38:43] a huge impact in data fusion. So, if the [38:45] linearization is done if you have a very [38:47] let's say non-linear system, [38:49] linearization uh could also um end up in [38:52] a mess. [38:53] That's why uh the unscented Kalman [38:56] filter exists um or the particle filter [38:58] or you just sample um from from the [39:01] posterior using Markov chain Monte Carlo [39:03] whatever. [39:08] Right. And one of the important things [39:09] to mention once again, I think Marcus [39:11] has definitely said it um the Kalman [39:14] filter is able to deal with let's say [39:17] single hypothesis. That means [39:20] if you have one object, we are fine. If [39:22] you have two objects, we actually need [39:24] two Kalman filters. For n objects, we [39:26] need n Kalman filters because um the [39:29] Kalman filter is only able to deal with [39:32] um [39:33] unimodal densities. That means like a [39:35] Gaussian distribution. Once you have two [39:37] peaks in your distribution or even more [39:39] peaks, um you need for every single [39:42] peak, you need let's say a mode [39:43] tracking. That means for n peaks, you [39:45] need n Kalman filters. [39:47] And there you have to find different [39:50] ways. [39:51] For instance, as I mentioned right [39:52] before, particle filter filtering could [39:54] help here or or even um [39:57] Markov chain Monte Carlo. But that's an [39:59] important thing [40:00] to um to to consider. Single hypothesis [40:04] filtering, that's the Kalman filter. [40:05] That's it. And we all know that for a [40:08] real car, we have to deal with a lot of [40:10] dynamic objects and that makes things a [40:13] bit more difficult. And you could also [40:15] imagine um that [40:18] if there are two pedestrians, one car, [40:20] so three objects in front of us, [40:23] how to work out which measurement is [40:26] associated which which object. So, the [40:29] the data association problem gets [40:31] critical then. [40:33] Um that means before just showing a [40:35] second example using the common filter [40:37] for both estimation, [40:39] um just have to be aware that's not that [40:42] simple in reality because just having [40:44] the sensor data and running the common [40:46] filter is a straightforward approach, [40:49] um but actually we're not done. Because [40:51] before running the common filter, we [40:53] should think about a few things like for [40:56] [snorts] instance, the the registration [40:57] problem. That means um where are the [41:00] coordinate frames of our different [41:01] sensors. So, the spatial orientation of [41:04] our sensors because the lidar is mounted [41:07] uh in a different place than the radar [41:08] sensor or the camera. [41:10] Um [41:11] Another issue, of course, is the the [41:13] temporal alignment. Um sensors are [41:16] different. It's the signal processing [41:18] chain is different. That means it could [41:20] easily happen that you you fuse data [41:23] from different time instance. Um that [41:25] means you have to think about something [41:27] like triggering unit. Um [41:30] just having because every single sensor [41:32] has its own clock. [41:34] How to synchronize those clocks? [41:36] Talking about hardware. [41:38] Um [41:38] do we find a global time in our car? Uh [41:41] what happens if we have external data, [41:43] data from infrastructure? How to make [41:45] sure that this actually runs on the on [41:48] the same time instance. So, it's there [41:50] are a lot of, let's say, few things to [41:53] to consider, um which you actually could [41:55] summarize in um let's say, in the [41:58] temporal alignment, in the spatial [42:00] alignment, um the registration issue, [42:03] and also um things like bias have to be [42:06] treated, but I'll come to the bias issue [42:08] a bit later on. [42:10] And once you have everything in the same [42:13] orientation, in the same frame, [42:16] um you have the temporal [42:18] synchronization. Then actually you could [42:20] start to implement the common filter. [42:24] Of course, this issue is not that [42:25] critical if you just run uh the filter [42:27] in Python [42:28] um or or [42:30] in whatever language you're preferring, [42:32] but it actually if you're doing real [42:35] real stuff on a real hardware where we [42:37] where we have to um think about the the [42:41] um [42:42] let's say requirement in terms of hard [42:43] real time, then you easily then you [42:45] quickly end up in those those kind of [42:47] issues. [42:48] But actually um this kind of [42:51] let's say temporal and spatial [42:53] alignment, the data association issue, [42:55] um how to work out um actually is is a a [42:58] lecture itself or even uh half of a [43:01] course to to get everything up and [43:02] running before you can do uh or run the [43:05] the filters afterwards. [43:09] Right. So, our three assumptions, [43:12] um [43:12] just briefly [43:14] revisit our our common filter. [43:18] We actually have [43:19] um [43:20] basically in our [43:23] recursive [43:24] estimation scheme, uh always the [43:26] prediction at the correction step. You [43:28] might recall um that we usually think [43:31] about the belief [43:33] in our [43:34] um state to be estimated XT at time [43:37] instant T. Uh we actually get the data [43:40] um which is sort of the belief from our [43:42] previous time step uh can be or is [43:44] comprised of the mean and the variance. [43:47] Um so, that actually is describing my [43:49] state of the system. And of course, the [43:52] belief is also um influenced by our [43:55] control variable, so the control input, [43:57] and of course, the measurement. [44:00] And we are actually interested in [44:02] working out for time step T our belief [44:05] in in our state. And in the first step, [44:08] the prediction step, uh [44:10] that's more or less all about [44:12] propagating densities. [44:14] Um we have [44:17] our linear model described by the system [44:19] dynamics AT. [44:21] Uh we have our control input UT scaled [44:23] by BT. [44:25] And we could actually work out our [44:27] covariance [44:28] um sigma T matrix formulation. And [44:31] that's basically the um [44:33] the step [44:35] for the prediction. So, no measurements [44:36] are are used. No measurements are there. [44:38] So, it's just the model uh that [44:40] influences our next step, our our [44:42] prediction. [44:44] And once we have, let's say, the next [44:46] state predicted, [44:48] we hopefully get new measurements in. Uh [44:51] and now we can actually work on that to [44:53] correct our [44:54] prediction and to work out if the [44:56] prediction was good, equal to the [44:58] measurement, or bad. And that's the, [45:01] let's say, the famous idea of Kalman to [45:04] uh come up basically with a correction [45:07] formulation that includes the Kalman [45:09] gain, calculating Kalman gain, which [45:12] actually scales measurements and the [45:15] model. [45:16] And [45:17] in the end, we have, just mentioned it [45:19] before, [45:20] um this innovation sequence there, [45:23] measurements and model, and the Kalman [45:24] gain is actually scaling that. So, if [45:27] the Kalman gain is close to one, we are [45:28] believing in the measurements. If close [45:30] to zero, we are just believing in in our [45:32] model. Um and in every single time step [45:35] T, [45:36] um this Kalman gain is calculated. [45:39] It's uh [45:40] is new, as I say, newly calculated, and [45:44] we uh assess the innovation sequence [45:46] again. Um work out if the measurements [45:48] are poor, if the measurements are good, [45:50] or [45:51] um if we treat them in, let's say, [45:53] equally the distribution between the [45:55] measurements and and the Kalman gain. [45:57] And in the end, we hopefully expect that [45:59] we work out, let's say, the belief in [46:01] our state XT [46:04] com- compromised of our mu T and the [46:07] covariants that hopefully gives us a [46:10] good uh good estimate of our current [46:12] state uh of our of our system. [46:15] And [46:16] that's basically the um the great idea [46:20] uh of Kalman and which we actually use [46:22] for [46:24] for um our [46:26] pose or position estimation right right [46:29] away. [46:31] Right. So far so good. Everyone already [46:34] implemented a Kalman filter? [46:37] Perfect. [46:38] Then have Let's have a look how to fuse [46:41] or how to use the Kalman filter for data [46:42] fusion because that's one of the things [46:44] you will also do on Friday um using the [46:46] AutoWare stack that you um use different [46:50] sources to work out um the data fusion. [46:54] Let's go for the following situation. We [46:57] assume a single track model for the [46:58] vehicle. Um standard. [47:01] We define that we have measurements from [47:04] a lidar, [47:05] uh from a GPS and IMU. [47:08] So we get [47:09] um lidar data and we just assume that [47:11] those lidar data, they're not that good. [47:14] Uh so we have a low precision on on [47:15] those data. [47:17] We get our GPS and we have our IMU that [47:21] is able to [47:22] measure the acceleration. And that's [47:24] also one of the the critical things [47:26] uh why sensor fusion just talking about [47:27] availability. [47:29] Um if we in the best case have lidar, [47:31] GPS, and IMU like shown here in [47:33] parallel, then we are on the safe side [47:36] because GPS could actually fall Let's [47:38] say GPS could be very poor [47:40] uh in uh in certain [47:43] instances. And if we just look at the [47:46] IMU at the odometry in our car, um we [47:49] measure the acceleration and then we are [47:51] integrating twice, means first [47:53] integration is the velocity, then we get [47:56] out the the position. [47:58] And if you just use the IMU because the [48:00] question could be why why do use the GPS [48:02] or or or the lighter sensor? [48:04] Um basically the IMU due to the fact [48:06] that we integrate twice actually [48:09] um [48:10] and you can prove that in in real and we [48:12] did it. Um you can make it maybe 10 to [48:15] 20 seconds then you are somewhere. So we [48:17] you are far away uh because you just get [48:19] the bias over time uh due to the double [48:21] integration and just relying on the IMU [48:24] on the inertial measurement unit is [48:27] just forget it because uh you you [48:30] can't get a proper uh pose estimation [48:32] just using the IMU. That's why it makes [48:35] sense to [48:36] go for uh redundant system with GPS and [48:39] lidar because the pose estimation is [48:40] safety critical because without knowing [48:42] the pose um unless with a certain [48:45] accuracy you shouldn't drive [48:47] autonomously. [48:49] Right. [48:53] So, just look at our um state vector [48:57] with our position, velocity, and [49:00] acceleration. [49:01] That's our [49:02] um mu t. [49:04] And we assume our standard [49:07] single track model which is quite known [49:10] which is used in many many publications [49:12] and which works quite good [49:15] for let's say for certain [49:18] circumstances. If you think about [49:20] lateral control, then you better should [49:23] find a better way of this driving that [49:25] because then you have to think about the [49:26] tire model or tire models and uh about [49:29] the dynamics, but for longitudinal [49:31] dynamics [49:32] that that one is pretty fine. TS, as you [49:35] know, is the sampling time. [49:37] And we actually have [49:39] already written in the matrix [49:41] version position, velocity, acceleration [49:44] and each line of course is [49:47] subject to noise. [49:50] Right. [49:52] So, just put that one in in our matrix [49:54] formulation. [49:56] Pretty easy. We get our state vector, [49:58] and we could actually [50:00] bring it together in this red line. Um [50:02] our mu is our linear model. [50:06] And we have our [50:08] um [50:10] noise, which also comprises [50:12] different So, let's say three entries [50:15] attached to the ear. And we, as I said [50:17] before, [50:19] we go for IID, independent and identical [50:22] distributed, with a sort certain RT. [50:30] Then, let's assume two cases. First one [50:32] is we just have lidar and IMU. Um let's [50:36] work out how that works. [50:38] Um [50:39] of course, you will see otherwise we [50:41] wouldn't have a second case, but that [50:43] one is not that good. [50:44] Uh [50:45] So, we also go for a fusion of [50:48] three sensors, lidar, GPS, and IMU in in [50:50] our second scenario. [50:54] So, as I said before, we assume that [50:55] lidar is is of low precision, and we [50:58] have the, let's say, the IMU. Those [51:00] sensors are there in our car, [51:02] and we would like to fuse the data. [51:05] Right. In this in this first attempt. [51:07] So, we actually could think about [51:10] exactly [51:11] um the measurement equation or sensor [51:13] model. CT again is our sensor model. [51:16] Uh we have our state vector, position, [51:18] velocity, acceleration. We have our [51:21] measurement noise of the lidar sensor [51:23] and the IMU. We just have two sensors. [51:25] And [51:26] our [51:27] hopefully our result is the position, [51:29] the estimated position, and the [51:31] estimated acceleration. So, that's what [51:33] we're aiming for. Again, we have [51:36] um zero mean and a certain covariance [51:38] for our measurements. [51:40] Right. [51:41] Um a few words to, let's say, the [51:43] initial state vector. Um how to choose [51:46] it, how to deal with it. [51:47] Um, yeah, it's it's a bit of a tricky [51:49] thing because there is no classical [51:51] rule. Of course, if you have the chance [51:53] to have an estimate from from the [51:55] history, from the previous state, then [51:57] go for that. [51:58] Um, is if your algorithm is pretty good [52:01] and this [52:02] let's say the [52:03] the common filter converges pretty [52:05] quickly, then uh, [52:07] just setting it to zero usually works. [52:10] But it's a bit of let's say a tuning [52:11] thing. Um, that's not a let's say the [52:13] rule for for every and every single [52:16] application that's just setting it to [52:18] zero works. Uh, sometimes it could [52:20] actually happen that uh, you're not [52:22] converging, then you have to think about [52:24] the initial start point. And of course, [52:26] the closer you are to the final [52:28] estimate, the better is it. But we know [52:31] sometimes we know do not know the ground [52:32] truth, of course. Sometimes it helps to [52:34] have the GPS signal and and just um, [52:37] trigger our initial state just using the [52:39] the GPS data. [52:43] Right. And the same thing for our [52:44] initial covariance matrix. [52:47] Um, [52:47] it's again, [52:49] let's say a [52:50] a tuning thing. Um, and you already [52:54] implemented common filters and you know [52:56] that tuning the initial value, tuning R [52:58] and Q are the usual ways to get things [53:01] done. [53:02] Um, but be aware um, by because in many [53:05] applications, the measurement [53:08] uncertainty measurement covariance is [53:09] just very high while the [53:11] um, [53:12] the process noise is set or the process [53:14] covariance is set to low uh, let's say [53:15] to small numbers, that actually [53:19] favors the model and you're actually [53:22] relying on a single track model and not [53:24] on the measurements. [53:25] Um, that's why usually [53:28] you should go away from those kind of [53:30] thinking about tuning uh, [53:32] thinking in tuning parameters because [53:34] especially when it comes to the [53:35] measurement noise to that one, [53:39] that's it. [53:40] There is a way to work that out. So you [53:42] actually could uh if you have the [53:43] chance, if you have a real hardware, [53:44] real system, you could actually work it [53:46] out um by sampling from from your [53:49] system. You could actually work out [53:51] um the [53:53] the mesh the measurement is let's say [53:54] the the uncertainty and and QT by real [53:57] measurements. And then, you could [53:58] actually go uh with the real [54:00] distribution uh in our in in in your in [54:03] your estimation procedure. [54:06] Right. [54:14] So, that's what we have for our last [54:16] four [54:19] for our initial covariances, where we [54:21] start off. So, sigma zero. [54:24] The other thing is [54:27] just a a few additional words what I [54:29] just said before, that um by tuning Q [54:32] and R, um you actually [54:35] can find or can improve the convergence [54:37] of the common filter. But be aware that [54:40] um [54:41] especially when it comes to that one, um [54:44] you should better measure it if you have [54:46] real access to a system, because [54:47] otherwise, uh what I was doing is what I [54:50] just wrote here is that you um the same [54:52] for for the measurement. Small values [54:54] indicate higher confidence. So, the [54:56] predicted values are more more weighted, [54:58] while large values of the process noise [55:01] indicate a lower confidence in the model [55:03] in your in in your system model AT. And [55:06] the same holds here for CT for our [55:09] sensor model. [55:10] So, [55:11] um [55:12] you might lose the physics if you just [55:15] tune too too much in Q and R. It's just [55:17] the point I would like to mention. Uh [55:19] for our example, so we what we what you [55:22] will see in a second is this just [55:24] synthetic data data that is simulated. [55:26] Some noise is added to it. Um just [55:29] decided to go for some zero mean [55:31] Gaussian distributed noise uh on the [55:33] let's say on on the to get noise [55:35] measurement data. And those those data [55:37] is [55:38] used then for for the estimation. [55:40] And that's the starting way for for [55:43] let's say for RT [55:46] where we have for each sensor different [55:48] different [55:49] let's say for for the process version [55:51] and for the measurements. So here we go [55:53] for lighter and IMU the different the [55:55] variances in there. [55:57] And what we are actually doing of course [55:58] is [56:00] first propagating densities the [56:02] prediction step. Um and then we get new [56:05] data new lighter data [56:07] and new acceleration data from from from [56:11] our IMU and we actually end up in let's [56:14] say in this [56:17] two-stage [56:18] recursive procedure between prediction [56:21] and correction and hopefully we get a [56:23] proper result. And ac- [56:27] actually the you will also get let's say [56:29] the basic code which is [56:32] implemented in MATLAB and Python which [56:34] will be shared by Boris. [56:37] So that you can easily [56:39] work out how the how this example is [56:41] implemented. [56:43] What we see here is basically our [56:45] position so position [56:48] in [56:49] let's say in the metric system in meters [56:50] over time. [56:52] We are starting at zero and and that's [56:55] where we have our ground [56:57] truth our ground truth data for the [56:59] position. [57:00] And in our first example we have our [57:02] lighter sensor and we assumed the [57:04] lighter data is very corrupted by noise [57:06] and very bad let's say poor poor [57:08] measurement data from our lighter sensor [57:10] so we get a lot of noise there. [57:12] And if you then [57:14] do let's say our estimate running our [57:16] Kalman filter our standard Kalman filter [57:19] the linear linear Kalman filter we see [57:22] first thing is but that's why we did the [57:24] first simple example that the variance [57:28] now is smaller [57:29] that's good but we see the estimate is [57:31] still [57:33] um [57:34] pretty bad. Um of course we could [57:36] actually do some filtering afterwards [57:38] some teeth [57:39] uh let's say some some low pass [57:40] filtering uh afterwards uh but in fact [57:43] it's it's not not that good uh using [57:46] let's say this let's say lighter data [57:48] for for position estimating alone. [57:52] Let's have a look at the velocity um [57:55] that fits together [57:56] um [57:58] just look here um [58:00] Yeah, right. So it's just time second 6 [58:03] to 8 so we are flat right here. So we [58:05] should end up [58:06] here at zero around here. So um that's [58:09] our velocity in [58:12] in meters per second [58:14] um so that's [58:15] actually not miles [58:17] in there meters per second. Um [58:20] That's it. So that's our ground ground [58:21] truth data and if we [58:26] just look at the estimate coming out of [58:28] our common filter as implemented right [58:29] there for we got pretty good pretty good [58:32] result. When we have let's say [58:33] stationary data usually it's not very [58:36] good. The common filter is much better [58:38] when when it comes to dynamics and [58:41] you'll see here some some offset in the [58:43] in the result there which which is not [58:45] actually not that good. [58:47] Um [58:48] Offset is another issue where we have a [58:49] few words [58:51] in a couple of minutes [58:52] um common filter and [58:55] it's [58:56] Kryptonite. [58:58] Velocity and finally acceleration our [59:00] third entry in our state vector [59:03] um in meter per second squared. [59:07] Again the ground truth data so that's [59:09] what we actually in that case so we have [59:10] the ground truth we would like to see an [59:12] estimate that perfectly fits to that [59:13] one. [59:14] And what we actually get from our IMU is [59:17] the green the green plot. A lot of [59:19] noises in there. [59:21] Um that's what we get from our inertial [59:23] measurement unit by measuring the [59:25] acceleration. [59:27] And doing the the estimation um like we [59:29] had before with the common filter, we [59:31] get more or less the the red curves for [59:34] our estimate, which are again a bit [59:35] noisy. And [59:38] And that's basically it. Um [59:41] it's there. So, we we got some results, [59:43] but actually they are [59:45] to be honest, we are not satisfied with [59:46] those kind of results. [59:49] And that's why let's have a have a look [59:51] what can we do if we just now use GPS [59:54] data in addition to our two other [59:56] sensors, lidar and and IMU. [59:59] And [60:00] and have a look what what changed so [60:02] far. Yeah, sorry. [60:07] >> Yeah, we don't see noise here. [60:10] In the estimate. [60:11] >> Right. [clears throat] So, we are we are [60:12] not measuring velocity directly, so [60:14] that's just the the the estimate. While [60:17] lidar, so we have a lidar sensor and we [60:19] have we have the IMU. So, we have real [60:21] measurements from two sensors, which so [60:23] the one measures [60:25] the position based so the lidar based on [60:27] the point cloud, the other one measures [60:28] acceleration using the the IMU [60:31] accelerometers. Meters. But we don't [60:34] have any velocity sensors in there. So, [60:35] it's just a a pure estimate out of our [60:38] let's say single using the single track [60:40] model and that's why we don't see that [60:42] that noise in there because there is um [60:44] not a let's say assume that a big noise [60:46] for for the velocity. That's that's the [60:48] that's the reason for it. [60:50] Okay. [60:53] >> [clears throat] [60:54] >> Right. [60:55] So, back to our um [60:57] fusion example using now our [61:00] third sensor. So, we [61:03] put it right in here. Um so, that line [61:05] uh goes along with the GPS. Uh we also [61:08] assume some uh measurement noise for our [61:10] GPS sensor, which is actually um [61:14] as defined or as assumed in the [61:16] beginning for our example, that one is [61:17] still the worst sensor, while uh GPS is [61:20] the best sensor in terms of uncertainty. [61:24] And [61:26] we got this new situation and we [61:28] actually could run the common filter [61:29] just with one new variable. So, um, [61:33] very easy for a common filter, [61:35] um, [61:35] to, let's say, to augment your [61:38] system from before, uh, with an [61:40] additional, um, measurement input. So, [61:43] actually only our measurement equation [61:45] changes while our state equation is [61:47] pretty much the same because we are [61:48] still estimating um, position, velocity, [61:52] and acceleration. So, the the state [61:54] equation [61:55] So, the process equation is pretty much [61:57] the same. The measurement [61:59] equation changes because now we also [62:00] need a [62:01] sensor model. [62:03] So, one row in in C that reflects the [62:06] sensor model of GPS. [62:12] Right. And [62:15] for our example, we just used for for, [62:18] um, [62:19] let's say, Q T or a measurement [62:21] covariance. [62:23] Those kind of parameters, uh, but you [62:25] could easily play around in in the [62:26] Python code to see what happens if if [62:29] that changes, [62:30] um, [62:31] for for the different sensors. That's [62:33] just an assumption just in there. [62:35] Right. [62:36] And looking at our result [62:39] what we get here, so the same scenario, [62:41] same way of position, uh, over time. [62:45] The ground truth data, um, what I see [62:47] here is lighter again. The bad the bad [62:50] signal or the bad measurements while GPS [62:52] is even better. So, the yellow curve [62:54] shows the measurement data of GPS. So, [62:57] pretty good [62:58] pretty good, [62:59] um, [63:01] measurement data. Usually in practice, [63:04] you're going for DGPS, differential GPS, [63:07] which allows you basically to get out [63:11] um, let's say, sometimes it can just a [63:14] few inches, um, so in the metric [63:16] systems, 5 cm, so pretty a pretty good a [63:19] accuracy. Uh, if you go for instead of [63:21] the classical GPS, which is now in every [63:24] single car that that you buy, if you go [63:25] for the differential GPS, DGPS, [63:28] um, [63:29] we will [63:31] spend a few words on Thursday and Friday [63:33] on that. It really gives gives you a [63:35] very good estimate or let's say [63:37] measurement data, which is actually very [63:39] good for for poses and measurement. Of [63:40] course, if GPS fails, [63:42] you have to deal with exactly that one, [63:45] that you have to use lighter or IMU on [63:48] top of it or in addition to do the fuse [63:50] diffusion. [63:53] Right. So, that's that's our measurement [63:55] for position, and [63:57] let's go for for our camera filter. What [63:59] we see here right now is that our [64:02] estimate, the red curve, [64:04] um, [64:05] shows us a very, let's say, compared to [64:08] the lighter sensor, a [64:09] perfect [64:11] um, distance or let's say a [64:13] great difference between the green [64:15] curve. Um, of course, from from that, [64:17] let's see, from from that kind of [64:19] scaling, it looks like that the [64:20] estimate, [64:22] uh, is pretty much the same that the GPS [64:23] data would actually would say would [64:25] brings us to the question, why to go for [64:28] for for the camera filter when the GPS [64:30] data is is pretty good. One thing which [64:33] we already [64:34] touched this, of course, what happens if [64:36] GPS fails, then we are [64:39] hopefully have, let's say, a better [64:40] solution. And the other thing is, if you [64:42] have a closer look in in this, let's [64:45] say, in [64:46] in in this behavior of the red and [64:48] yellow plots, we actually see that, um, [64:52] and that's, of course, that's the [64:53] theory, but we have proven in our first [64:55] example that the overall, [64:57] um, variance [64:59] of the Kalman filter, of the estimate, [65:00] is smaller than the variance of the the [65:03] smallest variance [65:05] of the best sensor, in our case, the [65:07] GPS. That means the overall variance of [65:09] the estimated [65:11] result has to be smaller per per [65:13] definition than the GPS variance. That's [65:16] that's the that's the point. Even that [65:18] example, um but it's subject to let's [65:21] say to the the choice of the the [65:24] variances of the measurement data, we [65:26] actually can show that even for [65:29] um you see here that's the lighter it's [65:30] the green one, yellow is GPS, and the [65:35] estimate one is is the red one. We are [65:37] even for [65:38] So, green is of course far away. Looking [65:40] at this part, just having a detailed [65:43] plot here, you see the red one is is [65:45] below the the yellow one, which is [65:48] basically that what we expected because [65:51] in the theory behind that actually [65:53] um [65:54] should be the case. [65:56] Right. [66:02] So, so far, questions to the procedure [66:05] of combining data in [66:07] in the common filter? [66:10] I think it's it's [66:13] pretty straightforward approach. One of [66:15] the things which I mentioned is of [66:17] course before doing the common filter [66:19] applying the common filter there are a [66:20] few steps to [66:22] um to keep in mind [66:24] like the temporal synchronization and [66:27] and um [66:28] the the let's say registration problem [66:30] means the the spatial [66:34] the spatial um [66:36] calibration, let's put it in that way. [66:39] And another issue, [66:41] and that might be the last part for [66:43] today, is common filter of course [66:47] is perfect to decrease variances. [66:50] Um [66:50] that's really intuitively there, but [66:52] what about bias? And it happens quite [66:54] often that sensors feature a bias. What [66:58] do you think is the common filter able [66:59] to treat bias? So, if you have a bias on [67:02] your IMU, [67:04] can the common filter work that out? [67:06] What do you think? [67:10] Is it a [67:13] Right, it's [67:14] it's it's a bit of an issue the common [67:16] filter is not able to deal with bias [67:18] signals and [67:19] um do you have an idea how to deal [67:20] because in [67:22] in measurements, that's quite often the [67:24] case. You have, let's say, [67:26] um [67:27] different kind of uncertainties and [67:29] different kind of errors. Usually, [67:31] systematic errors, random errors. [67:34] Um random errors [67:35] always somehow uh related to variances. [67:38] Common filter can work with those. [67:40] Systematic errors are usually um [67:42] reproducible and they have the same sign [67:46] in every single So, they they are [67:47] reproduce also in in in terms of signs. [67:50] And usually, you do some sort of [67:51] correction. [67:52] Um and the bias, if it's [67:55] constant over time, then it's easier to [67:56] correct. If it changes over time or even [68:00] every single time step, then it's a bit [68:01] more trickier [68:03] to get that done. [68:04] Um but basically, what do you think? How [68:06] can we deal with a bias and cuz the IMU [68:09] is is is one of those kind of sensors [68:12] where bias could easily happen. [68:15] Do you have an an idea how how to work [68:17] work it out? [68:21] It's So, it's not that [68:24] that complicated and [68:26] just show you without, let's say, the [68:27] theory behind. There are a few things. [68:29] The one thing is the bias. Uh other [68:31] things are colored noise. So, we we [68:33] assumed quite quite often Gaussian, zero [68:36] mean, normally distributed noise. [68:38] We also know that's actually not the [68:40] case. Um that's always the case when we [68:42] in in in let's say in [68:45] in the classroom that we have linear [68:46] systems and [68:48] and white noise, Gaussian Gaussian white [68:51] noise. But in in practical examples, we [68:53] often have longer tails, fat tails in [68:56] our distribution, which are then uh [68:58] actually somehow different [69:00] distributions. Um and we might work that [69:02] out uh by [69:04] um sampling many, many times, doing some [69:06] sort of a histogram fit, and hopefully [69:09] figure out it's close to a Gaussian [69:11] distribution or a chi-squared [69:13] distribution or student t, whatever. [69:15] Um [69:17] But in fact, we have to also deal with [69:19] bias and with some kind of colored [69:21] noise. And what actually is the the [69:24] great, let's say a great trick, is just [69:28] in the common filter and a state [69:29] estimation, [69:30] uh but if you can cannot treat the [69:34] problem, then just estimate it uh along [69:37] with the other parameters. That means [69:39] you just augment your state uh state [69:41] vector. Um instead of just estimating [69:44] position, velocity, and acceleration, [69:47] you actually [69:49] add another another row, the bias of the [69:51] IMU, for instance, and you just estimate [69:53] the bias. [69:54] Um [69:55] that's one of the, let's say, well-known [69:57] tricks, pretty useful, just to estimate [70:00] the bias. [70:01] And that actually [70:03] could help you. Uh the one thing is if [70:05] the the bias changes over time, [70:08] um to work it out uh because that also [70:10] could be, let's say, giving you a [70:12] feedback that the sensor degrades over [70:14] time in terms of uh condition [70:17] monitoring, state state monitoring. [70:19] And other things like the colored state [70:22] noise, colored measurement noise, [70:24] um can be also handled by, let's say, by [70:28] estimating the the noise parameters. So, [70:30] augment the state vector and estimate [70:32] the noise parameters, mean and variance, [70:34] or whatever distribution you're [70:36] uh you have to manage. [70:38] Other thing, and another quite helpful [70:41] approach actually is to do some sort of [70:44] pre-whitening, um which is a nice [70:46] approach, um or the code shaping filter, [70:48] that whatever, so you put colored noise [70:51] in magic box, and you get white noise [70:53] out of it. Um [70:55] It's one of the things where Marco will [70:57] definitely give you a a few ideas on [70:59] that, but that's that's a pretty helpful [71:00] approach to do this shaping of the noise [71:03] distribution. [71:04] And [71:05] I also put So, we talked about the [71:07] offset and [71:09] offset over time, so drifts. And [71:11] finally, we have two other issues, [71:12] sensor faults. And And that's one of the [71:14] things which is quite important for for [71:17] autonomous cars. The one thing is to [71:19] have the proper estimation of position [71:21] of all the dynamic objects around me, do [71:24] a proper path planning, path matching. [71:26] Um that's the one thing [71:29] where we still have a lot of work to do. [71:31] But, the other thing is what happens or [71:33] how to to detect errors in our in my [71:35] system because that's how to monitor the [71:37] overall performance of the system. And [71:39] that actually is one of the things we [71:41] have to deal with. And the common filter [71:42] is pretty cool pretty cool in that [71:44] because just checking the innovation [71:48] sequence, that means the the difference [71:50] between measurements and the sensor [71:51] model, helps you quite a lot. Just Um [71:55] defining a threshold and and watching [71:57] that one, monitoring that one, can give [71:59] you actually an idea if if sensors are [72:02] are are let's say have a let's say a [72:03] breakdown or are completely completely [72:06] uh unusable um unusable anymore. [72:09] And the same happens for the actuators. [72:11] Um so, you can use the common filter for [72:13] monitoring uh system monitoring or [72:15] condition monitoring of your entire [72:17] system. [72:18] Which is pretty nice. And it turns out [72:20] that a lot of work meanwhile is [72:23] moving to that part um because what [72:25] happens actually if an autonomous car [72:27] has some issues from the technical point [72:28] of view. I'm not talking about just the [72:30] sensor is not working. It could be even [72:32] more in in your system. [72:35] Right. [72:36] So, but the let's say message basically [72:39] is [72:40] if [72:41] if you have a problem with certain [72:42] parameters, just include it in the [72:44] problem, augment the state vector, and [72:46] just go for the for estimating that one [72:48] with the with the uh other parameters. [72:53] Right. And that brings me to, let's say, [72:55] the final slide for today, the the [72:57] conclusion. [72:59] Um we actually started [73:03] this morning by thinking about what are [73:05] the benefits of multi- [73:08] sensor perception, multi-sensor data [73:10] fusion. We talked about the different [73:12] taxonomies of fusion. We had these four [73:15] different taxonomies [73:17] uh where we talked about, so you can [73:19] find different ways of uh talking about [73:22] sensor fusion. [73:24] Um more on the data part, more on the [73:26] architecture part, uh talking about [73:29] raw data or object lists, um [73:31] complementary, cooperative, competitive [73:33] fusion. So, we had those four kinds. [73:36] Then we actually worked out um [73:38] how fusion works, how can you fuse two [73:42] different PDFs. So, we just did this [73:45] with [73:45] um two Gaussian distributions to work [73:47] out and to show that the overall [73:49] variance is smaller than [73:52] the variance of the best sensor in in [73:54] your fused system, in your fusion [73:56] system, [73:57] um which can be proven, um [74:00] we also had um [74:02] let's say a bit more improved example [74:04] where [74:05] or let's say more advanced one where we [74:07] talked about [74:09] how to fuse data from different sources. [74:11] And we also spent a few, let's say, a [74:14] few comments on on our covariances, [74:17] which are in many, many cases seen just [74:19] as tuning parameters, but um be careful [74:22] there. They're not only tuning [74:24] parameters. Of course, the process [74:26] noise, we can't measure it. It's [74:29] it's hidden um in in our in our um [74:32] system dynamics, but for the measurement [74:34] uncertainties, we actually, if we have a [74:35] real system, real sensors uh in our lab [74:38] or right out on the road, we could [74:40] actually work out um the the measurement [74:43] uncertainties and the measurement [74:45] covariances of our system, and then we [74:46] actually can deal with the right uh [74:48] right parameters in in our estimation [74:51] procedure. [74:52] And I think the the takeaway definitely [74:55] is that doing fusion has [74:58] many many advantages. [75:00] Um it's not only about the accuracy, [75:03] getting the variance down. [75:05] Um that's of course one of the great [75:06] advantages, but we also have to think [75:09] when we talk about autonomous cars that [75:11] sensors might fail, that sensors might [75:13] deliver wrong data. [75:15] Um and you don't always have all the [75:18] sensors available. Uh just thinking [75:20] about um [75:22] driving through tunnels or uh bad [75:24] weather conditions. Of course you're on [75:26] the sunny side here in California. It's [75:28] not that that bad, but there are a lot [75:30] of countries around the world where you [75:33] have a lot of tunnels. [75:35] Uh and you know what I'm speaking about [75:36] because in Austria we have a lot of we [75:38] have the Alps. There are a lot of [75:39] tunnels. [75:40] Um [75:41] weather is changing over the year. Um [75:43] and that happens still many places [75:45] around the world. So we have definitely [75:47] as an as engineers think about those [75:49] those kind of things that you're not [75:51] only running here in the Silicon Valley, [75:53] um that they can be be distributed all [75:55] around the world. [75:57] Right. [75:58] Then are there any any questions? [76:02] Otherwise I would like to close. Thanks [76:05] for for your attention and we will have [76:08] our next lecture on Thursday when we [76:10] talk about let's see Autoware, about the [76:13] software stack, and then the hands-on [76:15] session on Friday. Thank you very much.