Single Camera Structure and Motion Estimation - Visual Servoing via Advanced Numerical Methods - page 214

· e ω =( K ω + I 3 × 3 ) e ω ( t )+ t
t 0

+ t

( K ω + I 3 × 3 ) e ω d τ

t 0 ρ ω sgn ( e ω ) d

τ (12.46)

3 × 3 3

where K ω , ρ ω ∈ R are positive constant diagonal gain matrices, and e ω ( t ) ∈ R

quantifies the observer error as

e ω ( t ) e ω − e ω .

A Lyapunov-based stability analysis is provided in [8] that proves
· e ω ( t )

· e ω ( t )

− → 0 as t → ∞ , (12.47)

and that all closed-loop signals are bounded. Based on (12.44) and (12.47), the
angular velocity can be determined as

· e ω ( t )

( t )= L − 1

ˆ

ω as t → ∞ . (12.48)

ω

˜ 3 ( t ) T

3

˜ ω

1 ( t ) ˜

ω

2 ( t ) ˜

ω

An angular velocity estimation error ω ( t ) ∈ R is de-

fined as

ω ˜

i ( t )=

ω

i ( t )

− ω ˆ

i ( t )

, ∀ i = { 1 , 2 , 3 }. (12.49)

As shown in [8], the angular velocity estimator, given by (12.46), is asymptotically
stable, thus the angular velocity estimation error ω( t ) → ˜ 0as t → ∞ .

12.4.2.2 Step 2: Structure Estimation

This section presents an estimator to estimate the structure and motion. One of the
linear velocities is assumed to be known and provides the scene scale information
to the structure and motion estimator.
The observed states y 1 ( t ), y 2 ( t ), u 1 ( t ), u 2 ( t ) and u 3 ( t ) are generated according to
the update laws
· y 1 = u 1 −

ω 1 +(1 + y 1 ) ˆ y 1 y 3 b 3 − y 1 y 2 ˆ ω 2 − y 2 ˆ ω 3 + ρ 1 e 1 (12.50)

· y 2 = u 2 −

(1 + y 2 ) ˆ ω 1 + y 1 y 2 ˆ ω 2 + y 1 ˆ y 2 y 3 b 3 − ω 3 + ρ 2 e 2 (12.51)

· u 1 = y 3 b 3 u 1 +( y 2 ˆ

y 1 ˆ

ω 1 − ω 2 ) u 1 + q ( u 1 ) u 1 (12.52)

ρ 1 e 1 )+ y 1 ρ 4 ( · e 6 +

ρ 6 e 6 )
c 1

ρ 4 ( · e 1 + + + e 1

· u 2 = y 3 b 3 u 2 +( y 2 ˆ

y 1 ˆ

ω 1 − ω 2 ) u 2 + q ( u 2 ) u 2 (12.53)

ρ 2 e 2 )+ y 2 ρ 5 ( · e 6 +

ρ 6 e 6 )
c 1

ρ 5 ( · e 2 + + + e 2

· u 3 = q ( y 3 b 3 ,

t ) b 3 + ρ 6 e 6 (12.54)