Localization, Mapping, SLAM and The Kalman Filter According To George PDF

Localization, Mapping, SLAM and
The Kalman Filter according to George
Robotics Institute 16-735

http://voronoi.sbp.ri.cmu.edu/~motion
Howie Choset
http://voronoi.sbp.ri.cmu.edu/~choset
RI 16-735, Howie Choset, with slides from George Kantor, G.D. Hager, and D. Fox
The Problem
• What is the world around me (mapping)
– sense from various positions
– integrate measurements to produce map
– assumes perfect knowledge of position
• Where am I in the world (localization)

– sense
– relate sensor readings to a world model
– compute location relative to model
– assumes a perfect world model
• Together, these are SLAM (Simultaneous Localization and

Mapping)
Localization
Tracking: Known initial position Challenges

– Sensor processing
Global Localization: Unknown initial position – Position estimation
Re-Localization: Incorrect known position – Control Scheme
– Exploration Scheme
(kidnapped robot problem)
– Cycle Closure
– Autonomy
SLAM –
–
Tractability
Scalability
Mapping while tracking locally and globally
Representations for Robot Localization
Kalman filters (late-80s?)

Discrete approaches (’95) • Gaussians
• Topological representation (’95) • approximately linear models
• uncertainty handling (POMDPs) • position tracking
• occas. global localization, recovery
• Grid-based, metric representation (’96)
• global localization, recovery
Robotics
AI
Particle filters (’99) Multi-hypothesis (’00)
• sample-based representation • multiple Kalman filters
• global localization, recovery • global localization, recovery
Three Major Map Models
Grid-Based: Feature-Based: Topological:

Collection of discretized Collection of landmark Collection of nodes and
obstacle/free-space pixels locations and correlated their interconnections
uncertainty
Elfes, Moravec, Smith/Self/Cheeseman, Kuipers/Byun,

Thrun, Burgard, Fox, Durrant–Whyte, Leonard, Chong/Kleeman,
Simmons, Koenig, Nebot, Christensen, etc. Dudek, Choset,
Konolige, etc. Howard, Mataric, etc.
Three Major Map Models
Grid-Based Feature-Based Topological
Resolution vs. Scale Discrete localization Arbitrary localization Localize to nodes
Computational Grid size and resolution Landmark covariance (N2) Minimal complexity
Complexity
Exploration Frontier-based No inherent exploration Graph exploration

Strategies exploration
Atlas Framework
• Hybrid Solution:
– Local features extracted from local grid map.
– Local map frames created at complexity limit.
– Topology consists of connected local map frames.
Authors: Chong, Kleeman; Bosse, Newman, Leonard, Soika, Feiten, Teller
H-SLAM
What does a Kalman Filter do, anyway?
Given the linear dynamical system:

x ( k + 1) = F ( k ) x ( k ) + G ( k )u( k ) + v ( k )
y ( k ) = H ( k ) x ( k ) + w( k )
x ( k ) is the n - dimensional state vector (unknown)
u( k ) is the m - dimensional input vector (known)
y ( k ) is the p - dimensional output vector (known, measured)
F ( k ), G ( k ), H ( k ) are appropriately dimensioned system matrices (known)
v ( k ), w( k ) are zero - mean, white Gaussian noise with (known)
covariance matrices Q( k ), R ( k )
the Kalman Filter is a recursion that provides the

“best” estimate of the state vector x.
What’s so great about that?
x ( k + 1) = F ( k ) x ( k ) + G ( k )u( k ) + v ( k )
y ( k ) = H ( k ) x ( k ) + w( k )
• noise smoothing (improve noisy measurements)

• state estimation (for state feedback)
• recursive (computes next estimate using only most recent measurement)
How does it work?
x ( k + 1) = F ( k ) x ( k ) + G ( k )u( k ) + v ( k )
y ( k ) = H ( k ) x ( k ) + w( k )
1. prediction based on last estimate:
xˆ ( k + 1 | k ) = F (k ) xˆ (k | k ) + G (k )u (k )
yˆ (k ) = H (k ) xˆ (k + 1 | k )
2. calculate correction based on prediction and current measurement:

Δx = f ( y (k + 1), xˆ (k + 1 | k ) )
3. update prediction: xˆ ( k + 1 | k + 1) = xˆ (k + 1 | k ) + Δx
Finding the correction (no noise!)
y = Hx
Given prediction xˆ (k + 1 | k ) and output y, find Δx so that xˆ = xˆ (k + 1 | k ) + Δx
is the " best" estimate of x.
Want the best estimate to be consistent

with sensor readings
x̂ (k+1|k)
•
Ω = {x | Hx = y} “best” estimate comes from shortest Δx
y = Hx
Given prediction xˆ (k + 1 | 1) and output y, find Δx so that xˆ = xˆ (k + 1 | 1) + Δx
“best” estimate comes from shortest Δx
shortest Δx is perpendicular to Ω
xˆ (k + 1 | k )
•
Δx
Ω = {x | Hx = y}
Some linear algebra
a is parallel to Ω if Ha = 0
Null ( H ) = {a ≠ 0 | Ha = 0}
Ω = {x | Hx = y} a is parallel to Ω if it lies in the

null space of H
for all v ∈ Null ( H ), v ⊥ b if b ∈ column( H T )

Weighted sum of columns means b = Hγ , the weighted sum of columns
y = Hx

xˆ (k + 1 | k )
⇒ Δx ∈ null(H ) ⇒ Δx ∈ column(H )
• ⊥ T
Δx ⇒ Δx = H T γ
Ω = {x | Hx = y}
y = Hx

xˆ (k + 1 | k )
• ⊥ T
Ω = {x | Hx = y}
Real output – estimated output

ν = y − H ( xˆ (k + 1 | k )) = H ( x − xˆ (k + 1 | k ))
innovation
y = Hx

xˆ (k + 1 | k )
• ⊥ T
Ω = {x | Hx = y}
assume γ is a linear function of ν
⇒ Δx = H T K ν
Guess, hope, lets face it, it has to be some
for some m × m matrix K
function of the innovation
y = Hx
Given prediction xˆ− and output y, find Δx so that xˆ = xˆ (k + 1 | k ) + Δx
we require
H ( xˆ (k + 1 | k ) + Δx) = y
xˆ (k + 1 | k )
•
Δx
Ω = {x | Hx = y}
y = Hx
we require
H ( xˆ (k + 1 | k ) + Δx) = y
xˆ (k + 1 | k ) ⇒ HΔx = y − Hxˆ (k + 1 | k ) = H ( x − xˆ (k + 1 | k )) = ν
•
Δx
Ω = {x | Hx = y}
y = Hx
we require
H ( xˆ (k + 1 | k ) + Δx) = y
•
Δx substituting Δx = H T Kν yields
Ω = {x | Hx = y}
HH T Kν = ν
Δx must be perpendicular to Ω b/c anything in the

range space of HT is perp to Ω
y = Hx
we require
H ( xˆ (k + 1 | k ) + Δx) = y
•
Ω = {x | Hx = y}
HH T Kν = ν
⇒ K = HH T ( )−1
The fact that the linear solution solves the equation makes assuming K is linear a kosher guess
y = Hx
we require
H ( xˆ (k + 1 | k ) + Δx) = y
•
Ω = {x | Hx = y}
HH T Kν = ν
⇒ K = HH T ( )−1
Δx = H T
(HH )T −1
ν
A Geometric Interpretation
Ω = {x | Hx = y}
xˆ (k + 1 | k )
••
Δx
A Simple State Observer
x(k + 1) = Fx(k ) + Gu (k )
System:
y (k ) = Hx(k )
1. prediction:
xˆ (k + 1 | k ) = Fxˆ (k | k ) + Gu (k )
2. compute correction:
Observer:
Δx = H T
(HH )
T −1
( y (k + 1) − Hxˆ (k + 1 | k ))
3. update:
xˆ ( k + 1 | k + 1) = xˆ (k + 1 | k ) + Δx
Caveat #1
Note: The observer presented here is not a very good

observer. Specifically, it is not guaranteed to converge for
all systems. Still the intuition behind this observer is the
same as the intuition behind the Kalman filter, and the
problems will be fixed in the following slides.
It really corrects only to the current sensor information,

so if you are on the hyperplane but not at right place, you
have no correction…. I am waiving my hands here, look in book
Estimating a distribution for x
Our estimate of x is not exact!

We can do better by estimating a joint Gaussian distribution p(x).
1
−1
(
( x − xˆ )T P −1 ( x − xˆ ) )
p( x) = 1/ 2
e2
(2π ) n / 2 P
(
where P = E ( x − xˆ )( x − xˆ ) T ) is the covariance matrix
Finding the correction (geometric intuition)
Given prediction xˆ (k + 1 | k ), covariance P, and output y , find Δx so that

xˆ = xˆ (k + 1 | k ) + Δx is the " best" (i.e. most probable) estimate of x.
−1
(
( x − xˆ )T P −1 ( x − xˆ ) )
Ω = {x | Hx = y} p( x) =
1
e2
1/ 2
(2π ) n / 2 P
xˆ•(k + 1 | k ) The most probable Δx is the one that :

1. satisfies xˆ ( k + 1 | k + 1) = xˆ (k + 1 | k ) + Δx
Δx
2. minimizes ΔxT P −1Δx
A new kind of distance
Suppose we define a new inner product on R n to be :

x1 , x2 = x1T P −1 x 2 (this replaces the old inner product x1T x2 )
= x , x = x T P −1 x
2
Then we can define a new norm x
The xˆ in Ω that minimizes Δx is the orthogonal projection of xˆ (k + 1 | k )

onto Ω, so Δx is orthogonal to Ω.
⇒ ω , Δx = 0 for ω in TΩ = null ( H )
ω , Δx = ω T P −1Δx = 0 iff Δx ∈ column ( PH T )
Finding the correction (for real this time!)
Assuming that Δx is linear in ν = y − Hxˆ ( k + 1 | k )

Δx = PH T Kν
The condition y = H ( xˆ (k + 1 | k ) + Δx ) ⇒ HΔx = y − Hxˆ (k + 1 | k ) = ν
Substituti on yields :
HΔx = ν = HPH T Kν
⇒ K = HPH ( T
)
−1
∴ Δx = PH T
(HPH )T −1
ν
x(k + 1) = Fx(k ) + Gu (k ) + v(k )
A Better State Observer
y (k ) = Hx(k ) Sample of Guassian Dist. w/
COV Q
We can create a better state observer following the same 3. steps, but now we
must also estimate the covariance matrix P.
We start with x(k|k) and P(k|k)
Where did noise go?
Step 1: Prediction Expected value…
xˆ (k + 1 | k ) = Fxˆ (k | k ) + Gu (k )
What about P? From the definition:

(
P ( k | k ) = E ( x (k ) − xˆ (k | k ))( x(k ) − xˆ (k | k ))T )
and
(
P ( k + 1 | k ) = E ( x(k + 1) − xˆ (k + 1 | k ))( x(k + 1) − xˆ (k + 1 | k ))T )
Continuing Step 1
To make life a little easier, lets shift notation slightly:

(
Pk−+1 = E ( xk +1 − xˆ k−+1 )( xk +1 − xˆ k−+1 )T )
(
= E (Fx k + Gu k + vk − ( Fxˆ k + Gu k ) )(Fx k + Gu k + vk − ( Fxˆ k + Gu k ) )T )
= E ((F (x − xˆ ) + v )(F (x − xˆ ) + v ) )
k k k k k k
T
= E (F (x − xˆ )(x − xˆ ) F + 2 F (x − xˆ )v + v k v Tk )
T T T
k k k k k k k
= FE ((x − xˆ )(x − xˆ ) )F + E (v v )
T T T
k k k k k k
= FPk F T + Q
P (k + 1 | k ) = FP (k | k ) F T + Q
Step 2: Computing the correction
From step 1 we get xˆ ( k + 1 | k ) and P (k + 1 | k ).

Now we use these to compute Δx :
(
Δx = P ( k + 1 | k ) H HP ( k + 1 | k ) H T
)
−1
( y (k + 1) − Hxˆ (k + 1 | k ) )
For ease of notation, define W so that
Δx = Wν
Step 3: Update
xˆ ( k + 1 | k + 1) = xˆ ( k + 1 | k ) + Wν
(
Pk +1 = E ( x k +1 − xˆ k +1 )( x k +1 − xˆ k +1 )T )
= E (( x k +1 − ˆ
x −
k +1 − W ν )( x k +1 − ˆ
x −
k +1 − W ν ) T
)
(just take my word for it…)
P (k + 1 | k + 1) = P (k + 1 | k ) − WHP (k + 1 | k ) H T W T
Just take my word for it…
(
Pk +1 = E ( x k +1 − xˆ k +1 )( x k +1 − xˆ k +1 )T )
= E (( x k +1 − xˆ k−+1 − Wν )( xk +1 − xˆ k−+1 − Wν )T )
= E ⎛⎜ (( x )( ) − Wν ) ⎞⎟
T
ˆ k−+1 ) − Wν ( xk +1 − xˆ k−+1
k +1 − x
⎝ ⎠
(
= E ( xk +1 − xˆ k−+1 )( xk +1 − xˆ k−+1 )T − 2Wν ( xk +1 − xˆ k−+1 )T + Wν (Wν )T )
(
= Pk−+1 + E − 2WH ( xk +1 − xˆ k−+1 )( xk +1 − xˆ k−+1 )T + WH ( xk +1 − xˆ k−+1 )( xk +1 − xˆ k−+1 )T H T W T )
= Pk−+1 − 2WHPk−+1 + WHPk−+1 H T W T
(
= Pk−+1 − 2 Pk−+1 H T HPk−+1 H T ) HP
−1 −
k +1 + WHPk−+1 H T W T
= Pk−+1 − 2 Pk−+1 H T (HP −

k +1 H ) (HP
T −1 −
k +1 H
T
)(HP −
k +1 H )
T −1
HPk−+1 + WHPk−+1 H T W T
= Pk−+1 − 2WHPk−+1 H T W T + WHPk−+1 H T W T
Better State Observer Summary
x(k + 1) = Fx(k ) + Gu (k ) + v(k )

System: y (k ) = Hx(k )
1. Predict xˆ (k + 1 | k ) = Fxˆ (k | k ) + Gu (k )
P (k + 1 | k ) = FP (k | k ) F T + Q
( )
Observer
−1
W = P (k + 1 | k ) H HP (k + 1 | k ) H T
2. Correction
Δx = W ( y ( k + 1) − Hxˆ (k + 1 | k ) )
xˆ ( k + 1 | k + 1) = xˆ ( k + 1 | k ) + Wν
3. Update
P (k + 1 | k + 1) = P (k + 1 | k ) − WHP (k + 1 | k ) H T W T
•Note: there is a problem with the previous slide, namely the covariance matrix of
the estimate P will be singular. This makes sense because with perfect sensor
measurements the uncertainty in some directions will be zero. There is no
uncertainty in the directions perpendicular to Ω
P lives in the state space and directions associated with sensor noise are zero. In
the step when you do the update, if you have a zero noise measurement, you end
up squeezing P down.
In most cases, when you do the next prediction step, the process covariance matrix
Q gets added to the P(k|k), the result will be nonsingular, and everything is ok again.
There’s actually not anything wrong with this, except that you can’t really call the
result a “covariance” matrix because “sometimes” it is not a covariance matrix
Plus, lets not be ridiculous, all sensors have noise.
Finding the correction (with output noise)
y = Hx + w
The previous results require that you know
which hyperplane to aim for. Because there
Ω = {x | Hx = y} is now sensor noise, we don’t know where to
aim, so we can’t directly use our method.
If. we can determine which hyperplane aim

for, then the previous result would apply.
xˆ (k +• 1 | k ) We find the hyperplane in question as follows:
1. project estimate into output space
2. find most likely point in output space based on
measurement and projected prediction
3. the desired hyperplane is the preimage of this point
Projecting the prediction
(putting current state estimates into sensor space)
xˆ (k + 1 | k ) → yˆ = Hxˆ (k + 1 | k )
P (k + 1 | k ) → Rˆ = HP(k + 1 | k ) H T
P(k + 1 | k ) R̂
xˆ (k + 1 | k ) • ŷ •
state space (n-dimensional) output space (p-dimensional)
Finding most likely output
ence independent, so multiply them because we want them both to be true at the same time
The objective is to find the most likely output that results

from the independent Gaussian distributions
( y, R) measurement and associate covariance
( yˆ , Rˆ ) projected prediction (what you think your measurements
should be and how confident you are)
Fact (Kalman Gains): The product of two Gaussian distributions given by
mean/covariance pairs (x1,C1) and (x2,C2) is proportional to a third Gaussian
with mean
x3 = x1 + K ( x2 − x1 )
and covariance
C3 = C1 − KC1 Strange, but true,
where this is symmetric
K = C1 (C1 + C2 )
−1
Most likely output (cont.)
Using the Kalman gains, the most likely output is
y = yˆ + ⎛⎜ Rˆ Rˆ + R
*
⎝
( )
−1
⎞⎟( yˆ − y )
⎠
(
= Hxˆ (k + 1 | k ) + HPH HPH + R
T
( T
)−1
)(Hxˆ(k + 1 | k ) − y)
Finding the Correction
Now we can compute the correction as we did in the noiseless case, this time
using y* instead of y. In other words, y* tells us which hyperplane to aim for.
{
Ω = x | Hx = y * }
The result is:
x̂−
( ) (y )
• −1
Δx Δx = PH HPH T T *
− Hxˆ ( k + 1 | k )
ot going all the way to y, but splitting the difference between how confident you are with your
Sensor and process noise
Finding the Correction (cont.)
Δx = PH HPH T
( T
) (y − Hxˆ (k + 1 | k ))
−1 *
(
= PH T HPH T ) (Hxˆ + HPH (HPH + R ) ( y − Hxˆ (k + 1 | k )) − Hxˆ (k + 1 | k ))
−1 T T −1
= PH HPH + RT
( T
) ( y − Hxˆ (k + 1 | k ))
−1
For convenience, we define

W = PH HPH + R T
( T
) −1
So that
Δx = W ( y − Hxˆ ( k + 1 | k ) )
Correcting the Covariance Estimate
The covariance error estimate correction is computed from

the definition of the covariance matrix, in much the same
way that we computed the correction for the “better
observer”. The answer turns out to be:
P (k + 1 | k + 1) = P (k + 1 | k ) − W HP (k + 1 | k ) H W( T
) T
LTI Kalman Filter Summary
x(k + 1) = Fx(k ) + Gu (k ) + v(k )

System:
y (k ) = Hx(k ) + w(k )
xˆ (k + 1 | k ) = Fxˆ (k | k ) + Gu (k )
1. Predict
P (k + 1 | k ) = FP (k | k ) F T + Q
Kalman Filter
S = HP ( k + 1 | k ) H T + R
2. Correction W = P ( k + 1 | k ) H T S −1
Δx = W ( y ( k + 1) − Hxˆ (k + 1 | k ) )
xˆ ( k + 1 | k + 1) = xˆ ( k + 1 | k ) + Wν
3. Update
P (k + 1 | k + 1) = P (k + 1 | k ) − WSW T
Kalman Filters
Kalman Filter for Dead Reckoning
• Robot moves along a straight line with state x = [xr, vr]T
• u is the force applied to the robot
• Newton tells us or
Process noise
from a zero
mean
Gaussian V
• Robot has velocity sensor

Sensor noise from a
zero mean Gaussian W
Set up
Assume At some time k
PUT ELLIPSE FIGURE HERE
Observability
Recall from last time
Actually, previous example is not observable but still nice to use Kalman filter
Extended Kalman Filter
• Life is not linear
• Predict
Extended Kalman Filter
• Update
EKF for Range-Bearing Localization
• State position and orientation
• Input forward and rotational velocity
• Process Model
• nl landmarks Association map

• can only see p(k) of them at k
Be wise, and linearize..
Data Association
•BIG PROBLEM
Ith measurement corresponds to the jth landmark
innovation
where
Pick the smallest
Kalman Filter for SLAM (simple)
state
Inputs are commands to x and y velocities, a bit naive
Process model
Kalman Filter for SLAM
Range Bearing
Inputs are forward and rotational velocities
Greg’s Notes: Some Examples
• Point moving on the line according to f = m a
– state is position and velocity
– input is force
– sensing should be position
• Point in the plane under Newtonian laws
• Nonholonomic kinematic system (no dynamics)

– state is workspace configuration
– input is velocity command
– sensing could be direction and/or distance to beacons
• Note that all of dynamical systems are “open-loop” integration

• Role of sensing is to “close the loop” and pin down state
Kalman Filters
Bel ( xt ) = N ( μ t , σ t2 )
−
⎧ μ t +1 = μt + But
−
Bel(xt +1 ) = ⎨ − 2
σ
⎩ t +1 = A σ
2 2
t + σ 2
act
⎧ μt +1 = μt−+1 + K t +1 ( μ zt +1 − μt−+1 )
Bel(xt +1 ) = ⎨
⎩ σ t2+1 = (1 − K t +1 )σ − t2+1
−
⎧ μ t + 2 = μt +1 + But +1
−
Bel(xt + 2 ) = ⎨ − 2
σ
⎩ t +2 = A σ
2 2
t +1 + σ 2
act
Kalman Filter Algorithm
1. Algorithm Kalman_filter( <μ,Σ>, d ):
2. If d is a perceptual data item y then

3. (
K = ΣC CΣC + Σ obs
T T
)
−1
4. μ = μ + K ( z − Cμ )
5. Σ = ( I − KC )Σ
6. Else if d is an action data item u then
7. μ = Aμ + Bu
8. Σ = AΣAT + Σ act
9. Return <μ,Σ>
Limitations
• Very strong assumptions:

– Linear state dynamics
X t−+1 = AX t + But + Σ act
– Observations linear in state
• What can we do if system is not linear? Z t = CX t + Σ obs
– Non-linear state dynamics
– Non-linear observations
X t−+1 = f ( X t , ut , Σ act )
Z t = c( X t , Σ obs )
• Linearize it!
• Determine Jacobians of dynamics f and observation
function c w.r.t the current state x and the noise.
∂f i ∂c ∂f i ∂ci
Aij = ( xt , ut ,0) Cij = i ( xt ,0) Wij = ( xt , ut ,0) Vij = ( xt ,0)
∂x j ∂x j ∂Σ act j ∂Σ obs j
Extended Kalman Filter Algorithm
1. Algorithm Extended_Kalman_filter(<μ,Σ>, d):
2. If d is a perceptual data item z then

3. (
K = ΣC CΣC + VΣ obsV
T T
)
T −1
(
K = ΣC CΣC + Σ obs
T T
)
−1
4. μ = μ + K (z − c( μ ,0) ) μ = μ + K ( z − Cμ )
5. Σ = ( I − KC )Σ Σ = ( I − KC )Σ
6. Else if d is an action data item u then

7. μ = f ( μ , u , 0) μ = Aμ + Bu
8. Σ = AΣAT + WΣ actW T Σ = AΣAT + Σ act
9. Return <μ,Σ>
Kalman Filter-based Systems (2)
• [Arras et al. 98]:
• Laser range-finder and vision
• High precision (<1cm accuracy)
Courtesy of RI
K. 16-735,
Arras Howie Choset, with slides from George Kantor, G.D. Hager, and D. Fox
Unscented Kalman Filter
• Instead of linearizing, pass several points from the Gaussian through the non-
linear transformation and re-compute a new Gaussian.
• Better performance (theory and practice).
μ μ
f f
μ ' = f ( μ , u ,0)
Σ = AΣAT + WΣ actW T
μ’
μ’
Kalman Filters and SLAM
• Localization: state is the location of the robot
• Mapping: state is the location of beacons
• SLAM: state combines both
• Consider a simple fully-observable holonomic robot
– x(k+1) = x(k) + u(k) dt + v

– yi(k) = pi - x(k) + w
• If the state is (x(k),p1, p2 ...) then we can write a linear observation

system
– note that if we don’t have some fixed beacons, our system is unobservable
(we can’t fully determine all unknown quantities)

Localization, Mapping, SLAM and The Kalman Filter According To George PDF

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Localization, Mapping, SLAM and The Kalman Filter According To George PDF

Uploaded by

Copyright:

Available Formats

Localization, Mapping, SLAM and

The Kalman Filter according to George

Robotics Institute 16-735

• Where am I in the world (localization)

• Together, these are SLAM (Simultaneous Localization and

Tracking: Known initial position Challenges

Kalman filters (late-80s?)

Grid-Based: Feature-Based: Topological:

Elfes, Moravec, Smith/Self/Cheeseman, Kuipers/Byun,

Grid-Based Feature-Based Topological

Resolution vs. Scale Discrete localization Arbitrary localization Localize to nodes

Exploration Frontier-based No inherent exploration Graph exploration

Given the linear dynamical system:

the Kalman Filter is a recursion that provides the

• noise smoothing (improve noisy measurements)

2. calculate correction based on prediction and current measurement:

Want the best estimate to be consistent

Ω = {x | Hx = y} “best” estimate comes from shortest Δx

Ω = {x | Hx = y} a is parallel to Ω if it lies in the

for all v ∈ Null ( H ), v ⊥ b if b ∈ column( H T )

“best” estimate comes from shortest Δx

“best” estimate comes from shortest Δx

Real output – estimated output

“best” estimate comes from shortest Δx

Δx must be perpendicular to Ω b/c anything in the

Note: The observer presented here is not a very good

It really corrects only to the current sensor information,

Our estimate of x is not exact!

Given prediction xˆ (k + 1 | k ), covariance P, and output y , find Δx so that

xˆ•(k + 1 | k ) The most probable Δx is the one that :

Suppose we define a new inner product on R n to be :

The xˆ in Ω that minimizes Δx is the orthogonal projection of xˆ (k + 1 | k )

ω , Δx = ω T P −1Δx = 0 iff Δx ∈ column ( PH T )

Assuming that Δx is linear in ν = y − Hxˆ ( k + 1 | k )

The condition y = H ( xˆ (k + 1 | k ) + Δx ) ⇒ HΔx = y − Hxˆ (k + 1 | k ) = ν

What about P? From the definition:

To make life a little easier, lets shift notation slightly:

From step 1 we get xˆ ( k + 1 | k ) and P (k + 1 | k ).

For ease of notation, define W so that

= Pk−+1 − 2 Pk−+1 H T (HP −

= Pk−+1 − 2WHPk−+1 H T W T + WHPk−+1 H T W T

x(k + 1) = Fx(k ) + Gu (k ) + v(k )

Plus, lets not be ridiculous, all sensors have noise.

If. we can determine which hyperplane aim

state space (n-dimensional) output space (p-dimensional)

The objective is to find the most likely output that results

Using the Kalman gains, the most likely output is

For convenience, we define

The covariance error estimate correction is computed from

x(k + 1) = Fx(k ) + Gu (k ) + v(k )

• u is the force applied to the robot

• Robot has velocity sensor

Assume At some time k

PUT ELLIPSE FIGURE HERE

• State position and orientation

• Input forward and rotational velocity

• nl landmarks Association map

Pick the smallest

Inputs are commands to x and y velocities, a bit naive

Inputs are forward and rotational velocities

• Point in the plane under Newtonian laws

• Nonholonomic kinematic system (no dynamics)

• Note that all of dynamical systems are “open-loop” integration