State Estimation for Continuous-Time Systems with Perspective - - PDF document

SLIDE 1

1

• Dept. Electrical and Computer Engineering

University of California Santa Barbara, CA 93106, USA

State Estimation for Continuous-Time Systems with Perspective Outputs from Discrete Noisy Time-Delayed Measurements

CDC’04 – 43rd IEEE Conference on Decision and Control December 14-17, 2004 Paradise Island, Bahamas

António Pedro Aguiar

aguiar@ece.ucsb.edu

João Pedro Hespanha

hespanha@ece.ucsb.edu

Motivation – indoors robot navigation

CCD camera (sensor)

• provides image coordinates of

visual landmarks

• one image every 33ms

(time-stamped)

• variable delay 20-100ms

(image processing)

• some images without landmarks

Wheel encoders (sensor)

• provide position/orientation with

respect to initial configuration

• significant drift (low frequency error)
• one measurement every 100ms

with 16-28ms (known) delay (from embedded µcontroller) Wheel motors (actuator)

• independent wheel velocities
• delay of 5ms

Activmedia Pioneer 2

SLIDE 2

2

Motivation – landing on vision acquire site

Frog UAV (I. Kaminer, NPS) CCD IR camera (sensor)

• provides image coordinates of

landing strip

• one image every 33ms

(time-stamped)

• variable delay
• landing strip not on all images

Inertial sensor & GPS

• GPS provides position with respect

to earth coordinate system (low sampling rate)

• INS provides data at high sampling

rate-frequency data with significant drift (low frequency error)

• Systems with perspective outputs
• State estimation

– The observer equations – Estimator convergence

• Pose estimation of autonomous vehicles using CCD cameras
• Simulation and Experimental results

– Experimental setup – Motion estimation – Output feedback control (parking experiment)

Outline

SLIDE 3

3

Process model

This system can describe the kinematic model of a rigid-body whose outputs are the (homogeneous) image coordinates of N fixed points provided by an on-board camera. State-affine system with multiple perspective outputs – Normalization constraint (specifies αj)

disturbance input (cannot be measured) measurement noise jth perspective

• utput

constant vector inspired by the (single

• utput) perspective

systems introduced by Ghosh et al JMSEC’94 scalar

Process model

– Discrete & time-delayed measurements

generally a strict subset because not all measurements are available simultaneously discrete measurement

State-affine system with multiple perspective outputs – Normalization constraint (specifies αj)

disturbance input (cannot be measured) measurement noise jth perspective

• utput

scalar delay

SLIDE 4

4

state-transition matrix of

Process Model

jth discrete & delayed perspective output scalar

State-affine system with multiple discrete & delayed perspective outputs

Problem statement

jth discrete & delayed perspective output scalar

Goal: Design an optimal observer to estimate the continuous time state vector x(t), given the discrete time-delay measurements yj(ti). State-affine system with multiple discrete & delayed perspective outputs

SLIDE 5

5

Minimum-energy state estimator [Mortensen’68] state value for which the measured

• utputs can be made compatible with

the system dynamics for the “smallest” noise and disturbance

Minimum-energy state-estimation

in purely continuous or discrete-time gives rise to Kalman-like filter…

jth discrete & delayed perspective output scalar

State-affine system with multiple discrete & delayed perspective outputs

The observer equations

The exact estimate of the state is obtained as the solution to the impulse system Proof: Show that the cost to be minimized can be written as follows…

piecewise constant

SLIDE 6

6

Problem: Under what conditions the state estimate converges to the true state x

• Assuming that P −1 remains uniformly bounded, there exist positive constants

c, r < 1, γd, γ1, ... γN such that

• P −1 remains uniformly bounded provided that there exist positive constants

N, ε such that

Estimator convergence

ISS-like bound state estimation error trajectory-dependent grammian state transition matrix of

Goal: estimate the position & orientation of autonomous vehicle using onboard CCD camera that observes the apparent image motion of stationary points. The position can be estimated using

homogeneous image coordinates of the point Qj (qb≡body, qi≡inertial) configuration of the body frame with respect to the camera frame camera intrinsic parameters

Vehicle motion estimation using vision

kinematics

SLIDE 7

7

Experimental results

• Pioneer 2-DXE mobile robot from ActivMedia

– two front wheels powered by independent reversible-DC motors – one passive rear caster – Sony EVI D30 pan-tilt-zoom (PTZ) color video camera

Visual landmark Q1 Q2 Q3 Q4

Simulation results

The vision sampling interval is T = 0.4s and the time-delay is τ = 0.2s. The estimation errors only reduce when the visual landmarks are in the camera's field of view.

20 40 60 80 100 120 140 160 180 200

• 2

2 4 time [s] x error [m] 20 40 60 80 100 120 140 160 180 200

• 6
• 4
• 2

time [s] y error [m] 20 40 60 80 100 120 140 160 180 200

• 1
• 0.5

0.5 1 time [s] z error [m] 20 40 60 80 100 120 140 160 180 200 10 20 30 time [s] θ error [degree] 20 40 60 80 100 120 140 160 180 200 1 2 3 4 5 time [s] σmin (P) 20 40 60 80 100 120 140 160 180 200 10 20 30 40 50 time [s] σmax (P) 20 40 60 80 100 120 140 160 180 200

• 2
• 1.5
• 1
• 0.5

0.5 1 time [s] σ

measurements available no measurements

σmin[P] σmax[P] x estimation error y estimation error z estimation error θ estimation error Following a circular path experiment

SLIDE 8

8

50 100 150 200 1 2

time [s] σ

min (P) 50 100 150 200 500 1000

time [s] σ

max (P) 50 100 150 200

• 100

100

time [s] pan angle

50 100 150 200

• 0.5

0.5 1 1.5

time [s] γ

Experimental results

50 100 150 200 200 400

time [s] Point 1

50 100 150 200 200 400

time [s] Point 2

50 100 150 200 100 200 300

time [s] Point 3

50 100 150 200 100 200 300

time [s] Point 4

50 100 150 200

• 1

1

time [s] x hat [m]

50 100 150 200

• 2
• 1

1

time [s] y hat [m]

50 100 150 200 0.5

time [s] z hat [m]

50 100 150 200

• 200

200

time [s] θ hat

Following a circular path experiment

σmin[P] σmax[P]

x estimate y estimate z estimate θ estimate measurements

Output feedback control (experimental results)

Minimum-energy state estimator & pan controller & point stabilization controller (Aicardi et al, IEEE Rob. & Autom. Mag., 95).

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8

• 0.5
• 0.4
• 0.3
• 0.2
• 0.1

0.1

x [m] y [m]

Parking experiment

10 20 30 40 50 60

• 1.5
• 1
• 0.5

time [s] x [m] 10 20 30 40 50 60

• 0.5

0.5 1 time [s] y [m] 10 20 30 40 50 60

• 1

1 2 time [s] z [m] 10 20 30 40 50 60

• 2
• 1

1 time [s] θ [rad] 10 20 30 40 50 60 0.02 0.04 0.06 0.08 time [s] u1 [m/s] 10 20 30 40 50 60

• 0.15
• 0.1
• 0.05

0.05 0.1 time [s] u2 [rad/s] 10 20 30 40 50 60

• 30
• 20
• 10

10 20 time [s] pan [degree]

x estimate y estimate z estimate θ estimate left wheel velocity right wheel velocity camera pan angle

SLIDE 9

9

We addressed the position/orientation estimation for autonomous vehicles using onboard cameras that observe the apparent motion of stationary points (landmarks). We formulated the problem in the framework of state estimation of a system with perspective outputs with measurements are noisy, arrive at discrete-time instants, suffer delays, and may not be complete. We designed a dynamical impulsive system that produces an estimate

• f the state that is “most compatible” with the dynamics, in the sense

that it requires the least amount of noise energy to explain the measured output. The convergence of the proposed estimator system was analyzed and illustrated through computer simulation and experimentally.

Conclusions