Post on 05-Jun-2020
1© 2016 The MathWorks, Inc.
Developing Reliable ADAS and Autonomous Driving
Functions in MATLAB and Simulink
Marco Roggero
Senior Application Engineer
marco.roggero@mathworks.de
2
The MathWorks environment helps ADAS engineers …
Gain insight by replaying and visualizing logged and synthetic
vehicle data
Efficiently model components and develop processing algorithms
Speed up prototyping of algorithms by generating code
3
Agenda
Workflow for ADAS algorithm development
-Forward Collision Warning example
-Sensor Fusion techniques
Sensor components modelling and simulation
-Radars
-Lidars
-Cameras
Ground Truth Labeling
Integration in Model Based Design workflow
4
•Worst-case
scenarios
•Scenarios identified
from real world test
drive data
How do I know my ADAS algorithm is robust enough?
• OEM specific
test scenarios
• Fail Operation
test scenarios
5
Synthetic
data
Example workflow for ADAS algorithm development
Logged
vehicle
data
ADAS
algorithm
C Code
Create new traffic scenario or refine sensor model
Drive and collect more vehicle data
Refine algorithm
Generate code
Integrate
with embedded
environment
yes
no
Expected
behavior
?
6
Gain insight by replaying and visualizing logged vehicle data
Synthetic
data
C Code
Create new traffic scenario or refine sensor model
Generate code
Integrate
with embedded
environment
Logged
vehicle
data
ADAS
algorithm
Drive and collect more vehicle data
Refine algorithm
yes
no
Expected
behavior
?
7
Test vehicle equipped with sensors
Delphi ESR
• 76GHz electronically scanning radar
• Dual FoVs, 90x60m, 20x174m
• CAN interface
Mobileye 560
• FoV: 38x150m
• CAN interface
Mobileye
560
Delphi
ESR
Point Grey Blackfly
• Stand “ice cube” vision camera
• 800x600, 27FPS
• GigE interface
XSENS MTI-G-700
• Stable and sensitive
• MEMS-based AHRS
• USB interface
Velodyne LiDAR HDL-32E
• Horizontal FoV: 360
• Vertical FoV: +10..-30
• Range: 80..100m
• 100 Mbps Ethernet
8
Example test scenarios in public road
01_city_c2s_fcw 02_city_stopngo 03_local_streetParking
04_highway_cornering 05_highway_lanechange 06_highway_cutin
9
• Calculate Ground
Speed
• Object classification
• Filtering
• Offset Compensation
Zoning
Path
Estimation
Vision
Object
Radar
Object
Vision
LD
Vehicle
CAN
Sensor fusion algorithm for FCW
Sensor
Fusion
Kalman
Filter
MIO: Most-Important Object
Risk
Assessment
Maneuver
Analysis
Zoning
Find
MIOFCW
Kalman
Filter
Data
Pre-processing
Path
Estimation
Sensor Fusion
& Tracking
Threat
Assessment
10
• Calculate Ground
Speed
• Object classification
• Filtering
• Offset Compensation
Data
Pre-processing
Zoning
Path
Estimation
Path
Estimation
Sensor fusion algorithm for FCW
Vision
Object
Radar
Object
Vision
LD
Vehicle
CAN
Sensor
Fusion
Kalman
Filter
Sensor Fusion
& Tracking
MIO: Most-Important Object
Threat
Assessment
Risk
Assessment
Maneuver
Analysis
Zoning
Find
MIOFCW
Kalman
Filter
11
• Calculate Ground
Speed
• Object classification
• Filtering
• Offset Compensation
Data
Pre-processing
Zoning
Path
Estimation
Path
Estimation
Sensor fusion algorithm for FCW
Vision
Object
Radar
Object
Vision
LD
Vehicle
CAN
Sensor
Fusion
Kalman
Filter
Sensor Fusion
& Tracking
MIO: Most-Important Object
Threat
Assessment
Risk
Assessment
Maneuver
Analysis
Zoning
Find
MIOFCW
Kalman
Filter
12
Sensor fusion Sensor Fusion
Birds-Eye View object notations
Radar object (stationary)
Radar object (moving)
Vision object
Fused object (safe zone)
Fused object (warn zone)
Fused object (FCW zone)
Fused object (most important object)
Calculate Ground Speed
Object classification
Filtering
Offset Compensation
Data
Pre-processing
Zoning
Path
Estimation
Path
Estimation
Vision
Object
Radar
Object
Vision
LD
Vehicle
CAN
Sensor
Fusion
Kalman
Filter
Sensor Fusion
& Tracking
Kalman
Filter
13
Sensor Fusion
Made Easy by MATLAB CVST
Vision
Object
Radar
Object
Radar
VisionR1 R2 Rm
V1
V2
Vn
Radar
costMatrix
Assignments
V1 + R2
V2 + R1
Vn + Rm
Fusion
𝑓(𝑉1) + 𝑓(𝑅2)𝑓(𝑉2) + 𝑓(𝑅1)
𝑓(𝑉𝑛) + 𝑓(𝑅𝑚)
Fused Object List
Computer Vision System Toolbox™
[assignments, unassignedVisions, unassignedRadars] = ...assignDetectionsToTracks(costMatrix, param.costOfNonAssignment);
Pairs of visions and
associated radars
14
• Calculate Ground
Speed
• Object classification
• Filtering
• Offset Compensation
Data
Pre-processing
Zoning
Path
Estimation
Path
Estimation
Object Tracking by Kalman Filter
Vision
Object
Radar
Object
Vision
LD
Vehicle
CAN
Sensor
Fusion
Kalman
Filter
Sensor Fusion
& Tracking
MIO: Most-Important Object
Threat
Assessment
Risk
Assessment
Maneuver
Analysis
Zoning
Find
MIOFCW
Kalman
Filter
15
What is the Kalman Filter?
It is an iterative mathematical process that uses a set of equations and
consecutive data inputs to quickly estimate the true value, position, velocity,
etc. of the object being measured, when the measured values contain
random noise.
Estimated
temperature
Actual temperature
Measured
temperature
16
Challenges in object tracking
𝑥ሶ𝑥𝑦ሶ𝑦
State Matrix
17
Kalman Filter
0x̂
0P
Initial state
& covariance
1ˆ
kx
1kP
Previous state
& covariance
Pk
APk1A
TQ
(1) Predict state based on physical model and previous
state
(2) Predict error covariance matrix
Time Update (“Predict”)
K k Pk
H
T(HPk
H
TR)
1(1) Compute Kalman gain
(2) Update estimate state with measurement
Measurement Update (“Correct”)
ˆ x k ˆ x kKk(zk Hˆ x k
)
(3) Update the error covariance matrix
Pk (IK kH) Pk
kkk vHxz
Measurementkx̂
kP
Output of
updated state
1 kk
Current becomes previous
R : Sensor noise covariance matrix (measurement error)
K : Kalman gain
uw
][ TE wwQ
: Control variable matrix
: Process (state) noise
][T
kkk E eeP kkk xxe ˆ
: Process (state)
covariance matrix
(estimation error)
: Process noise
covariance matrix
v
H
: Measurement noise
k minimize P
A : State matrix relates the state at the
previous, k-1 to the state at the current, k
: Output matrix relates the state to the
measurement
kkkk wBuxAx
1ˆˆ
From sensor spec or experiment
18
Kalman Filter Made Easy by MATLAB CVST
0x̂
0P
Initial state
& covariance
1ˆ
kx
1kP
Previous state
& covariance Time Update (“Predict”)
Measurement Update (“Correct”)Current Measurement
Output of
updated state
1 kk
Current becomes previous
[z_pred,x_pred,P_pred] = predict(obj)
z_pred : prediction of measurementx_pred : prediction of stateP_pred : state estimation error covariance
at the next time step
[z_corr,x_corr,P_corr] = correct(obj,z)
z_corr : correction of measurementx_corr : correction of stateP_corr : state estimation error covariance
z
x_corrP_corr
Predicted state
x_pred
kalmanFilterSysObj = vision.KalmanFilter(A,H,'ProcessNoise',Q,'MeasurementNoise',R)
Computer Vision System Toolbox™
19
FCW
• Calculate Ground
Speed
• Object classification
• Filtering
• Offset Compensation
Data
Pre-processing
Zoning
Path
Estimation
Path
Estimation
Sensor fusion algorithm for FCW
Vision
Object
Radar
Object
Vision
LD
Vehicle
CAN
Sensor
Fusion
Kalman
Filter
Sensor Fusion
& Tracking
Risk
Assessment
Maneuver
Analysis
Zoning
Find
MIO
Threat
Assessment
Kalman
Filter
20
Synthetic
data
Reduce time on the road by synthesizing data to test algorithms
Logged
vehicle
data
ADAS
algorithm
C Code
Create new traffic scenario or refine sensor model
Drive and collect more vehicle data
Refine algorithm
Generate code
Integrate
with embedded
environment
yes
no
Expected
behavior
?
21
Common approaches to synthesizing test data
“Buy It” Buying “Off the shelf” solutions like PreScan and CarMaker enable you to
author traffic scenarios and synthesize sensor data
“Build It” Building it yourself enables you to control the level of
fidelity/complexity appropriate for your application
22
ADAS System Level Simulation – Lane Keeping Support
23
Vision-
Algorithm
Control-
Algorithm
Model-Based Design for ADAS & Automated Driving
Radar-
Algorithm
Se
ns
or
Fu
sio
n &
Sit
ua
tio
na
l A
na
lys
is
Camera
Sensor
Traffic
Infrastructure Vehicle Dynamics
time
delay
be
at fr
eq
ue
ncy
fre
qu
en
cy
Sensor Data Algorithm Control Algorithm
Radar
Sensor
24
Example synthetic test scenarios – intersection and stop & go
Intersection – turning w/ braking Intersection – near crash Intersection – U-turn Stop & go @ 3020kph
50kph
Ego car
Target car
25
Key components defining a Traffic Scenario
Define Road• Road type
• NumLanes
• LaneWidth
Define Vehicles• Ego vehicle
• Target vehicles
• NumTargetsDefine Trajectories
• Path type
• Waypoints
• Velocity, acceleration
• Heading angle, yawrate
Sensor Model• Vision, Radar
• Range, FoV
• Position, Velocity error
• NumCluster
26
Example radar spec
Reference) Stanislas, Leo & Peynot, Thierry, “Characterisation of the Delphi Electronically Scanning Radar for robotics applications”. In Australasian Conference
on Robotics and Automation (ACRA 2015), 2-4 December 2015, Canberra, A.C.T.
27
Agenda
Workflow for ADAS algorithm development
-Forward Collision Warning example
-Sensor Fusion techniques
Sensor components modelling and simulation
-Radars
-Lidars
-Cameras
Ground Truth Labeling
Integration in Model Based Design workflow
28
FMCW Radar: How Does it Work?
Frequency modulated source (chirp)
– Saw tooth, triangular, custom sweep
Direct RF conversion (homodyne)
MIMO: multiple antennas
PA
LNA
Target =
Delay
DSP
RF
time
delay
be
at
fre
qu
en
cy
fre
qu
en
cy
29
What Behavior Can Be Modeled?
PA
LNA
DSP
Determination of waveforms (range,
resolution)Algorithms for data analysis
Channel modeling
(interference, noise)
Modeling of antenna arrays
(no of elements, position)
Modeling of RF Impairments
(noise, non-linearity, frequency dependency)
30
Which MathWorks Tools Can Help?
PA
LNA
DSP
Phased Array System Toolbox
Signal Processing Toolbox
Instrument Control Toolbox
Phased Array System Toolbox
DSP System Toolbox
Communications System Toolbox Phased Array System Toolbox
Antenna Toolbox
SimRF
31
What is LiDAR
LiDAR: Light Detection And Ranging
Measures distance by illuminating environment with laser light
Provides highly accurate distance measurements
Image courtesy Wikipedia (link)
32
Why use LiDAR for Autonomous Driving
Account for limitations of vision and radar sensors
Cameras perform poorly in bad weather or
limited visibility.
Radar not efficient at detecting object classes.
33
Why use LiDAR for Autonomous Driving
LiDAR provides accurate distances and enables terrain and surface mapping
Effective for autonomous path planning
34
Key Takeaways
1. Single environment for:
– Data visualization and analysis
– LiDAR algorithm development
– Test and verification
2. Easiest way to get started with LiDAR processing
3. Integrate with autonomous robotic workflows
35
Developing Active Safety Systems
Developing the Vision Algorithm Part
MATLAB based workflow allows
– Easy to debug scripting language
– Pixel level, 2D, 3D visualization
– Easy-to-use powerful image processing, and computer vision algorithms
36
Agenda
Workflow for ADAS algorithm development
-Forward Collision Warning example
-Sensor Fusion techniques
Sensor components modelling and simulation
-Radars
-Lidars
-Cameras
Ground Truth Labeling
Integration in Model Based Design workflow
37
Stream Processing in MATLAB
System Objects:
• make stream processing easier and more efficient
• can accelerate your MATLAB code
• are very easy to create, configure and execute
• support floating and fixed-point data types as well as automatic C/C++ and
HDL code generation
• Can be easily converted into Simulink blocks
38
Working with System Objects
39
How to create test bench in MATLAB
Initialize
Terminate
Processing
loop
%% Create and Initialize
SamplesPerFrame = 1024;
Fs = 44100;
Microphone=dsp.AudioRecorder('SamplesPerFrame',SamplesPerFrame);
Spectra=dsp.SpectrumAnalyzer ('SampleRate',Fs);
%% Stream processing loop
tic;
while toc < 20
% Read frame from microphone
audioIn = step(Microphone);
% View audio spectrum
step(Spectra,audioIn);
End
%% Terminate
release(Microphone)
release(Spectra)
40
41
Work on all the
data at once…
Batch processing
All the data
Deliver all at once
42
Systems Objects enable stream processing
Each frame available as soon as processed
– Reduced memory footprint
– Near real-time / Life analysis
Typical applications
– Communications Systems
– Audio / video signal processing
– Data acquisition
Incremental delivery
43
Stream processing in MATLAB
Streaming techniques* process continuous data from a captured signal or large file by dividing it
into “frames” and fully processes each frame before the next one arrives
Memory efficient
Streaming algorithms in DSP System Toolbox provide
Implicit data buffering, state management and indexing
Simulation speed-up by reducing overhead
MATLAB Memory
Stream
Source
Stream
Processing
44
Where to find System Objects?
Communications System Toolbox
DSP System Toolbox
Phased Array System Toolbox
Computer Vision System Toolbox
45
Signal processing example for algorithm’s -
performance comparison
46
Compare algorithm execution time with different
implementations :
------------------------------------------------------------------------------------------------------------------
Versions of the Algorithm | Elapsed Time (sec)| Acceleration Ratio
________________________________________________________________________________________
1. Traditional MATLAB functions | 18.0953 | 1.0000
2. System Objects in-lieu of MATLAB functions | 3.6914 | 4.9020
3. MATLAB Coder MEX version | 1.5241 | 11.8725
-------------------------------------------------------------------------------------------------------------------
47
Accelerating algorithm execution
Parallel
Computing
MATLAB to C
User’s Code
System
Objects
Optimize
MATLAB CodePre-allocation and vectorization
Parallel computations on multicore
computers, GPUs, and clusters
Pre-defined efficient implementations of
algorithms
Generate MEX files automatically with
MATLAB Coder
48
Options to speeding up MATLAB algorithms:
1,00
1,87
1,06
2,03
4,77
9,07
1,00
1,93
6,78
13,29
11,94
22,24
1,00
1,93
3,62
7,12
73,20
129,20
0,00 20,00 40,00 60,00 80,00 100,00 120,00 140,00
Toolbox Functions
Toolbox Functions + PCT (parfor)
System Objects
System Objects + PCT (parfor)
System Objects + Code Generation
System Objects + Code Generation +
PCT (parfor)
Maximum-Likelihood Equalizer
Voice Pitch Estimation
DCT-based Image Compression
PCT: Parallel Computing Toolbox
Dual core machine used for this example
Faster
49
Create components in MATLAB and reuse them in Simulink
50
Enable coverage of MATLAB and C code with
Simulink Verification and Validation
Collects coverage on MATLAB code
in “Normal” mode
Collects coverage on generated C
code in “Software in the loop” mode
51
Automate regression testing with Simulink Test
Specify tests and Interact with results in the Test Manager
Generate a report to share results
52
Prototype on hardware with Simulink Real-Timeas seen in today’s Test drive your ADAS algorithms presentation
Algorithm Models
Vehicle and
Environment
Models
Forward
Collision
Warning
Autonomous
Emergency
Braking
53
Agenda
Workflow for ADAS algorithm development
-Forward Collision Warning example
-Sensor Fusion techniques
Sensor components modelling and simulation
-Radars
-Lidars
-Cameras
Ground Truth Labeling
Integration in Model Based Design workflow
54
The MathWorks environment helps ADAS engineers …
Gain insight by replaying and visualizing logged and synthetic
vehicle data
Efficiently model components and develop processing algorithms
Speed up prototyping of algorithms by generating code
55© 2016 The MathWorks, Inc.
Developing Reliable ADAS and Autonomous Driving
Functions in MATLAB and Simulink
Marco Roggero
Senior Application Engineer
marco.roggero@mathworks.de