Multiple Model Techniques in Estimation and Control

1
Multiple Model Techniques in Estimation and
Control

• Derek Caveney, Ph.D.
• Center for Collaborative Control of Unmanned
  Vehicles (C3UV)
• University of California, Berkeley

2
Presentation Overview

• Multiple Models in State Estimation
• Application to BMW target tracking research
• Incorporation of multiple targets and multiple
  sensors
• Connecting Multiple Model State Estimation to
  Control
• Application to Adaptive Cruise Control systems

3
A Multisensor Multitarget Tracking System for
Driver Assistance Systems

• In cooperation with
• BMW Forschung und Technik

4
Problem Statement

• Develop a real-time, multiple target tracker
  that can track objects 360deg around a test
  vehicle using measurements from heterogeneous
  sensors with asynchronous sampling times.

NOT TO SCALE
5
BMW Forschung und Technik (ZT)
ZT-3

• Driver assistance
• Active safety
• Sensor data fusion

• Parking-aid
• Pre-crash
• Lane change assist
• Stop-and-Go ACC

6
The Tracking Routine Attributes

• Routine is probabilistic and incorporates
  uncertainty in both target motion and target
  measurement. Its attributes include
• Both measurement origin and accuracy
  uncertainties are considered
• Target motion uncertainty is approached using
  multiple models
• A random quantity of randomly ordered returns
  arriving from multiple sensors, asynchronously,
  is accepted
• Possible dropouts, clutter, and multiple
  measurements of a single target are handled.

7
Multiple Model (MM) Estimation

• Problem Statement
• Given all noise-corrupted measurements, Zk, up
  to the current time step, k, estimate the current
  state, xk, of the plant, P. Furthermore, the
  actual operating mode of the plant is unknown at
  any time step. Thus, incorporate multiple models,
  which are connected by an underlying Markov
  Chain, so that at all times there exists one
  model, mik, that closely represents the current
  operating mode of the plant.
• Goal For each discrete sampling time, k, find

8
A Particular MM Estimation Diagram
zk

Mixing
Filter 1
µ1k
xO,1k
xhat,1k
xhat,1k-1 µ1k-1
Filter 2
µ2k
xO,2k
xhat,2k
xhat,2k-1 µ2k-1
xhatk
zk
Filter r
µrk
xhat,rk
xO,rk
xhat,rk-1 µrk-1
9
Interacting Multiple Model Approach

• The approach involves invoking the Total
  Probability Theorem to incorporate prior
  knowledge of mode transitions and share
  information between the filters.
• Subsequent moment matching prunes the number of
  mode transition possibilities to explore.
• Different multiple model routines perform this
  step at different points. The Interacting
  Multiple Model (IMM) algorithm does moment
  matching prior to the filtering step and thus
  limits the number of filters in the bank to the
  number of models incorporated.

10
IMM Simplifying Approximation

• The moment matching is conducted by
  approximating the following mixed Gaussian prior
  density with a single Gaussian density of the
  same mean and covariance.

11
IMM Complexity and Optimality

• Complexity
• Almost linear in the number of models
• Slight computational overhead for Mixing and
  Probability Update functions
• Optimality
• Each filter may be a Kalman Filter, each optimal
  in the MMSE sense, but the convex combination of
  outputs most likely will not be.
• However, the goal is not optimality. Having
  multiple models should reduce state estimation
  errors over all operating modes of the system.
  Furthermore, depending on the system and models
  incorporated, the model probabilities, µi , can
  be used to interpret the current system mode.

12
Simulated Single Target Tracking

• 20 Monte Carlo simulations of a lane change
  scenario were performed

13
Autobahn Target Performs Lane Change
14
Block Diagram of the Tracking Routine
IMM Block
State estimates for established target
Model j Kalman Filter
State estimate covariances for established target
Sensor measurements
Model probabilities for established target
j 1r
15
Multiple Target Tracking

• Probabilistic Data Association (PDA) Filter
  replaces Kalman Filter
• Generates elliptical gating regions about the
  predicted measurement using innovation
  covariances. Associates measurements within
  these gates to particular targets.
• Combines multiple measurements into a combined
  innovation by weighting each measurement by its
  distance from the predicted measurement
• Also accounts for the possible lack of
  measurements at a particular time step

16
Validation Regions
17
Experimental Results w/ Multiple Targets
18
Block Diagram of the Tracking Routine
IMM block for nth established target
State estimates for EACH established target
Model j PDAF
State estimate covariances for EACH establish
target
Sensor measurements
Model probabilities for EACH established target
j 1r
Unassociated measurements
Routines for initiation and deletion of
established tracks
n 1N
Targets w/o associated measurements
New established targets
19
Multiple Ranging Sensor Fusion

• Sequential Filtering
• Different qualities and quantities of information
  available from different sensors
• Have a sequence of PDA filters running through
  sensor measurements from worst sensor to best
• Complexity grows by the product of the number of
  sensors and the number of models
• Measurement update time can vary across different
  sensors and all sensors may not detect the same
  targets at any given time

20
Block Diagram of the Tracking Routine
Tracking block for nth established target
State estimates for each established target
Sensor 1
Multiple Model
State estimate covariances for each establish
target
Sensor s
Bus
Model probabilities for each established target
Sensors measurements
j 1r
Unassociated measurements
Sensor S
Routines for initiation and deletion of
established tracks
n 1N
Targets w/o associated measurements
New established targets
21
Single versus Tandem Radar Sensors
Ellipses represent 90 confidence region for
target location
22
Long-Range Laser Radar (Lidar)
Short-Range Laser Radar (Lidar)
Short-Range 24GHz Radar
20deg
Max 20m
Max 60m
57deg
14.4deg
Max 165m
Max 60m
57deg
Max 20m
20deg
NOT TO SCALE
23
(No Transcript)
24
Final System Performance

• 34 targets simultaneously tracked
• Continuous 360deg tracks for individual targets
• 4 or 6 state dimension
• Real-time activation/deactivation of sensors
• Up to three heterogeneous sensors per sector
• Option for single model PDAF

25
Multiple Model Techniques in Automotive
Estimation and Control

• How do we connect multiple model estimation to
  control?
• Doctoral thesis under the supervision of
• Prof. J. Karl Hedrick

26
MM Control Block Diagram
Plant f(x,u)
z
r
u
MM State Estimator
u1(x)
p1
u2(x)
p2
p
xhat
ur(x)
pr
MM Controller
27
Multiple Model (MM) Control

• Problem Statement
• Given a set of (state-feedback) stabilizing
  controllers that are each appropriate for a given
  plant at different times, for what switching
  signals, p(t), is closed-loop stability
  guaranteed. Conversely, given arbitrary
  switching signals, p(t), what limitations does
  that put on the set of controllers we can
  incorporate.
• Goal Guaranteed closed-loop stability for the
  plant, P, using a weighted control law, u(t)
  ?ui(x)pi(t), where pi(t) ? 0,1 and ?pi(t) 1,
  for all i and t.
• Note This includes blending the control inputs,
  as well as discretely switching between
  controllers (i.e. hybrid control).

28
Linear Results Summary

• Convex combinations of stabilizing controllers
  are not necessarily stabilizing
• Sets of stabilizing controllers can stabilize a
  system if discrete switches are of low frequency.
  Hespanha and Morse 1999
• CL stable for all p(t) if a common Lyapunov
  function can be found. Determine if all (Hurwitz)
  ACL commute Narendra 1994 or solve a
  Semi-Definite Program (SDP) for the feasibility
  problem, Does there exist a P such that ACL,i P
  P ACL,i lt 0 for all i ?.
• CL stable for all p(t) such that where
  Pi s are solutions of the CT Lyapunov equation
  ACL,i Pi Pi ACL,iQi , Vi(x) xPix and
  Vdot,i(x) lt -cix2

29
MM Sliding Control

Sliding Control Define a sliding surface, s,
that must be relative degree one and s ? 0 is the
control goal. By the choice of our control
input, we force the following sliding surface
dynamics to happen Multiple sliding controllers
? switch between different values of ? Theorem
Given a set of values for ? such that all values
are positive and each produces a stabilizing
controller for the nonlinear system, any convex
combination of the values in this set will also
result in a stabilizing sliding controller.
30
Cruise Control Application

The switching signal, p(t), will arrive form the
multiple model tracker and be computed from the
maneuver probability, µi , and the state estimate
covariance, Pi , of each maintained
target. where i is the number of vehicles
currently being tracked. The variable p(t) can
be considered an environment probability that
reflects the degree of dynamics and uncertainty
in the current driving environment. Therefore,
the multiple controllers will represent a
tradeoff between safety and ride comfort in the
ACC application.
31
Conclusions

• Multiple Model Estimation approaches are
  appropriate when
• A system exhibits multiple modes of behavior and
  the current behavior can switch quickly.
• Multiple Model Control approaches are appropriate
  when
• The operator wishes to control a system
  differently at different times or in different
  operating environments.

32
Collaborative Multi-Agent Control and Ground
Target Tracking

• In cooperation with the
• Office of Naval Research

33
Demonstrated Capabilities of Platform

• A multi-aircraft system where each aircraft has
  its own on-board autonomy and the fleet is
  operated by a single user.
• Task allocation and conflict resolution performed
  in the air.
• Inter-vehicle communications using the 2.4GHz ISM
  band. Mission plans and states shared among all
  aircraft.
• Ground-to-Aircraft communications necessary only
  for relaying new mission plans from mission
  control.
• Vision-in-the-loop navigation of locally-linear
  features

34
Current Collaborative Architecture

• Mission statements are relayed from the ground to
  all aircraft.
• Each plane without a task picks the closest
  available task for itself. Each plane allocates
  tasks only for itself.
• Conflicts in task allocation are resolved using
  Euclidean distance.
• Each aircraft broadcasts its current state and
  its knowledge of other vehicles states. It only
  has overwriting permissions for its own state.
• More involved communication, tasking, and
  conflict resolution protocols are currently under
  development for future system integration

35
Current Collaborative Architecture
Task 1
Border Patrol
Allocated Task 1
Task 2
Location Visit
36
Aircraft Level Architecture
Payload Responsible for Relaying Commands between
the PC-104 and Mission Control
Piccolo Responsible for Relaying Commands between
the PC-104 and Aircraft Avionics
Database Permits inter-process communication
Vision Processing Frame Grabbing Capabilities
Orinoco Inter-vehicular communication protocol
Switchboard Task Allocation, Conflict Resolution,
and Controller Switching
Vision Control For Turn-Rate Based Path Following
Waypoint Control For Single-Point Visits
Orbit Control For Closed-Loop Multi-Point Paths
37
System Level Architecture
Mission Control
Ground-to-Air Communications
Air-to-Air Communications (802.11b)
Collaboration
Collaboration
Switchboard
Switchboard
UAV Piccolo Autopilot
UAV Piccolo Autopilot
Aircraft 1
Aircraft 2
38
Mission Control

• Mission control is the single operator interface
  with the fleet of UAVs.
• Mission Programming, Transmission, and Monitoring
  are performed within the Mission Control GUI.
• Available resources and permissible tasks are
  checked against the desired commanded mission.

39
Mission Control
40
(No Transcript)
41
Process Control

• Process Control is C3UVs development interface
  with the fleet.
• From within the Process Control GUI, researchers
  can
• adjust design parameters
• enable and disable processes
• trigger telemetry logging
• receive valuable debugging feedback from the
  fleet
• while the planes are in flight.

42
(No Transcript)
43
Current UAV Platform Configuration

• Wing-Mounted Camera allowing for vision-based
  control, surveillance, and obstacle avoidance
• Ground-to-Air UHF Antenna for ground operator
  interface
• GPS Antenna for navigation
• 802.11b Antenna for A-2-A comm.
• Payload Tray for on-board computations and
  devices
• Payload Switch Access Door for enabling /
  disabling on-board devices

44
Current Payload Configuration

• Off-the-shelf PC-104 with custom Vibration
  Isolation for on-board recording.
• Orinoco 802.11b Card and Amplifier for A-2-A
  comm.
• Analog Video Transmitter for surveillance
  purposes
• Printed Circuit Board for Power and Signal
  Distribution among devices.
• Umbilical Cord Mass Disconnect for single point
  attachment of electronics to aircraft.
• Keyboard, Mouse, Monitor Mass Disconnect for
  access to PC-104 through trap door while on the
  ground.

45
Vision-based California Aqueduct Following
46
Geometric Path Planning for Closely Spaced
Targets

• Grouping targets into feasible groups for each
  pass of the UAV
• Caveney, D, and Hedrick, J.K. Path Planning for
  Targets in Close Proximity with a Bounded
  Turn-Rate Aircraft. Accepted to AIAA GNC
  Conference 2005, San Francisco, CA.

47
Geometric Path Planning for Closely Spaced
Targets
48
Multiple Model Obstacle Detection and
Probabilistic Map Making

• In cooperation with the
• NASA
• And
• Scientific Systems Company, Inc.

49
Obstacle Detection

• Goal To develop algorithms for unmanned
  rotorcraft for the detection of static, moving,
  and pop-up obstacles. Subsequently, use this
  information for the path planning and obstacle
  avoidance.
• IMM-based obstacle detection using two models.
  One hypothesizing an obstacle is present one
  hypothesizing an obstacle is not present
• Kang, Y., Caveney, D., and Hedrick, J.K. An
  IMM-based Obstacle Detection Routine for
  Autonomous Rotorcraft. Proc. of ASME IMECE 2004,
  Anaheim, CA.

50
Probabilistic Mapping

• Goal To take point mass target estimates and
  build a global obstacle map
• Means and covariances of target state estimates
  are used to generate and update quadtree-based
  global map.

Caveney, D., Kang, Y., and Hedrick, J.K.
Probabilistic Mapping For Unmanned Rotorcraft
Using Point-Mass Targets and Quadtree Structures.
Proc. of ASME IMECE 2005, Orlando, FL.
51
Probabilistic Mapping
52
Questions