ADAS Algorithm Design and Prototyping - MathWorks · •Horizontal FoV: 360 ... Sensor fusion algorithm for FCW Sensor Fusion Kalman Filter MIO: Most-Important Object Risk Assessment

1© 2016 The MathWorks, Inc.

ADAS Algorithm Design and PrototypingForward Collision Warning Example

Seo-Wook Park, Principal Application Engineer

Mark Corless, Principal Application Engineer

2

•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

3

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

?

4

The MATLAB environment helps ADAS engineers …

Gain insight by replaying and visualizing logged vehicle data

Reduce time on the road by synthesizing data to test algorithms

Speed up prototyping of algorithms by generating code

5


Synthetic

data

C Code


Generate code

Integrate

with embedded

environment

Logged

vehicle

data

ADAS

algorithm


Refine algorithm

yes

no

Expected

behavior

?

6

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

7

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

8

• 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

9



• Filtering


Data

Pre-processing

Zoning

Path

Estimation

Path

Estimation


Vision

Object

Radar

Object

Vision

LD

Vehicle

CAN

Sensor

Fusion

Kalman

Filter

Sensor Fusion

& Tracking


Threat

Assessment

Risk

Assessment

Maneuver

Analysis

Zoning

Find

MIOFCW

Kalman

Filter

10



• Filtering


Data

Pre-processing

Zoning

Path

Estimation

Path

Estimation


Vision

Object

Radar

Object

Vision

LD

Vehicle

CAN

Sensor

Fusion

Kalman

Filter

Sensor Fusion

& Tracking


Threat

Assessment

Risk

Assessment

Maneuver

Analysis

Zoning

Find

MIOFCW

Kalman

Filter

11

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

12

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

13



• Filtering


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


Threat

Assessment

Risk

Assessment

Maneuver

Analysis

Zoning

Find

MIOFCW

Kalman

Filter

14

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

15

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

16

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™

17

FCW



• Filtering


Data

Pre-processing

Zoning

Path

Estimation

Path

Estimation


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

18

Synthetic

data


Logged

vehicle

data

ADAS

algorithm

C Code



Refine algorithm

Generate code

Integrate

with embedded

environment

yes

no

Expected

behavior

?

19

Example synthetic test scenarios – EuroNCAP

ENCAP – CCRs ENCAP – CCRm ENCAP – CCRb

stationary

50kph

50kph

20kph

50kph

50kph

@ -2m/s2

50kph

50kph

@ -6m/s2

ENCAP – CCRb

40m

12m

Ego car

Target car

20

Example synthetic test scenarios – Fail Operation

FO1 – cornering FO2 – overtaking FO4 – lane change

60kph

30kph130kph

90kph

stationary

50kph

Ego car

Target car

21

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

22

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

If building for multiple users, it

is important to select an

approach which is scalable,

maintainable, and testable

23

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

24

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.

25

Architecture we selected to build a data synthesis tool

Synthesized

Data

Data Model

(Programmatic Interface)

Controls state of data

• Specifies scenario (i.e. road,

vehicles, waypoints, sensor models)

• Synthesizes data to test algorithm

• Enables automation

1

26


Synthesized

Data

Data Model


App

(Graphical Interface)






1

Controls state of interface

• Reacts to user input

• Updates data model

• Draws user interface and

visualizations

• Simplifies interaction with

data

2

27


Synthesized

Data

Data Model


App


Unit Test






1





visualizations


dataAutomates regression testing

• Increases likelihood of catching

issues introduced during iterations

• Facilitates collaboration

3

2

28


Synthesized

Data

Data Model


App


Unit Test






1





visualizations


dataAutomates regression testing

• Increases likelihood of catching

issues introduced during iterations

• Facilitates collaboration

3

2

29

Synthetic

data


Logged

vehicle

data

ADAS

algorithm

C Code



Refine algorithm

Generate code

Integrate

with embedded

environment

yes

no

Expected

behavior

?

30

Generate C code for your algorithm with MATLAB Coder

Software in the loop &

Processor in the loop

enabled by

Embedded Coder

MATLAB

C

31

Create components in MATLAB and reuse them in Simulink

32

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

33

Automate regression testing with Simulink Test

Specify tests and Interact with results

in the Test Manager

Generate a report to share results

34

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

35

Synthetic

data

Example workflow for ADAS algorithm development

Logged

vehicle

data

ADAS

algorithm

C Code



Refine algorithm

Generate code

Integrate

with embedded

environment

yes

no

Expected

behavior

?

36

What key product features enabled this workflow?

Design Kalman filters with Computer Vision System Toolbox

Encapsulate data with MATLAB Classes

Test MATLAB code with MATLAB Unit Test

Generate C code from your algorithm with MATLAB Coder

Verify generated code using SIL with Embedded Coder

Collect model and code coverage with Simulink Verification and Validation

Automate regression testing and reporting with Simulink Test

37

How can you get started?N

um

ber

of

Users

Complexity

Documentation and

Training Services can

help you learn how to use

product features

Consulting Services can help

you select and establish a

baseline architecture to meet the

needs of your application

Consulting helped us

select an architecture for

our Traffic Scenario App

38

The MATLAB environment helps ADAS engineers …




Documents

ADAS Algorithm Design and Prototyping - MathWorks · •Horizontal FoV: 360 ... Sensor fusion algorithm for FCW Sensor Fusion Kalman Filter MIO: Most-Important Object Risk Assessment