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