elective in roboticselective in robotics - state estimation (m. vendittelli) 7attitude estimation...

Elective in Robotics

State Estimation (Marilena Vendittelli)

Elective in Robotics - State Estimation (M. Vendittelli) 2

• Sensors

•Key state estimates

• attitude

•velocity

•position

Outline


basic instrumentation for state estimation

• IMU• barometer

common augmentations

• sonar, laser, infrared (for eight measurements)• monocular camera

less common equipment

• RGB-D sensors (like Kinect)• scanning laser range finders• GPS• VICON

Sensors (1/4)


Sensors (2/4) IMU & barometer

camerasonar


IMU

Humming Bird (IMU strap down configuration)

Sensors (3/4)

(2) accelerometer (x,y,z)

gyroscope

(3) yaw

(4) pitch

(5) roll


IMU

Sensors (4/4)

3D-MAG: three-axis compass

Humming Bird


Attitude estimation

rate gyro

• measures the angular velocity of SRB relative to SRI, expressed in SRB

SRISRB

⌦IMU

= ⌦+ b

⌦

+ ⌘ 2 SRB

a

IMU

= R

T (v � g

!zI

) + ba + ⌘a 2 SRB

u = [uThu

Te ]

T v = [vThv

Te ]

T

uh = 1p2b(Fh + bVh) ue =

1p2b(Fe � bVe)

vh = 1p2b(Fh � bVh) ve =

1p2b(Fe + bVe)

1

2

RvTvdt 1

2

RuTudt

v(t) = S(t)u(t) ) F (t)� bV (t) = S(t)[F (t) + bV (t)]

v(s) = S(s)u(s) ) F (s)� bV (s) = S(s)[F (s) + bV (s)]

||S(s)|| 1

||S(s)|| = sup! �

1/2max

{S⇤(j!)S(j!)} 1

⇢x = f(x) + g(x)uy = h(x) x 2 R

n, u 2 R

m, y 2 R

m

⇢x

1

= f

1

(x1

) + g

1

(x1

)u1

y

1

= h

1

(x1

)

⇢x

2

= f

2

(x2

) + g

2

(x2

)u2

y

2

= h

2

(x2

)

⇢u

1

= ± y

2

+ v

1

u

2

= ⌥ y

1

+ v

2

1

magnetometer

⌦IMU

= ⌦+ b

⌦

+ ⌘

⌦

2 SRB

a

IMU

= R

T (v � g

!zI

) + ba + ⌘a 2 SRB

' � T

m

!zI

� T

m

DR

Tv

m

IMU

= R

T I

m+Bm + ⌘m 2 SRB

a

IMU

' � T

m

R

T !zI

˙R = R(⌦

IMU

� b)⇥ � ↵

˙b = kb↵

↵ = (ka

g

2

((RT !zI

)⇥ a

IMU

) +km

|Im|2 ((RT I

m)⇥m

IMU

)⇥ + . . .

I

a

IMU

= Ra

IMU

' Ra

IMU

' �gR

!zI

�gRDR

Tv

+

v = �1

g

(RDR

T )�1(IaIMU

+ gR

!zI

)

˙v = �g(R

!zI

+RDR

Tv)� kw(v � v)

1

constant or slowly time-varying bias

measurement noise

local magnetic disturbance

• provides measurements of the ambient magnetic field


Attitude estimation SRISRB

accelerometer

• measures the instantaneous linear acceleration of SRB due to exogenous forces⌦

IMU

= ⌦+ b

⌦

+ ⌘

⌦

2 SRB

a

IMU

= R

T (v � g

!zI

) + ba + ⌘a 2 SRB

m

IMU

= R

T I

m+ bm + ⌘m 2 SRB

a

IMU

⌘ � T

m

R

T !zI

˙R = R(⌦

IMU

� b)⇥ � ↵

˙b = kb↵

↵ = (ka

g

2

((RT !zI

)⇥ a

IMU

) +km

|Im|2 ((RT I

m)⇥m

IMU

)⇥ + . . .

u = [uThu

Te ]

T v = [vThv

Te ]

T


1p2b(Fe � bVe)


1p2b(Fe + bVe)

1

2

RvTvdt 1

2

RuTudt



||S(s)|| 1

||S(s)|| = sup! �

1/2max

{S⇤(j!)S(j!)} 1

1

near hovering, i.e.,

⌦IMU

= ⌦+ b

⌦

+ ⌘

⌦

2 SRB

a

IMU

= R

T (v � g

!zI

) + ba + ⌘a 2 SRB

' � T

m

!zI

� T

m

DR

Tv

m

IMU

= R

T I

m+ bm + ⌘m 2 SRB

a

IMU

' � T

m

R

T !zI

˙R = R(⌦

IMU

� b)⇥ � ↵

˙b = kb↵

↵ = (ka

g

2

((RT !zI

)⇥ a

IMU

) +km

|Im|2 ((RT I

m)⇥m

IMU

)⇥ + . . .

u = [uThu

Te ]

T v = [vThv

Te ]

T


1p2b(Fe � bVe)


1p2b(Fe + bVe)

1

2

RvTvdt 1

2

RuTudt



||S(s)|| 1

||S(s)|| = sup! �

1/2max

{S⇤(j!)S(j!)} 1

1

constant or slowly time-varying biases

measurement noise

blade flapping

and induced drag

B

v ⇡ 0

z = x2 � ↵(x1)

x1 = 0 x2 = ↵(x1) ↵(0) = 0 g1(x1) 6= 0 g2(x1, x2) 6= 0 8(x1, x2)

x1 = f1(x1) + g1(x1)x2

x2 = f2(x1, x2) + g2(x1, x2)x3

.

.

.

xn = fn(x1, . . . , xn) + gn(x1, . . . , xn)u

x1 = f1(x1) + g1(x1)x2

x2 = f2(x1, x2) + g2(x1, x2)u

m

˙V I = �TRe3 + F e(V I ,˙V I ,⌦,

˙⌦,d(t)) +R⌃R ⌧

˙R = R S(⌦)

J ˙⌦ = �S(⌦)J ⌦+ ⌧ + ⌧ e(V I ,˙V I ,⌦,

˙⌦,d(t)) + ⌧ g +⌃T Te3

⌧1 ⌧2 ⌧3 ⌧ �!T ⌦

!ib

!jb

!kb

C ˙q

mg|dB = �|F (j!⇡)|dB m' = ⇡ + F (j!c)

m'1

mg! = 0

�! = 0

+! = �1 ! = +1 ✏ ! 0

F (s) =

K

s(1 + ⌧s)

x

NF = n

+F

1


state observer

SRISRB

⌦IMU

= ⌦+ b

⌦

+ ⌘

⌦

2 SRB

a

IMU

= R

T (v � g

!zI

) + ba + ⌘a 2 SRB

m

IMU

= R

T I

m+ bm + ⌘m 2 SRB

a

IMU

⌘ � T

m

R

T !zI

˙R = R(⌦

IMU

� b)⇥ � ↵

˙b = kb↵

↵ = (ka

g

2

((RT !zI

)⇥ a

IMU

) +km

|Im|2 ((RT I

m)⇥m

IMU

)⇥ + . . .

u = [uThu

Te ]

T v = [vThv

Te ]

T


1p2b(Fe � bVe)


1p2b(Fe + bVe)

1

2

RvTvdt 1

2

RuTudt



||S(s)|| 1

||S(s)|| = sup! �

1/2max

{S⇤(j!)S(j!)} 1

1

Attitude estimation

complementary filter: uses high frequency part of gyro and low-frequency of accelerometer and magnetometer

possible contributions from other sensors


hyp.: horizontal flight

SRISRBVelocity estimation

⌦IMU

= ⌦+ b

⌦

+ ⌘

⌦

2 SRB

a

IMU

= R

T (v � g

!zI

) + ba + ⌘a 2 SRB

' � T

m

!zI

� T

m

DR

Tv

m

IMU

= R

T I

m+ bm + ⌘m 2 SRB

a

IMU

' � T

m

R

T !zI

˙R = R(⌦

IMU

� b)⇥ � ↵

˙b = kb↵

↵ = (ka

g

2

((RT !zI

)⇥ a

IMU

) +km

|Im|2 ((RT I

m)⇥m

IMU

)⇥ + . . .

I

a

IMU

= Ra

IMU

' Ra

IMU

' �gR

!zI

�gRDR

Tv

+

v = �1

g

(RDR

T )�1(IaIMU

+ gR

!zI

)

˙v = �g(R

!zI

+RDR

Tv)� kw(v � v)

1


Position estimation

• height and position in the plane are often decoupled

• absolute: barometer, limited information from IMU, GPS, VICON,...

• relative: acoustic, laser-ranging or infrared, RGB-D cameras, SLAM

• measures provided by sensors are often fused in a Kalman filter

Elective in Robotics - Quadrotor Modeling (M. Vendittelli) 12

• R. Mahony, V. Kumar, P. Corke, "Multirotor Aerial Vehicles: Modeling, Estimation, and Control of Quadrotor," IEEE Robotics & Automation Magazine, vol.19, no.3, pp. 20-32, Sept. 2012.

• R. Mahony, T. Hamel and J. M. Pflimlin, "Nonlinear Complementary Filters on the Special Orthogonal Group," in IEEE Transactions on Automatic Control, vol. 53, no. 5, pp. 1203-1218, June 2008. doi: 10.1109/TAC.2008.923738

References

elective in roboticselective in robotics - state estimation (m. vendittelli) 7attitude estimation...

Documents