Fusing Sensor Data to Improve Perception in Autonomous Systems
Overview
Autonomous systems continue to be developed and deployed at an accelerated pace across a wide range of applications including automated driving, robotics, and UAVs. The four main components of an autonomous system are sensing, perception, decision making, and action execution and control.
In this webinar, we will focus on the perception component of an autonomous system. We will demonstrate a sensor fusion development framework that includes an integrated test bed, algorithms, and metrics for performance analysis. We will look at the strengths and weaknesses of different sensor modalities for localization and multi-object tracking. We will also explore how measurements from different sensor modalities such as radar and lidar can be combined to improve the quality of the perception system.
Multiple examples will be used to highlight concepts that are critical to architecting and integrating systems that perform sensor fusion.
Highlights
Attendees will learn:
- Why sensor fusion and tracking are a key part of perception systems
- How to combine measurements that are taken from different sensor to improve pose estimates and situational awareness
- How to explore system level trade-offs including sensor accuracy, location, and update rates
About the Presenter
Rick Gentile focuses on Phased Array, Signal Processing, Radar, and Sensor Fusion applications at MathWorks. Prior to joining MathWorks, Rick was a Radar Systems Engineer at MITRE and MIT Lincoln Laboratory, where he worked on the development of many large radar systems. Rick also was a DSP Applications Engineer at Analog Devices where he led embedded processor and system level architecture definitions for high performance signal processing systems, including automotive driver assist systems. Rick co-authored the text “Embedded Media Processing”. He received a B.S. in Electrical and Computer Engineering from the University of Massachusetts, Amherst and an M.S. in Electrical and Computer Engineering from Northeastern University, where his focus areas of study included Microwave Engineering, Communications and Signal Processing.
Recorded: 16 Nov 2021
Hi, everyone. My name is Rick Gentile. I'm the product manager for our radar and sensor fusion products at MathWorks. Welcome, and thank you for attending this webinar on fusing sensor data to improve perception in autonomous systems.
Now, autonomous systems continued to be developed and deployed at an accelerated pace. This is across a wide range of applications, including autonomous driving, robotics, UAVs to name a few. There's four main components of autonomous system. Sensing, perception, decision making, and control.
Now in this webinar, we'll focus on the perception component of the autonomous system. We'll demonstrate a sensor fusion development framework you can use to drive your system forward. We'll look at the strengths and weaknesses of different sensor modalities for localization and multi object tracking. And we'll also explore how measurements from different sensor modalities, such as radar and lidar can be combined to improve the quality of perception systems.
Now building an autonomous system that can operate safely in challenging in dynamic environments is hard. To ensure the perception function of an autonomous system is up to the task, high resolution sensors with high update rates are needed to comprehend surroundings around the autonomous system. Because of these systems, they operate in such close proximity to static, and dynamic objects, and actors, the returns produced by the sensors drive the need for trackers that can make sense of the data that's being collected.
Now some of you might already be familiar with this layer based diagram of an autonomous system, where the different tasks are divided into the ones I mentioned earlier. Perception, planning, and control. The tracking algorithms along with their data processing algorithms are typically categorized in this perception layer. And this layer is really responsible for providing situational and self-awareness of the autonomous system. Now by situational awareness, I mean the information about the surroundings of the autonomous system.
Now sensor fusion techniques can help take advantage of the strengths of individual sensor modalities, while de-emphasizing the weaknesses of these sensors. Now here you see some of the most common sensors used by autonomous systems. In applications where safety is critical, you'll typically see more than one sensor, or some combination of sensor modalities. It's the system designers responsibility to balance performance and cost constraints, while still achieving the end requirements for safety and mission readiness.
So in our session today, I'll try to show you how you can use modeling and simulation to answer these types of questions. Now note, the nature of the questions go beyond how a specific algorithm choice performs, although that's also very important. You can also explore architectural decisions, like how many sensors do I need, how well do they need to perform, and what data rate do these need to provide data at.
I'll use a range of tools in this webinar, but I want to start with sensor fusion and tracking toolbox. I started here because the toolbox provides a test bed, which includes tools to simulate sensor data for algorithms. A library of tracking algorithms, as well as analysis tools. They're required to really complete the workflows from designing, simulating, testing, and finally deploying these sensor fusion and tracking systems.
Workflow is in place to help you integrate data from your own sensor or sensors. This could be in the form of recorded data or data you feed directly into the processor. You can also generate data directly from our sensor models. We also have a range of trackers to pick from. And you can use trackers out of the box, or you can customize our trackers with libraries of tracking filters, motion models, assignment algorithms, and track fusers.
The toolbox is written in MATLAB, so you have access to the code if you want to understand how we've implemented algorithms, or if you want to use it as a starting point for your own custom system. You can design, simulate, and test tracking algorithms for a range of different sensor modalities. And it includes some of these combinations that I show on this slide. And we'll talk about some of these in more detail.
Now, many of the examples that I showed in this session use automated driving scenarios, but as I noted at the beginning, the concepts that I talk about apply more broadly to autonomous systems, including robots, autonomous drones, UAVs, and so on.
We'll start with the sensor models and tracking algorithms for autonomous systems. So it's probably good to start with the sensor measurements. And these can take many forms. We don't make any assumptions on what the detection is from a sensor. You can define what a measurement consists of. And I've included some common types here. The top row shows what you might receive from a radar or a lidar. A point object would typically be seen for targets that are far away for a long range radar. For higher resolution radars, which we'll focus on in this session, you may see extended objects, where you get multiple detections per object per scan.
Now sometimes, these types of extended objects can be clustered into a single detection. In that case, in the case of the high resolution lidar for example, we can fit a 3D bounding box around the object and track that bounding box. And that's due to the fact that the lidar is such a high resolution sensor.
You can also bring in measurements that are considered incomplete, meaning they might be angle only detections or range only detections from passive sensors. And these types of observations typically require data from either more than one sensor or they need the platform with the passive sensor on it to maneuver to get information on the objects around it.
Now you can use your own sensor measurements with the algorithms and tools I discussed in the session. We have an example that's referenced here that shows how you can map your own detections to the object detection format. Now, this object detection format is our standard API if you will, into the tracking algorithms. In addition to the measurement itself, the container includes information that defines the time of the measurement, the measurement noise, and other related measurement parameters.
There are five examples on our website that show various mappings. And it depend on the coordinate system being used on your sensor, and how you can map those in. And this really provides the information, including sensor orientation with respect to the platform, it's in motion. So I encourage you to take a look at this, if you have your own data and you want to get started with some of the tools that I show you.
Now mapping recorded detections to our object detection is straightforward. On the left side, you can see that the visualization of the recorded data from a radar in a passive sensor. In this passive sensor generates angle only measurements. Now here, the ego vehicle is in motion. On the left, you can see the blue object that's moving up into the top of the plot. On the right side, you can see the zoomed in version of two targets from the same set of measurements. And this includes the blue radar detections, and the yellow angle only measurements. Now note that the measurement uncertainty in the form of the ellipsoid is also shown as part of the radar detection. This is also part of the object detection format that I showed you on the previous slide.
Now you can create scenarios and test data to augment your own data sets. And so what I'm showing here, is a variant, actually of what I showed you on the previous slide. So we took data in, mapped it in, visualized it, made sure it could map properly. And then we can create this scenario. And really, we can use it to augment the data that we've collected in the field.
It's easy to model trajectories and configure sensor models to recreate the scenarios that your system will encounter. This allows you to focus on corner cases that might be difficult to recreate in the field. Now once the scenario in sensor models are in place, you can generate larger data sets using our perturbation workflow. And I'll talk some more about that in the next slide,
You can start with a baseline scenario. In this case, it's an aircraft approaching a runway. And you can perturb the trajectory, and also the sensor parameters to generate a larger data set. And the right, you can see the results after perturbing the scenario on the left. Now note that the single trajectory transforms into many trajectories with this perturbations applied.
Well so far, I've shown some really simple scenarios. And we'll talk about some more complicated ones later, but you can build much more complicated scenarios that match the kinds of systems you're building. You can generate these scenarios programmatically in MATLAB, or interactively using apps. And there's multiple options to get started with, depending on the application that you're focused on. The scenarios I show in this webinar are actually generated using Automated Driving Toolbox and UAV toolbox. We also have some more generic scenario tools in sensor fusion and tracking toolbox to make it easy to generate test data for tracking infusion workflows.
Now I won't go into these in more detail because there's some great short videos available on our website to get started with. So please check them out. As I noted earlier, there are a range of sensors that are typically used for an autonomous system. You can process your own sensor data, or you can synthesize sensor data using MathWorks tools.
Now depending on the type of work you're doing, it's important to have access to the right level of data. You can see the range here from signals on the left signals and pixels on the left depending on the sensor modality, to object detections and tracks. Now each of these types of data can be useful in the development process.
In addition, if a sensor is purchased from supplier, for example, that's going to be integrated into your autonomous system. Sometimes, you have to take what you get. And it could be any one of these types of outputs, so it's handy to have the ability to generate these. I've also included toolbox names on the right, here. If you're working with any of these types of sensors, I encourage you to take a look at some of these tools, either to process data or to synthesize data that you can use to augment your system.
And I briefly discussed this range of sensors that are typically used, but I want to go through a little more detail on radar because of the challenges that the environment introduces into the perception system when radar is involved. Modeling and simulation are important ways to build up data sets for algorithm development to really give you insights and help you understand the challenges that your system is going to face. And there's no one size fits all for any modeling approach. In reality, it's a combination that probably best matches the problem on hand.
So on one hand, radar models that generate IQ signals are sometimes referred to as waveform level models. And the signals that I mentioned here are typically used to develop things like signal processing algorithms. And as we move to the right of this picture, you can see that the radar models also can generate either detections, clusters of detections, or tracks directly from the system. And we refer to these as measurement level models. And this type of data is typically used by data processing teams to develop their tracking and Fusion Systems.
Here you see some code snippets that generate detections or tracks directly from the measurement level model using our radar data generator, which is part of our Radar Toolbox. And then also, on the left, the corresponding code to generate the in-phase and quadrature data, using the radar transceiver model. And these are components in Radar Toolbox that you can use to generate the data directly. They both have a parameterizable and have a lot of flexibility to match the kinds of systems that you use in your system. We want to simulate the radar as closely as possible to what will happen in the field. And for radar, I mentioned multipath effects drive how you configure and tune the trackers.
On the left plot, you can see the simulation with the ground truth playing out along with it. And the detections from the radar returns, including returns off the static and dynamic objects in the scene. And we'll look at this type of system and how the tracker can handle a system like this with all of these detections coming in. We'll show that shortly.
With multipath, we see that a lot of different returns will come back to the system. And in reality, many reflections in addition to the primary reflection off the target come back into the radar. We know that the dominant set of returns are the ones that have traveled up to three bounces, typically. So these are the ones that we model. So in our radar models, you can optionally include the ability to generate reflections from two or three bounces. And these are in addition, of course, to the direct line of sight path that you get the return from the object as well. The additional detections generated from multipath result in ghost detections. And that adds to the situational awareness challenge that you have in an autonomous system, with these high resolution sensors, at close range, and additional detections being generated.
Now I showed the concept of moving between the statistical measurement level models and the signal waveform level models. Here you can see the both of them in action for the same scenario. On the right, you can see the corresponding waveform level model, which includes things like the antenna array, and the signal processing algorithms to generate a radar data cube.
OK. Well as we just saw for radar, autonomous systems use high resolution sensors to perceive their surroundings continuously. And at close ranges, again, these high resolution sensors typically generate multiple returns from other objects in the surroundings. In the examples of these types of returns include these high resolution sensors like lidars, where the return is a dense point cloud of the surroundings.
With radar, we see multiple detections. So it's a sparser point cloud. And it also includes information like Doppler, for example. Finally, with cameras, the returns include multiple pixels per object. Now in the tracking community, these objects are referred to as extended objects. And if you're from the tracking domain, you'll know that traditionally tracking algorithms were designed with the assumption that each object returns at most one, detection per object, per scan.
So let's discuss some of the ways that extended objects can be tracked. Now the good news is that with sensor fusion and tracking toolbox, you have several methods to track these types of objects. Starting with the simplest approach, you can simply cluster returns from the sensors, such as the returns from the same object are clustered together into a single detection. A Classic example of this is to cluster pixels from a camera, from the same car into a box. And once you have the object level detections, you can simply use any traditional tracking algorithm such as joint probabilistic data association, tracking algorithm to track objects.
Now the clustering, plus the tracking approach can fail, especially when you don't have enough features in your sensor returns to do perfect clustering. This is when you can use extended object trackers. Now these algorithms can directly process returns from sensors. These are in general, less sensitive to clustering errors. However, these trackers require detailed object and sensor modeling. This approach doesn't scale as well when you have a complex scene with different types of objects.
In that case, you can consider using grid based trackers. Now these trackers estimate an intermediate occupancy grid. And this helps avoid the data association that's required between the sensor returns and the objects.
Now before we go into some examples. It's important to note that performance evaluation is critical when you want to test and validate the designed tracking algorithm that you're working on. A tracks are just an estimate of some ground truth. So to check how good or poor the estimate is, these metrics require ground truth to be available. And this can be done either using simulation that you can label directly as you simulate, or you can use your own labeled ground truth.
Now in general, tracking metrics are represented as I show here. The track metrics block takes in tracks in ground truth. And it produces some metric that can be analyzed to check if the tracking is done properly. Now we have two categories of metrics you can use. The first one is what I refer to as a detailed analysis tools. These types of metrics provide you detailed information by track and by ground truth.
So for example, the assignment metrics you collect can inform you about things like whether the ground truth is tracked properly. And if yes, how much time did it take to establish a track. On the other hand, error metrics help inform you of how well a particular ground truth is tracked. That is what's the error between the estimated track and its actual ground truth.
Now the second category of metrics is what I call summarized analysis tools. And these metrics provide you a single cost value, as a summary for the entire system for each step. A higher value of this metric, typically denotes poor tracking performance. So you see in some of the plots I go through, lower is better on these type of integrated plots. This is an attractive metric when you're looking at the tracking system as a whole. And the toolbox offers two metrics in these categories. The OSPA metric and the GOSPA metric.
The OSPA metric can be considered as a statistical distance between the multiple tracks and truths. And the approach that's used to compute GOSPA is similar to the OSPA metric. We use a slightly different mathematical formulation. And the GOSPA metric additionally computes subcomponents, such as miss targets and false tracks, for example. These types of summarized metrics are great to compare integrated scores, as we'll see in some of the examples that are coming up. Now one use case of these metrics is to decide which algorithm to pick for your application.
In the last section I showed you, how you can use different algorithms to track extended objects. So in this example that I'm pointing to here, you can use three different types of algorithms to solve your problem. And we can use the OSPA metric to help assess the performance across those options.
Now note here, the first option is a point object tracker that's fed from a clustering algorithm, like DB scan. The other two options include PHD trackers, which are extended object trackers or we can have them configured as extended object trackers, that don't require any clustering detections as an input. And so when we put this kind of a system together, we can look at this integrated score and quickly tell by the asymmetric plots, that the Gaussian mean, PHD tracker offers the best tracking performance for the scenario that we've tested it under. We this because it has the lowest OSPA distance as shown on this plot.
Now, I want to come back to the animation I showed you earlier. This is where the radar sensors were reporting detections from the vehicles and from the barriers that are on both sides of the highway the radars also report detections that don't seem to originate from any real object in the scenario. And these are the ghost detections due to the multi path propagation that I talked about earlier.
Now, object trackers assume that all detections originate from real objects, or from uniformly distributed random clutter in the field of view. Now contrary to this assumption, the ghost detections are typically more persistent than clutter. And they behave like detections from real targets. This means that an object tracking algorithm is very likely to generate false tracks from these detections.
It's important to filter these types of detections out before processing the radar scan with a tracker. Now to filter detections, we use a process like the one we show here on the right. The radar sensors report the measured relative radial velocity of the reflected signals. And we use that information to determine if the target is static or dynamic. This greatly helps to improve the understanding of the scene.
While this pre-processing is in place, we can use the extended object tracker to track the vehicles around the the ego vehicle. Now note in this plot, that you can see a couple of things I want to highlight. The vehicle is being tracked here, have an ellipse around them. Since this is the extended object tracker output, and this gives us the result of the position and velocity information. But it also gives us an estimate of shape and orientation. You can see that in the ellipsoid around each of the vehicles.
Notice also, the color code of the different types of objects. We have ghosts that we've classified from static sources. We have ghosts that are classified as originating from dynamic sources. We have the reflectors, and the actual targets here. This is important because this is giving you some insights into the pre-processing and how that actually, we can identify these properly. Then what we pass to the tracker, can be much easier for the tracker to digest.
Coming back to the picture that I showed you for the block diagram of how we actually do that pre-processing We can also, similar to the tracking algorithms, we can actually quantitatively analyze the performance of the classification system, to get a sense of whether it's working or not. And I show the confusion matrix here on the left, and then the gospel metric on the right, the rows in the table of the confusion matrix denote the true classification information of the radar detection. So you can get that kind of insight is whether it's correct or not, what was predicted, and what did the classifier come back with.
Now note that almost all the target detections are classified correctly. That's good. A small percentage of the target detections are misclassified as ghosts from dynamic reflections. Now some of the ghosts from the static object reflections and the ghost from the dynamic object reflections are misclassified as targets. And these are sent to the tracker for processing. And due to the spatial distribution of the false alarms in the inside the field of the view, the majority of the false alarm detections are either classified as reflections, from static objects or from dynamic objects. So this gives you some insights into how your classification will work. And you can further tune that process if it needs to be stronger. But the point here is that you can simulate a system like this, understand the effects before you actually go out on the road.
On the right, we also show the gospel metrics, which shows that there's no false tracks, which is good to see. And after the initial setup, the numbers drop down, which again lower here is better.
I talked about three types of extended object tracking. And the third one that I want to tell you about is the grid based tracker, which is helpful because it enables early fusion of the data from high resolution sensors. Radars and lidars, for example. And the output of this is a global object list.
So in this example that I'm showing you here, there's actually six lidars that provide coverage around the ego vehicle. Now the grid based tracker uses dynamic cells from the estimated grid map to extract object tracks. And this animation that I'm showing you, shows the results of the tracker in this scenario.
So the panel shows the estimated dynamic map, as well as the estimated tracks of the objects. It also shows you the configuration of the sensors mounted on the vehicles. Now I want you to notice the area encapsulated by these sensors is estimated as gray. And this is what you see in the dynamic map, here is representing that this area is not covered by any of the sensors.
This patch also serves an indication that the ego vehicle's position is where it is on the dynamic map. So that's basically the center portion here, where I was talking about. Now notice that the tracks are also extracted from a dynamic grid cells. And that's why the tracker is able to filter out static objects.
Also note that after a vehicle enters the grid region, it's track establishment takes a few time steps. This is due to two main reasons. First, there's an establishment delay in classifying the classification of the cell as dynamic. Second, the confirmation threshold for the object takes some time to establish as a track is confirmed as an object. Now this type of tracker can be used in other non automotive applications as well, that operate with a 2D grid. For example, a maritime scenario, as one example.
And I spent some time going on radar and also some lidar example. Unfortunately, I don't have time to go through all the examples that I'd like to. But I wanted to share a quick overview of some of the other examples that may match your application. Just so you know that they're there. We have lidar sensing tracking examples. We also have radar and camera specific examples, where we show both extended object tracking algorithms, as well as the conventional tracking, using the DBSCAN clustering techniques that I mentioned a little bit earlier.
And finally, you can find track level fusion algorithms showing vehicle level fusion using radar and lidar. We'll talk some more about that one coming up. And also, vehicle to vehicle fusion, where we're using information from one vehicle and passing it to other vehicles in the system.
OK. That brings us to the next section, which is tracking architectures. Now when you have multiple sensors on a autonomous system, you can design your tracking architecture to use a centralized tracker where all the detections are fed into a tracking algorithm. And while this approach usually produces more accurate information, it has disadvantages, as large data needs to be processed by a single entity. And makes the tracking algorithm computationally more complex. You can also consider fusing data in a decentralized manner. And in this case data from each sensor is processed by a separate tracking algorithm. Then local tracks from each tracker are fused using a track level fusion algorithm. So another key advantage of this approach for autonomous systems, is that it allows you to configure sensor specific tracking algorithms.
For example, a tracker that's tuned for lidar and another tracker tuned for radar. Now I want to show you just a little bit of MATLAB code here, to show you how easy it is to build tracking architectures. We're going to use this type of architecture in our next couple of examples. And you can see in MATLAB, you define a tracking architecture to start, the JPDA tracker is a conventional tracker that we're going to use as one of the inputs. And then we're going to use that for our lidar sensor. And then we're going to add additional tracker for extended object trackers. That's going to take our radar track.
You can see a defined, the tracking architecture overall. Define what tracker one is. Define what tracker two is. I define the inputs. With one line of code, I can display the architecture here. So I can have detections from lidar systems that come into a JPDA tracker that get fused. That would typically be with the post segmentation and post mapping to a bounding box. And also on the radar side, we can take multiple detections in from different radar systems, feed it to a PHD tracker, extended object tracker, and then fuse at the track level here. I'm going to show you an example of that in more detail.
This picture that I'm showing at the top is basically what I showed you in the tracking architecture. It's just a drawn out with a little more detail. So again, I have 2D Radars, feeding unclustered detections into the PHD tracker. And a 3-D lidar with a high resolution point cloud, processed to a bounding box. And then tracking the bounding box using this conventional JPDA tracker. We can fuse those together.
In the first example, we're fusing four 2D radars in a single 360 degree lidar system. And so you can see, it's an automated driving example. The ego vehicle here, is in the middle. This is the front view. This is the rear view. This is the view from each of the sensor modalities lidar and radar. And what you'll see is, I'll play this out. But what you can see is that as this runs through here, we can plot here the lidar track, the radar track, and the fuse track. And when I show you the metrics you'll see how they relate. And how they compare to each other. This corner case is happening right here, where there's a segmentation confusion on the lidar sensor, is going to come up into the next piece. So we'll look some more at this in detail in the next slide.
The second example that we've applied this architecture to, is an urban air mobility example. And here, we've got a ego UAV that has a radar and a lidar on it. Similar to the automotive example. It's flying through an urban area. And there's actually three other objects that are flying through here. Three other different types of UAVs or drones that are in the same scene.
Let's take a look at what that looks like as it's playing out. You can see the ego vehicle here, is coming and it's illuminating the area with lidar and radar. As this plays out, you can see on top here the track versus ground truth. And what each of the sensors sees, compared to what's happening. And on top, kind of a zoom in down here on these plots, you can see exactly what the lidar bounding box looks like, what the radar measurements look like, and also what the fused value looks like. And it's an important view to look at because you can see exactly how each of the autonomous systems are viewed by the ego system. And it really depends on the field of view, how the sensor sees it, and what kind of data is being generated.
So let's take a look at this in a little more detail. I have a snapshot of each of these in the next slide. So for our automotive case, the concern we had was that when this, most of the time the lidar is going to return a better answer than radar. For this scenario, it's higher resolution, there's no obstacles in the way, the weather is fine. But what happens here, is that as these two vehicles are passing each other, you can see that the lidar segmentation piece has some trouble. And so, for this specific example, the lidar track is in the middle here. Now that kind of corresponds to the portion here where the lidar jumps up in this time period here. At the same time, the radar track, which is shown in the blue 2D rectangle here, is actually producing a better result than lidar for that case.
Now the fused dancer on the bottom here, is this yellow line. And what's nice to see is that the fused answer is better than the individual sensors. And that's good to see. Similar on the other side, when we look at the whole scene played out, you can see that the purple line here is the fused radar lidar line. That's the lowest line down here. So that's good to see, that the fused answer is stronger than the individual sensors. This is important because in some of the cases, the lidar didn't have a good read on the target. And so, it just shows that the combination of the two are better than the individual ones.
I want to extend the discussion for the automated driving example. So we want to look and see that, is the architectural choice that we made, the proper one. Is this the one that we should use for this scenario. And so this picture that you see here on the right, I'm going to call that architecture A. And that'll be sort of the baseline. This is what we started with, but let's see if there's a better option.
In the tracking architecture system, essentially this is what that system looks like. We have four radars that are feeding a tracker, an extended object tracker in a single lidar that's feeding that tracker. And they're fused at the track level. We have two variants that we consider our alternate B and alternate C. With alternate B, we have trackers dedicated to each sensor. And then we're fusing the results centrally. And in the other case, we have trackers at each of the radars. We're fusing those tracks. And then a lidar has a tracker. And we're fusing this architecture. So we'll call this A, B, and C.
And we can run the results here. And for this scenario that we talked about, we can look at a GOSPA metric like this, where again, where lower is better. Where we see the alternate architectures that we looked at. And for this, the good news is that for this scene, the blue indicates that the architecture A is still the best choice. This is where, the places where this rises, is exactly where we had those vehicles passing. So this is good to see.
All right. I'd like to now talk a little bit about integrating tracking with motion planning. The tracking algorithms along with the data processing, pre-processing algorithms are typically categorized in the perception layer. In this layer, they're really responsible for providing situational and self-awareness, as we talked about.
What we want to do now, is help integrate that stage with the next stage in planning. So this is a breakdown of the workflow that I just showed you, but a little bit more detail. And you can see this connection between the perception layer and the behavioral layer is really where we want the integration to happen. So we'll see the focus of this next example. That focus is on the integrating the perception and planning functions. When we're able to account for the dynamic environment and planning can be greatly improved when we do this.
So we've got two examples to take a look at. And this will help us understand how tracking and planning can be integrated. In general, situational awareness can be provided from tracking algorithms to motion planners, in two common formats. The first one is in an object list format. And this is when a tracking algorithm shares the track list with the planning algorithms.
This tracklist tells the planner about how many objects are present in the scene, their current state, and also predictions about their future trajectory. On the right, you can see the second format is based on grid maps. Now this is an object free model, where the surroundings are represented by a 2D grid, containing occupancy. And maybe a velocity estimate of the surroundings. Both formats have advantages and disadvantages, as you'd expect. You can also use a hybrid approach, where object level estimate is extracted from the dynamic grid. The grid based trackers in the toolbox allow you to obtain both grid and object level estimates of the environment.
So let's start with an object list example. Now here, are the object list and future predictions for motion planning are typically estimated by a multi object tracker. The multi object tracker accepts data from sensors and estimates the list of objects that are there. So in the track community, we refer to this list of objects as the track list.
Let's look at an example that uses radar and camera sensors. And we'll estimate the track list using a JPDA multi object tracker. The first step towards using any multi object tracker is defining the object state. And how the state evolves with time. And we refer to this as a state transition model. And how the sensor perceives it, which relates to the measurement model.
Now common state transition models include things like constant velocity, constant acceleration, and models like that. In the presence of a map information, in this case, a road network, we can use that information to integrate that into our motion model.
Let's take a look at what that looks like. We can use the Frenet coordinates to describe the object state. And this is a good choice of coordinate systems in modeling object motion because it allows you to integrate the highway reference path into the multi object tracking framework. The integration of the reference path acts as an additional set of information for the tracker. And it allows the tracker to improve the current state estimates, as well as the predicted trajectories of the objects. You can obtain measurement models by first transforming the object state into Cartesian positions and velocities, and then converting them to the respective measured quantities, such as azimuth and range.
Now as I noted when we started this section our goal is to integrate the output of the tracker with the planner. The motion planner uses a planning horizon, in this case, of five seconds, and considers three modes for sampling trajectories for the ego vehicle. Cruise control, lead vehicle follow, and basic lane change. We can find non-colliding trajectories. And in this case, the collision checking is performed in the entire planning horizon every half second.
In this animation, you can observe the planned ego vehicle trajectories, highlighted in the green color. The animation also shows all other sample trajectories for the ego vehicle. And for these other trajectories the colliding trajectories are shown in red. Unevaluated trajectories are shown in gray. In the kinematically and feasible trajectories are shown in cyan color. Each track is annotated by a ID representing its unique identity. And note that the ego vehicle successfully maneuvers around the obstacles in the scene. Our goal here is to make the tracker more accurate with these road integrated motion models.
Notice that the trajectory on the left plot predicted the green vehicle to the right of the blue ego vehicle. Now this predicted trajectory follows the lane of the vehicle because the road network information is integrated into the tracker. Now, if instead, you use the constant velocity model, for example, the predicted trajectory would follow a direction of the instantaneous velocity. And therefore, it will be falsely treated as a collision by the motion planner. In this case, the motion planner can possibly generate an unsafe maneuver, which would be bad.
Notice also on the right plot. The yellow vehicle on the right side of the ego vehicle initiates a lane change. And intends to enter the lane of the ego vehicle. Notice that it's predicted trajectory captures this maneuver. And here, the tracker predicts it to be in the same lane as the ego vehicle at the end of the planning horizon. Now this prediction informs the motion planner about a possible collision with the vehicle. So the planner first proceeds to test the feasibility for the ego vehicle to change the lane. But the presence of the purple vehicle on the left, and its predicted trajectory caused the ego vehicle to make a right lane change. You can also observe these colliding trajectories colored is red in the snapshot, here. It's important to understand tracking imperfections for planning.
Here, a multi object tracker can miss objects, report false tracks, or sometimes report redundant tracks. Now in the figure on the left, the tracker drops tracks on two vehicles in front of the ego vehicle due to occlusion. You'd see that represented with these two objects. In this particular situation, these missed targets are less likely to influence the decision because of the distance from the ego vehicle. But if we look at what happens on the right, as the ego vehicle approaches these vehicles, their influence on the ego vehicle, in the ego vehicle's decision increases in importance.
Notice here that the tracker is able to establish track on these in time to recover. So it didn't detect them. And as it gets closer to them, and they're no longer occluded, we have to take into account how long it takes to reestablish track on those objects. And then make sure that there's enough lead time to get the planner to ensure that there's not a unsafe change.
Now earlier in the webinar, I showed you a grid based tracker that can be used in this dense urban environment. And the introduction to this section on integrating trackers and planners using dynamic occupancy grid maps, we talked about grid based trackers that can provide the planners with dynamic occupancy grid estimate of the surroundings. So on the screen here, you'll see the blue autonomous system in the top left portion. How it navigates to its end goal. And we can analyze the results from this local path planning algorithm and show the predictions from the map. And we can see how it assisted the planner. This animation shows the results of the planning algorithm during the entire scenario. And note that the ego vehicle successfully reaches desired destination, and maneuvered around the different dynamic objects whenever necessary. And the plant trajectory of the ego vehicle passes through the occupied space of the region.
So what's nice about this is we're going to take a look at this on the next slide here. And it's easier to see. We could take a closer look to see what exactly, what's happening. You could see the predicted cost map, a prediction steps, along with the planned position of the ego vehicle on the trajectory. And that's shown by this green line at different delta Ts.
Now the predicted cost map is inflated to account for the size of the ego vehicle. So if a point object representing the origin of the ego vehicle can be placed on the occupancy map without any collision, then that can be interpreted as an ego vehicle does not collide with any obstacle. That's due to the padding that's there.
The yellow regions on the cost map denote areas where the guaranteed collisions with an obstacle. So the collision probability decays outside the yellow regions exponentially until the end of the inflation region. The blue regions indicate areas with zero probability of collision, according to the current prediction. Now notice that the yellow region representing the car in front of the ego vehicle moves forward on the cost map as the map is predicted in the future.
OK. Now I want to talk about out of sequence measurements. So anytime multiple sensors are being used for situational awareness, there's the possibility of an out of sequence measurement. An out of sequence measurement or OOSM for short, is a measurement that has taken in the past, but arrived at the tracker after the tracker is already updated to a later timestamp. Now, there's a lot of different reasons this can happen. Sensors can have inherent signal processing lags, data transfer delays between sensors, message buffers and distributed systems, or updating the tracker at a constant rate regardless of when the sensor data arrives, to name a few.
There are multiple methods to handle an out of sequence measurement. The simplest one is to just ignore it. Ignoring its a good idea if there's little or new information in it. But it's a bad idea as you can imagine, if it provides unique information, like identification or a new coverage area, for example.
Another common technique is to buffer the measurements until all of them are there. The main disadvantage of this is the tracker may be running behind real time. So for safety critical applications, this may reduce safety margins. At the top right here, the most computationally intensive is to reprocess whenever it arrives.
So here, the tracker rolls back to the state just before the out of sequence one. And reprocess all the detections since then. This is the most accurate method, but also the most expensive in terms of memory and power.
Then finally, retrodiction. And this is a technique where we predict back to the out of sequence measurement time, and then the out of sequence measurement is used to update the current track state, and the track covariance.
This is an example where we compare the neglect technique to the retrodiction technique. And you can see the trouble when we ignore a measurement here, on this second track. You can see this area right here, where we don't have a track until very late in the game. Whereas on the side here, these are handled in a much smoother manner. And you actually have the detections in the field of view with the retrodiction.
This last section now, I want to talk a little bit about deployment. All the tracking algorithms in the toolbox support C and C++ code generation. I mentioned earlier, their white box, meaning they're all written in MATLAB, but you can see it.
The other point that I want to mention is for a subset of the filter tracking filters and tracking algorithms, you can generate non-dynamic memory allocation code. So if the processor you're targeting doesn't have dynamic memory allocation, you have options for these trackers and filters. And also, these are strict single-precision.
I also want to mention Simulink here, because many controls engineers use this as a platform for development. We have a lot of blocks in the library that you can find in these sub-libraries in the toolbox. One advantage of using Simulink in this case, you can design and test small subsystems and integrate them into a larger model.
You can also compare design approaches and algorithm options. Here, I show a variant subsystem, which is in Simulink, which gives you some different algorithm alternatives. These maps are one of the examples I went through earlier, where you can use the variant subsystem for easy, what if analysis in your system.
Well that brings me to the end of the webinar. And I want to thank you for your time and your attendance today. I hope you found this helpful. There's a lot of information in here. So I'm hoping it inspires you to take a look at some of the examples in more detail.
And in summary, I think the MathWorks tools help with perception design, and with the focus on autonomous systems. You can model and simulate at all stages of your project. I listed again, some of the tool boxes that could be useful to get started with, depending on your application. There's a lot of resources to get started with. All the examples that I talked about today are shipping. They're on our website. You can see them even without installing them. We also have a series of Tech Talks and other great short videos that get you started on these different applications. So with that, I want to thank you again. Please stay on the line for a Q&A session. I'll get to your questions here in a minute. And I appreciate your time and attention today. Thank you very much.