Localization with a Nonlinear, Noninvertible Sensor Mapping

pillowfistsΤεχνίτη Νοημοσύνη και Ρομποτική

13 Νοε 2013 (πριν από 3 χρόνια και 9 μήνες)

150 εμφανίσεις

Localization with a
Nonlinear, Noninvertible
Sensor Mapping

Aaron Becker

237a Everitt Lab

11/4/2008

Overview


Problem statement


System model


Sensor model


State estimator


Results

Related Work


Localization via triangulation


(GPS)

P. Enge and P. Misra, “Special issue on GPS: The global positioning system,”, 1999.


(IR signals)

R. Want, A. Hopper, V. Falcao, and J. Gibbons,





“The active badge location system,” 1992.


(Ultrasonic & RF)

N. Prijantha, A. Chakraborty, and H. Balakrishan,




“The cricket location
-
support system,” 2000.


(RF signals)

T. W. Christ, P. Godwin, and R. Lavigne,






“A prison guard duress alarm location system,” 1993.


Map
-
based localization


(LIDAR & ceiling image)

S. Thrun, M. Bennewitz, W. Burgard, et. al,





“Minerva: A second generation museum tour
-
guide robot,” 1999.


(Light intensity)

N. Ravi and L. Iftode, “Fiatlux: Fingerprinting rooms using light intensity,” 2007.


(GSM and K
-
NNSS fingerprinting)

V. Otsason, A. Varshavsky, A. LaMarca, and E. de Lara,


”Accurate GSM indoor localization,” 2005.


WiFi localization


(NNSS)

P. Bahl, V. Padmanabhan, and A. Balachandran,





“A software system for locating mobile users,” 2000.


(Blueprints and transmitter locations)

M. Robinson and I. N. Psaromiligkos,




“Received signal strength based location estimation of a wireless LAN client,” 2005.


(Gaussian Processes)

B. Ferris, D. Hahnel, and D. Fox,





“Gaussian processes for signal strength
-
based location estimation,” 2006.


System Model

2
(2),
,
k k
k k
x
SE y
v


 
 
 
 
 
 
 
 
 
 
x x
u u
System State (pose)
Control Inputs
Derivation of control inputs


v
System Evolution

State update equation:

t
v
t

t
t


t
t


x
t
x
System
H
x
z
u
+
+
F
G
Z
-
1
,
2 2
,,,,
,
,~ (0,),~ (0,),...
x k
k y k x k x k x k
k
n
n assume n n N n N i i d
n
 

 
 
 

 
 
 
n
















1,
1
1
1
1
1
1
1
1
g
sin sin
cos co
f
s
k k k
k
k k k
k
k
k
k k k k
k
k
xk
k
k
k
k
k
k
k
k
y
k
n
n
x
y
v
t
x
t
n
v
y
t

  

  













  
 
  
 
 
 
 
 
      
 
 
 
 
 

 

 
 
 

 
 

 
 
 
 

 
x x u
x
n
x
Localization: Problem Statement







0
0 0 0 0
0 0
0 0
0 0
0
0
,,[0,2 )
~ (,)
T T
k k k k
k k k k
X Y X Y
X x
Y y
f
else
x y
X Unif a b


 
    


 

 


 


 
 
 



   
X
X X

Localization
from a known
starting point



Localization
from an unknown
starting point
0
0
,
~ (,),
~ (0,2 ),
,
Y Unif c d
Unif
a b c d


  


Objective

2
ˆ
min [ ]
k
k k k
k
E
 
x
x x x
x

Minimize the current expected
squared error


At all times, have a state
estimate
close

to the true state

Dead Reckoning

Dead
Reckoning

True Position


Causes of Drift


Model errors


Process noise


Expected Cost

2 2
2
[ ]
ˆ
ˆ
[( ) ] [( ) ]
[ ] [ ]
0
( )
k k
k k k
k k k
k
E
E E
E E
Var
 
 


X X
X X X
X X X
X
The optimal estimate of is
Though the expected error is zero, th
e expected
variance is nonzero and increases wi
th time
Sensor Model

[,]
- -
T
i
RSS MAC address
P
RSS

Every four seconds WiFi dongle
can scan for access points
Receives for each access po
int detected
Ideally, a range to landmark sen
sor















2 2
1 1
2 2
AccessPoint
2 2
2
2 2
4
k landmark k landmark
,i
k landmark k landmark
k
i
k landmark i k landmark i
x x y y
x x y y
R
x x y y
 
 
  
 
 
 
  
 
 
 
 
 
  
 
z
Maximum Received Signal Strength
in Everitt Lab

Commercial scan, January 2008

Actual WiFi Signal

Sensor Model

[,,] [,]
j j j i i
i i AP
RSS MAC RSS
RSS m b
  
x x
x x
Gather data , separate by access point i
nto
For each access point
1.) Fit a linear model to the da
ta
2.) Subtract linear model from t
he data, leav






1
2
2
1
2
-
f( ),~ (0,)
,,
(
i i i AP
T T
k k
i
f
i n
n
z RSS m b
z z
z N
k
 
 

   
 
 
x x
X x x Z
x
x
ing a zero mean data set



3.) Fit a covariance function to
this data set

2
2
2
,
2 2
1
exp
2
,)
(,)
i j
i j
i j
f
f n
i j
i j
i j
k

 

 

 

 

 





 

 
x x
x
K x x
4.) Save the covariance for all d
ata points

Prediction algorithm

Sensor Model
f(z|x)





*
2
* *
* *
1
* *
1
2 2 2
* * *
(,)
T
T
f n
k
 

  




  
x
k x X
k K Z
k K k
Given a new location , generate the expe
cted
value and expected variance
1.)
2.)
3.)
Then the probability of a certain se








2
* *
2
*
* *
* * * * *
2
* * *
*
|;,
1
;,
2
z
RSS
f RSS f z
f z e




 




x
x
nsor
measurement at location is:


Sensor Model


Data Collection

[,,]
j j j
RSS MAC
x
32 access points met the minimum detecti
on threshold
(recorded 4596 data points )
State Estimation



Observers without Probability




Often we have fewer sensors than states or
sensors that do not return our state directly


Observer

Correction Factor
True System
H
x
z
u
+
+
F
G
H
+
+
F
G
x
^


+
H
T
(
HH
T
)
-
1

+
+
z
^
Z
-
1
Z
-
1
e
State Observer Example


With feedback we can ‘estimate’ our state


We can design our observer to converge to the
correct state at any rate if system is:


Stable


Observable


Linear

1
1:1:0
| 1 1:1 1:1 0
| 1:1:0
( |,,)
:
( |,,)
k
k k k
k
k k
k
k
k k
k
f
f

  
X
X U Z X
x u z x
u z x
x
u
z
Given
Previous location probability
New control input
New sensor reading
Calculate
Propagating the probability distribution

1
1:1:0 1
| 1 1:1 1:1 0
| 1:1:0 | 1
( |,,)
:
( |,,) ( |,,)
k
k k k k k k k
k
k k
k
k
k
k k
k
k
k
k
f
f f


  


X
X U Z X X X U Z
u
z
u z
x u z x
u z x x
x x
Given
Previous location probability
New control input
New sensor reading
Calculate with Markov assumption
0
x
1
x
1
u
1
z
2
x
2
u
2
z
1
k

u
k
u
1
k

x
1
k

z
k
x
k
z
0
x
1
x
1
u
1
z
2
x
2
u
2
z
1
k

u
1
k

z
Bayesian Propagation

1
1:1:1
1 1:1 1:1
1:1:
1:1:
1:1:
1
1 1:1 1
1
:
1
1
( |,)
(
( |
( |,)
( |,
( |,)
( |
,)
( |,)
|,)
)
)
k
k k
k k k
k k k
k
k k
k k k k
k k
k k
k
k k k
k
f
new posterior f
prior
f
f
f
f
f
d
f


 

 
 










x X
x u x
x u z
x
x u
x
z
x u z
x u
u z
z
z
x u z
x
Link to Algorithm

Kalman Filter

1
1|
1 1 1
1 1 1 1
k
k k k k
T
k k k
k k
k k k k
k k k k k k


  
   
 
 
 
 
x Fx Gu
P FP F V
x x Re
P P RHP
Two Steps:
1. Predict the state and covariance mat
rix using motion model
2. Correct the state and covariance bas
ed on sensor data


1
1
1
1
1
1
k
k k
T
k
k k
T
k k






 
 

e z Hx
S HP H W
R P H S
where
1
1 1:1 1:
1:1:1
1:1:
1
1
1 1
1
1::
( |,)
( |,
( |,)
(
(
| )
)
| )
|
,
(,)
k
k k
k k k k
k k
k k k
k k
k
k
k
k k
f
f
f
f
f
d
f


  






 

x X
x u x x
x u z
x u z
x u
x u z
z
z x
Kalman Filter, Block Diagram

Update
Error Term
Kalman State Estimate
True System
H
x
z
F
G
H
+
+
F
G
x
^


+
e
+
+
z
^
+
+
w
v
+
+
+
Z
-
1
R
+
+
V
H
(
input
)
H
T
(
H
(
input
)
H
T
+
W
)
-
1
F
(
input
)
F
T
P
(
k
|
k
)
R
×
Z
-
1
+

Z
-
1
×
u
P
(
k
+
1
|
k
)
1
1 1 1
1 1 1 1
1|
k
k k k k
k k k k
T
k k k k k k
k k k
k k

  
   

 
 
 
 
x Fx Gu
x x Re
P P RHP
P FP F V
1. Predict 2. Correct
Kalman Filter Performance

Extended Kalman Filter


Dealing
With Nonlinearities


The Kalman filter is the
optimal linear estimator



The robotic system is nonlinear


System can be linearized


We will still have the best
linear
estimator at the estimated
operating point

EKF algorithm

Particle Filter


We can approximate any
probability distribution with
samples (particles)

6
4
2
2
4
6
4
2
2
4
Particle filters can
represent multimodal
distributions:

1
1 1:1 1:
1:1:1
1:1:
1
1
1 1
1
1::
( |,)
( |,
( |,)
(
(
| )
)
| )
|
,
(,)
k
k k
k k k k
k k
k k k
k k
k
k
k
k k
f
f
f
f
f
d
f


  






 

x X
x u x x
x u z
x u z
x u
x u z
z
z x
Particle Filter Benefits

| { }
:
( | )
k free
C_space k Q
f C

 
X x
x
Incorporating a pdf for
obstacles in the workspace
is a binary evaluation
Particle Filter











,
1
,
,
,
,
,
:
1
_ _,
,
i k
k
i k i k
i k
i k
i k
i k
i k
i k
i k
M
for i to M
x
sample from motion_model
y
x
y




 

 

 
 
 
 
 

 
 
 
p p u
p
p
Two Steps:
Using two sets
1. Predict using motion model
of particles
2. Correct based on sensor data












1
,,
1
k
i k i k
j k
i k j k
for i to M
w measurement_model C_space
for i to M
draw j with probability w





p z
p p
Particle Filters in Action

Here, a uniform
distribution is
approximated (blue)

Global sensors are
used to weigh the
particles


And the distribution
is resampled (red)

Initialization

Particle Filters in Action

Step


Here, a distribution
is propagated
(blue)

Global sensors are
used to weigh the
particles, along
with the
C_space

And then the
distribution is
resampled (red)



Results

The SegMonster can localize and track itself in the hallways of Everitt Lab

Literature Results, Wireless Track
ing Using WiFi Signal Strength
1.09 m SegMonster
2.12 m Gaussian Process, Particle Fil
ter
2.65 m Nearest Neighbor in Signal Sp
ace
3.88 m AP Locatio
ns and blueprints (no data-gathering sta
ge)
Video Of Particle Filter

Dead
Reckoning

True Position

Particle Filter

95% Confidence
Ellipse
(+/
-
2 std)


Video of Particles

Propagate

Update

Incorporating
C
-
space

data

Questions

Dead reckoning: velocities from encoders

Nonlinear observer example

Bayesian methods:


Continuous


Discrete

Particle filtering:


Block diagram


Motion model

SegMonster


Dynamics


Improvements

Potential fields

Multimodal distributions

Wireless mapping (outdoors)


Adaptive Control: parameter estimation/RWP








1,2,1
2 2 1
2,,
1 1 1
mean cov

 
   

 
   
   
 
x
Dimension Parameters Example Representation
Observing With Probability Distributions

Assumption:


our probability is normally distributed


Implication:


position can be represented with just the mean and
covariance

4
2
2
4
6
0.1
0.2
0.3
0.4
1
1 1:1 1:
1:1:1
1:1:
1
1
1 1
1
1::
( |,)
( |,
( |,)
(
(
| )
)
| )
|
,
(,)
k
k k
k k k k
k k
k k k
k k
k
k
k
k k
f
f
f
f
f
d
f


  






 

x X
x u x x
x u z
x u z
x u
x u z
z
z x
Dead Reckoning


Has high dependence on initial
conditions (open loop)

One of Igor's former masters had
made

a tick
-
tock man, all
levers and gearwheels and cranks and clockwork. Instead of a
brain, it had a long tape punched with holes. Instead of a heart,
it had a big spring. Provided everything in the kitchen was very
carefully positioned, the thing could sweep the floor and make a
passable cup of tea. If it
wasn't

carefully positioned, or if the
ticking, clicking thing hit an unexpected bump, then it'd strip
the plaster off the walls and make a furious cup of cat.

Thief of Time
, Terry Pratchet

Bayesian Filters

1 1:1 1:1
1
1:1:
1 1
1:1:
1:
1
1:
1 1:1
1:1
( |,)
(
Bayesi
(
an F
|
iltering(,,)
,)
( |,
1:0
2
|
:for do
3:( |,)
4:
_
,
( | )
5:
( )
)
|,
)
k k k
k k
k
k k
k
k k k
k k
k k
k
k k k
k k
k
k
f
f
unNormalized f
unNormal
al
i
l
zed
f
f
f
d
f

 
  
  


 




 

u z
x X
x u x
x u
x u
z
x u z
z
x u z
x
x
u z
z x
1:
1
1:
1:
:1:
1:1:
1:
(
6:endfor
7:for do
1
8:
9:endfor
10:retur
|
_ ( |,)
_
,
( |,)
)
( |,)
n
k k k
k k k
k k
k k k
k
k
f
unNormalized f
f
all
f


 
x
x
x u z
x
u z
x z
u z
u
X
correction using
effect of mo
sensor
new po
tion
ster
mode
ior
i r
l
pr o







Return to presentation

Discrete Bayesian Filter






















1
1
1:
1 1:1 1
1:1
1
:
1:1
1
:1:1
1:1
1
:
1
:1
|,
|,
|,
|
|,
|
|
|,
,
k
k
k k
k
k k k
k
k
k k
k k
x X
k k k
k k k
x
k
k
k k
X
k
k
P x u z
P x u z
P x u x
P x u z
P x u
P z x
P z x
P x u z
z







  





 

 



Prediction

Correction


Two Steps to propagate belief




sensor
e

ff
mo
ect
del
n
of motio
ew poste
n mod
rior
i
e
pr or
l







Return to Questions

Particle Filter Block Diagram

Weigh
(
for each particle
)
Predict
(
for each particle
)
Error Term
Particle State Estimate
Extract mean
,
or Maximize
Expectation
True System
H
x
z
F
G
H
F
G
x
^


+
z
^
+
+
w
v
+
+
+
Z
-
1
Z
-
1
u
Resample
(
until we have n particles
)
K
()
Draw

i

with probability
w
i
p
[
j
]
t

=
p
[
i
]
t
-
1


+
+
+
V
rand
Mean
Return to Questions

State Estimation Cookbook



Propagates one sample (do this to each sample):

1
1 1
1 1 1
1 1
sample_motion_model(,)
.: ,
.
k k
k k k
k k k k k
k k k
x x x
y y y
  

 
  
 
   
     
   
     
 
   
     
     
   
     
   
u x
u x
this is the code for
1 ) Inputs
2 ) Calculate
change in








1 1 1
2 2
1 1
2 1 1
1 1 1 2 2
3 4 1
atan2,
:
ˆ
.:
ˆ
rot k k k k k
trans k k k k
rot k k rot
rot rot rot trans
trans trans trans rot rot
y y x x
x x y y
sample
sample
 

   
   
     
 
 

   
   
  
  
   
position
3 ) Add noise










2
2 2 1 2 2
1 1
1 1
1 1 2
ˆ
ˆ ˆ
ˆ
cos
.
ˆ ˆ
:
ˆ
sin
ˆ ˆ ˆ
rot rot rot trans
k trans rot
k trans rot
k rot rot
sample
x x
y y
   
  
  
   



  
  
  
  
4 ) Calculate
estimate to return
Effect of Propagation Parameters


Tuning Parameters


α
1
,

α
2
,

α
3
,

α
4


Proportional to

Effects

α
1

rotation

rotation

α
2

translation

rotation

α
3

translation

translation

α
4

rotation

translation

Left:

[0.005, 0.001, 0.46, 0.02]

Right:

[0.01, 0.005, 0.0016, 0.02]

Calculating Errors





























m
i
m
i
i
i
m
1
1
2
1
2
2
2
1
1
2
1
cos
,
sin
2
atan
,
,
,
,
0
cos
cos
,














:
angles

of

set

a

of

mean

:
given
Return to Questions

How to calculate Covariance from
particles


Assumes
w
[
i
]k

is normalized (sums to 1) and that you
have calculated the means for x and y,















y
y
x
x
w
M
to
i
x
for
k
i
k
i
k
i






p
p
cov
cov
0
cov


y
x
,
Return to Questions

How to deal with a Multimodal
probability distribution


The particle filter can represent multimodal
distributions, but the covariance and mean
are now meaningless statistics.


k
-
means algorithm to find k means, and
choose the mean with the highest
probability (or highest average probability)


Better:

use Expectation Maximization
algorithm to identify most probable point.

Return to Questions

Generating Velocities from Encoders


Equations of motion, differential drive robot

[Steve M LaValle, Planning Algorithms pg 728 http://planning.cs.uiuc.edu/, 13.1]







cos
2
2
sin
2
2
l r
l l
k k t
l
l r
r r
k k t
r
r l
r
x u u
u t
r
y u u
u t
r
u u
L

 

 



 

 
 

 
 
Velocity Pose Update
Return to Presentation

SegMonster Dynamics


The objective for the design of the SegMonster was to produce a low cost robotic platform built
to handle both indoor and outdoor environments that could move at the speed of a jogging
human.


These design objectives resulted in a non
-
destructive addition to a standard Segway that
exhibited a certain level of autonomy. Specifically, the SegMonster could follow a sidewalk by
using a color camera to segment between grass and concrete, and follow another SegWay by
tracking a red LED array attached to the lead Segway. The SegMonster was equipped with a
single IR sensor to detect obstacles immediately in front of it and programmed to stop until the
obstacle was removed. This level of autonomy can be called 'behavior level', since the
SegMonster relied entirely on local data (wheel encoders, and the IR sensor) or calculated a
local control law based on the immediate inputs (using the camera to determine whether the
robot was close or far from the sidewalk edge, or visual servo
-
ing based on the size and
location of a red color blob segmented from the camera).



The SegMonster did calculate a position estimate directly from wheel encoder data, which was
reasonably accurate for paths of length less than 20 feet. We were able to use this data to move
the SegMonster in a 5’x5’ box pattern. Though the SegMonster had one global sensor (the
camera), it was used entirely for local based control.



The first controller for the Segmonster used classical techniques


inner PID control loops for
each motor, and outer loop velocity based PI loops for the reference body angle and turning
angle.



Enabling the SegMonster to carryout complex tasks, a global position estimate is necessary



The SegMonster


Moves by tilting forwards/backwards

Comparison to inverted pendulum


θ
is angle from vertical



length to Center Of Mass


M
C

is mass of cart


M
p

mass of pendulum


I inertia of pendulum


x horizontal displacement


of cart

M
C
M
p
,
I
?
x
l
Inverted Pendulum
at rest
Inverted Pendulum perturbed from
equilibrium by
?
The cart cannot move in vertical
direction, so

Sum forces on FBD of cart in
horizontal direction:



Sum forces on pendulum in horizontal
direction



Sub second equation in for N:




Cart and Pendulum forces can be
calculated separately

M
p
,
I
?
l
P
N
mg
x
..
I
?
.
I
?
2
Pendulum
,
and forces upon it
M
C
x
P
N
x
..
bx
.
F
Cart
,
and forces upon it
F
N
x
b
x
M
c










sin
cos
2







c
p
p
M
M
x
M
N





F
M
M
x
b
x
M
M
c
p
p
c









sin
cos
2








Forces perpendicular to pendulum

1.
Forces perp to pendulum

2.
To remove Pan N, sum
the moments about the
COM of the pendulum

3.
Sub (2) into (1)

4.
Group angular
accelerations

5.
Divide by mass of
pendulum

6.
Isolate horizontal
acceleration

7.
Simplify

8.
Near the equilibrium
position, with small
angular accelerations,
the horizontal
acceleration is fixed




M
p
,
I
?
l
P
N
mg
x
..
I
?
.
I
?
2
Pendulum
,
and forces upon it


x
g
x
g
M
I
x
g
M
I
x
g
M
I
x
M
g
M
M
I
x
M
M
g
M
I
I
N
P
x
M
M
g
M
N
P
p
p
p
p
p
p
p
p
p
p
p
p















































































































tan
)
8
(
tan
cos
)
7
(
cos
sin
)
6
(
cos
sin
)
5
(
cos
sin
)
4
(
cos
sin
)
3
(
cos
sin
)
2
(
cos
sin
cos
sin
)
1
(
Equations not entirely accurate


Offset of pivot force


Constant gravity on handle


Offset
x

= 2.25”


Offset
y

= 8.25”


M
p
,
I
?
l
P
N
mg
x
..
I
?
.
I
?
2
Offset
x
Offset
y
t
handle
The Segmonster has a slightly different structure
than the basic inverted pendulum
.
There is a
constant torque due to gravity on the handle
,
and
the pivot point is offset from the center by
(
Offset
x
,

Offset
y
)
Equivalent System


Let theta
2

be the angle to the COM from the
center of the wheel, and l
2

be the distance
from the COM to the center of the wheel

?
l
mg
x
..
Offset
x
Offset
y
t
handle
l
2
?
2
25
.
8
cos
25
.
2
sin
25
.
8
cos
25
.
2
sin

tan
sin
5
.
4
cos
5
.
16
125
.
73
25
.
8
cos
25
.
2
sin
2
2
2
2
2
2
2




































g
x
y
x
y
x
Conversion from motor angle

to body position.

?
a
1
a
2
b
s
L
a
Return to Questions

Potential Fields

Return to Questions

















*
*
*
*
*
,
,,
,,
,
1 1 1
goal
goal goal
goal
att
goal goal
goal goal
goal
rep
q q
d q q d
d
U q
d q q
d q q d
d q q
U q
D q Q








 






 

 
 
 

where is the threshold distance to
switch from a conic to a parabolic well











*
*
2
*
,
,
,
0
Q
D q
D q Q
D q
D q
D q Q








where is the threshold distance
for detecting an obstacle, is
distance to nearest obstacle
Wireless Networks on Campus


During our data gathering sessions we
identified 1819 unique wireless networks on
campus.

Return to Questions

MRAC Controller
*

u
B
Ax
x




r
m
m
m
B
x
A
x




Plant:


Reference
model:


Ideal Control:


m
m
r
u
B
BL
A
BK
A
L
x
K






*
*
*
*
bu
ax
x



r
b
x
a
x
m
m
m



m
*
m
*
*
*
b
bl
a
bk
a
r
l
x
k
u






scalar

Vector/matrix

*
P. A. Ioannou, J. Sun,
Robust Adaptive Control,

Prentice
-
Hall, 1996, Ch 6.2

MRAC Controller
*

Feedback
gain

Reference
gain:


Constraint:




b
ex
k
sgn
1





b
er
l
sgn
2






l
T
T
m
sgn
Pex
B
K




l
r
T
m
sgn
Pe
B
L



Must know
sign of
b
since



:
sgn
l
Is 1 if
L
*

is positive
definite,
-
1 if
negative definite
since

*
P. A. Ioannou, J. Sun,
Robust Adaptive Control,

Prentice
-
Hall, 1996, Ch 6.2

m
B
BL

*
scalar

Vector/matrix

m
*
b
bl

]
~
~
~
~
[
)
~
~
(
L
Γ
L
K
Γ
K
Pe
e
L
,
K
e,
T
T
T



tr
V
b
l
b
k
e
)
l
,
k
V(e,
2
2
1
2
2
2
~
2
~
2
~
~





MRAC Simulation Model

Return to Questions

SegMonster Improvements


Remade hand attachment to
allow full range of turn commands


Added wireless sensor and
gumstix processor


Added limit switches to robot
body


Improved precision for encoder


Added two IR distance sensors


Dead zone compensation for turn
commands


Forgetting factor on Integral
terms

Return to Questions


Simple Pendulum


Rotating wheel on end


Wheel actuated by DC
motor


Motor exerts torque on
wheel


Wheel produces
reaction torque on
pendulum


p

r

PENDULUM
REACTION
WHEEL
State Estimation

Example: RWP

State Estimation Example: RWP
















r
r
p
p






x
10


u
u
k
ki
u







r
p
y


State Estimation Example: RWP


With feedback we can ‘estimate’ our
state


We can design our estimator to
converge to the correct state at any rate
if system is:


Stable


Observable


Linear

State Estimation Example: RWP

Why can’t we do the same
thing with our robots?

Three Problems:

1.
Not stable

2.
Not observable

3.
Not linear

Return to Questions

GP_predict Algorithm





1
*
* *
*
1
*
*
*
2 2
*
*
GP_predict(,,,)
1:,
2:
3:
4:
5:return,
T
T
f n
k











  
x X L
α
k x X
k
α
v L k
v v




1
1
chol
\\
T




L K
α L L z
Return to presentation

EKF



1
1 1
1
1
1
1 1 1
1 1 1
,
k k
k
k k k k
k k k k
k
k
k k
T
k k k
k k
f



  
 

 


 
 

x
P P V
x
x
x Re
P P
u
F F
Two Steps:
1. Predict the state and covariance mat
rix using motion model
2. Correct the state and covariance bas
ed on sensor data


1
1
1
1
1
1
1
1
1
,
1
1 1
1
1
1 1
k
k k k k
k
k
k
T
k k
k
k
k k
k k
k
T
k
k k
k
k
f h



 











 
 


 
 
 
x u x
H
H
e z x
S P W
R P
R P S
x x
H
H
H
F H
where
The linearized state and observation
matrices are the following Jacobians:
Return to presentation

Data Collection

Return to presentation