Gaussian (or Normal) Distribution - The New Age of Discoverysleonard/review.pdfGaussian white noise...
Transcript of Gaussian (or Normal) Distribution - The New Age of Discoverysleonard/review.pdfGaussian white noise...
600.436/600.636G.D. Hager
S. Leonard
Gaussian (or Normal) Distribution
-s s
m
Univariate
Multivariate
๐ ๐ฅ ~๐ ๐, ๐2
๐ ๐ฅ =1
2๐๐๐โ12(๐ฅโ๐)2
๐2
๐ ๐ ~๐ ๐, ฮฃ
๐ ๐ =1
(2๐)๐/2 ฮฃ 1/2๐โ
12๐โ๐ ๐ฮฃโ1(๐โ๐)
600.436/600.636G.D. Hager
S. Leonard
Properties of Gaussians
โข We stay in the โGaussian worldโ as long as we start with
Gaussians and perform only linear transformations.
โข Same holds for multivariate Gaussians
๐~๐ ๐, ๐2
๐ = ๐๐ + ๐โน ๐~๐(๐๐ + ๐, ๐2๐2)
๐1~๐ ๐1, ๐1
๐2~๐ ๐2, ๐2โน ๐1 + ๐2~๐(๐1 + ๐2, ๐1 + ๐2 )
2
22 2
600.436/600.636G.D. Hager
S. Leonard
Covariance, covariance, covariance
โข Know whatโs a covariance
matrix
โ What it does, what it means,
why it matters, how you can
compute one, how you can
visualize them
๐ =1
๐
๐=0
๐
๐๐
๐๐ = ๐๐ โ ๐
ฮฃ = E[๐๐๐๐๐]
600.436/600.636G.D. Hager
S. Leonard
Kalman Filter
โข Recursive solution for discrete linear filtering problems
โ A state x Rn
โ A measurement z Rm
โ Discrete (i.e. for time t = 1, 2, 3, โฆ )
โ Recursive process (i.e. xt = f ( xt-1 ) )
โ Linear system (i.e xt = A xt-1 )
โข The system is defined by:
1) linear process model
xt = A xt-1 + B ut-1 + wt-1
2) linear measurement model
zt = H xt + vt
control
Input
(optional)
Gaussian
white
noise
state
transitionGaussian
white
noise
observation
model
How a state transitions into another state How a state relates to a measurement
600.436/600.636G.D. Hager
S. Leonard
Kalman Filter
โข Recipe:
1. Start with an initial guess
๐0, ฮฃ02. Compute prior (prediction)
๐๐กโฒ = ๐ด ๐๐กโ1 + ๐ต๐๐กโ1
ฮฃ๐กโฒ = ๐ดฮฃ๐กโ1๐ด
๐ + ๐
3. Compute posterior (correction)
๐พ๐ก = ฮฃ๐กโฒ๐ป๐ ๐ปฮฃ๐ก
โฒ๐ป๐ + ๐ โ1
๐๐ก = ๐๐กโฒ + ๐พ๐ก ๐๐ก โ ๐ป ๐๐ก
โฒ
ฮฃ๐ก = 1 โ ๐พ๐ก๐ป ฮฃ๐กโฒ
Time update
(predict)
Measurement
update
(correct)RE
PE
AT
Minimizes the sum of the expected
errors: ๐๐2
600.436/600.636G.D. Hager
S. Leonard
Initial
๐0, ฮฃ0
16-735, Howie Choset with slides
from G.D. Hager and Z. Dodds
Predict ๐1โฒ from ๐0 and ๐0
๐1โฒ = ๐ด ๐0 + ๐ต๐0
ฮฃ1โฒ = ๐ดฮฃ0๐ด
๐ + ๐
Correction using ๐1๐พ1 = ฮฃ1
โฒ๐ป๐ ๐ปฮฃ1โฒ๐ป๐ + ๐ โ1
๐1 = ๐1โฒ + ๐พ1 ๐1 โ ๐ป ๐1
โฒ
ฮฃ1 = 1 โ ๐พ1๐ป ฮฃ1โฒ
Predict ๐2โฒ from ๐1 and ๐1
๐2โฒ = ๐ด ๐1 + ๐ต๐1
ฮฃ2โฒ = ๐ดฮฃ1๐ด
๐ + ๐
600.436/600.636G.D. Hager
S. Leonard
Kalman Filter Limitations
โข Assumptions:
โ Linear process model ๐๐ก+1 = ๐ด๐๐ก + ๐ต๐๐ก +๐๐ก
โ Linear observation model ๐๐ก = ๐ป๐๐ก + ๐๐กโ White Gaussian noise ๐(0, ฮฃ )
โข What can we do if system is not linear?
โ Non-linear state dynamics ๐๐ก+1 = ๐ ๐๐ก , ๐๐ก , ๐๐ก
โ Non-linear observations ๐๐ก = โ ๐๐ก , ๐๐ก
600.436/600.636G.D. Hager
S. Leonard
โข Extended Kalman Filter Recipe:
โ Given
โ Prediction
โ Measurement correction
Extended Kalman Filter
โข Kalman Filter Recipe:
โ Given
โ Prediction
โ Measurement correction
600.436/600.636G.D. Hager
S. Leonard
EKF for Range-Bearing Localization
โข State ๐๐ก = ๐ฅ๐ก ๐ฆ๐ก ๐๐ก๐ 2D position and orientation
โข Input ๐๐ก = ๐ฃ๐ก ๐๐ก๐ linear and angular velocity
โข Process model
๐ ๐๐ก , ๐๐ก , ๐๐ก =
๐ฅ๐กโ1 + (โ๐ก)๐ฃ๐กโ1cos(๐๐กโ1)๐ฆ๐กโ1 + (โ๐ก)๐ฃ๐กโ1sin(๐๐กโ1)
๐๐กโ1 + (โ๐ก)๐๐กโ1
+
๐ค๐ฅ๐ก๐ค๐ฆ
๐ค๐๐ก
โข Given a map, the robot sees N landmarks with coordinates
๐1 = ๐ฅ๐1 ๐ฆ๐1 ๐ , โฏ , ๐๐ = ๐ฅ๐๐ ๐ฆ๐๐ ๐
The observation model is
๐๐ก =๐1(๐๐ก, ๐1)
โฎ๐๐(๐๐ก, ๐๐)
๐๐ ๐๐ก, ๐๐ก =๐ฅ๐ก โ ๐ฅ๐๐
2+ ๐ฆ๐ก โ ๐ฆ๐๐
2
tanโ1๐ฆ๐กโ๐ฆ๐๐๐ฅ๐กโ๐ฅ๐๐
โ ๐๐ก
+๐ฃ๐๐ฃ๐
x
xtyt
y lN
l1
r1
b1
600.436/600.636G.D. Hager
S. Leonard
Kalman Filters and SLAMโข Localization: state is the location of the robot
โข Mapping: state is the location of 2D landmarks
โข SLAM: state combines both
โข If the state is
then we can write a linear observation system
โ note that if we donโt have some fixed landmarks, our system is unobservable(we canโt fully determine all unknown quantities)
โข Covariance S is represented by http://ais.informatik.uni-freiburg.de
600.436/600.636G.D. Hager
S. Leonard
โข State ๐๐ก = ๐ฅ๐ก ๐ฆ๐ก ๐๐ก ๐1๐ โฏ ๐๐
๐ position/orientation of
robot and landmarks coordinates
โข Input ๐๐ก = ๐ฃ๐ก ๐๐ก๐ forward and angular velocity
โข The process model for localization is
๐๐กโฒ =
๐ฅ๐กโ1๐ฆ๐กโ1๐๐กโ1
+
โ๐ก๐ฃ๐กโ1cos(๐๐กโ1)โ๐ก๐ฃ๐กโ1sin(๐๐กโ1)
โ๐ก๐๐กโ1
This model is augmented for 2N+3 dimensions to
accommodate landmarks. This results in the process equation๐ฅ๐กโฒ
๐ฆ๐กโฒ
๐๐กโฒ
๐1๐กโฒ
โฎ๐๐๐กโฒ
=
๐ฅ๐กโ1๐ฆ๐กโ1๐๐กโ1
๐1๐กโ1โฎ
๐๐๐กโ1
+
1 0 00 1 00 0 10 0 0โฎ โฎ โฎ0 0 0
โ๐ก๐ฃ๐กโ1cos(๐๐กโ1)โ๐ก๐ฃ๐กโ1sin(๐๐กโ1)
โ๐ก๐๐กโ1
EKF Range Bearing SLAM
Prior State Estimation
Landmarks donโt depend
on external input
600.436/600.636G.D. Hager
S. Leonard
EKF Range Bearing SLAM
Prior Covariance UpdateWe assume static landmarks. Therefore, the function
f(s,u,w) only affects the robotโs location and not the
landmarks.
ฮฃ๐กโฒ = ๐ด๐กฮฃ๐กโ1๐ด๐ก
๐ +๐๐ก๐๐๐ก๐
Jacobian of the
robot motion
The motion of the robot does not
affect the coordinates of
the landmarks
Jacobian of the
process model
2Nx2N identity
๐ด =
๐๐ฅ
๐๐ฅ
๐๐ฅ
๐๐ฆ
๐๐ฅ
๐๐๐๐ฆ
๐๐ฅ
๐๐ฆ
๐๐ฆ
๐๐ฆ
๐๐๐๐
๐๐ฅ
๐๐
๐๐ฆ
๐๐
๐๐
0
0 ๐ผ
600.436/600.636G.D. Hager
S. Leonard
Bayes Filter
โข Given a sequence of measurements z1, โฆ, zk
โข Given a sequence of commands u0, โฆ, uk-1
โข Given a sensor model P(zk | xk)
โข Given a dynamic model P(xk | xk-1)
โข Given a prior probability P(x0)
โข Find P(xk | z1:k, u0:k-1 )
x0xk-
2
xk-
1xk
zk-2 zk-1 zk
u0 uk-2 uk-1
600.436/600.636G.D. Hager
S. Leonard
Bayesian Localization
โข Recall Bayes Theorem:
๐ ๐ฅ ๐ง) =๐ ๐ง ๐ฅ ๐(๐ฅ)
๐(๐ง)
โข Also remember conditional independence
โข Think of x as the state of the robot and z as the data we know
๐(๐๐|๐0:๐โ1, ๐1:๐) Posterior Probability Distribution
๐ ๐๐ ๐0:๐โ1, ๐1:๐ =๐ ๐๐ ๐๐ , ๐0:๐โ1, ๐1:๐โ1 ๐(๐๐|๐0:๐โ1, ๐1:๐โ1)
๐(๐๐|๐0:๐โ1, ๐1:๐โ1)
= ๐๐๐(๐๐|๐๐)
๐๐โ1
๐ ๐๐ ๐๐โ1, ๐๐โ1 ๐ ๐๐โ1 ๐0:๐โ2, ๐1:๐โ1 ๐๐๐โ1
observation recursionstate prediction
600.436/600.636G.D. HagerS. Leonard
Bayes Filter Recap
๐(๐๐โ1 = A)
๐(๐๐โ1 = B)
๐(๐๐โ1 = Z)
โฆ
๐(๐๐ = A |
๐๐โ1๐๐โ1)
๐(๐๐ = B|
๐๐โ1๐๐โ1)
๐(๐๐ = Z |
๐๐โ1๐๐โ1)
Apply command๐๐โ1
โฆ
Obtain z k and apply๐(๐๐|๐๐)๐(๐๐ = A |
๐๐โ1๐๐)
๐(๐๐ = B |
๐๐โ1๐๐)
๐(๐๐ = Z |
๐๐โ1๐๐)
โฆ
๐ ๐๐ ๐0:๐โ1, ๐1:๐ ๐ ๐๐โ1 ๐0:๐โ2, ๐1:๐โ1๐ ๐๐ ๐๐โ1, ๐๐โ1
600.436/600.636G.D. Hager
S. Leonard
Predict Motion (Prior Distribution)
โข Suppose we have ๐ ๐๐โข We have ๐(๐๐+1|๐๐ , ๐๐)
โข Put together
๐ ๐๐+1 = ๐๐
๐(๐๐+1|๐๐ , ๐๐)๐ ๐๐ ๐๐๐
What is the probability distribution for xk+1 given the command uk and all
the previous states xk?
600.436/600.636G.D. Hager
S. Leonard
System Model
State space X = { 1, 2, 3, 4}
P(xk+1 | xk,
uk)
Xk+1=
1
Xk+1=
2
Xk+1=
3
Xk+1=
4
Xk=1 0.25 0.5 0.25 0
Xk=2 0 0.25 0.5 0.25
Xk=3 0 0 0.25 0.75
Xk=4 0 0 0 1
xk
P(xk)
1 2 3 4
0.5
Prior probability
distribution ๐(๐ฅ๐)
Compute ๐ ๐ฅ๐+1 = ๐ฅ๐๐(๐ฅ๐+1|๐ฅ๐ , ๐ข๐)๐๐ฅ๐
Transition matrix: The probability P(j|i) of
moving
from i to j is given by Pi,j. Each row must sum
to 1.
600.436/600.636G.D. Hager
S. Leonard
Observation Model
โข How likely is the measurement zk given a state xk?
โข The measurement zk is what we get. So we have to stick by it
โข However, not all states will likely produce the measurement
โข Weโre not interested in finding the most likely state. We want the whole distribution
X
X
๐(๐ง๐|๐ฅ๐)
zk
xk
Doesnโt have to be Gaussian
600.436/600.636G.D. Hager
S. Leonard
Observation Model
Beam Model
โข Mixing all these cases together we get
๐ ๐ง๐ ๐๐ , ๐ =
๐คhit๐คshort๐คmax๐คrand
๐ ๐hit(๐ง๐|๐๐, ๐)
๐short(๐ง๐|๐๐ , ๐)
๐max(๐ง๐|๐๐, ๐)๐rand(๐ง๐|๐๐, ๐)
where the weights are parameters
๐คhit + ๐คshort + ๐คmax + ๐คrand = 1
Probabilistic Robotic
600.436/600.636G.D. Hager
S. Leonard
Likelihood Field Model
๐hit ๐ง๐ ๐๐ , ๐ = ๐๐โ๐๐ก2 (๐2)
Probabilistic Robotic
600.436/600.636G.D. HagerS. Leonard
Discrete Bayes Filter Algorithm Algorithm Discrete_Bayes_filter( u0:k-1, y1:k, P(x0) )
1. P(x) = P(x0) (if you donโt know: uniform distribution)
2. for i=1:k
3. for all states x X
4.
5. end for
6. h=0 Normalization constant
7. for all states x X
8.
9. h = h + P(x)
10. end for
11. for all states x X
12. P(x) = P(x) / h
13. end for
14. end for
Xx
xPxuxPxP )(),|()(
)()|()( xPxzPxP
Prediction given prior dist. and command
Update using measurement
Normalize to 1
600.436/600.636G.D. Hager
S. Leonard
Particle Filter
โข Computing ๐(๐๐|๐๐ , ๐) and ๐(๐๐+1|๐๐ , ๐๐) is not easy
โข In practice, it is never directly computable
โข Need to propagate an entire conditional distribution, not just
one state like we did with Kalman,
โข Represent probability distribution by random samples
โข Estimation of non-Gaussian, nonlinear processes
600.436/600.636G.D. Hager
S. Leonard
Particle Filter
600.436/600.636G.D. Hager
S. Leonard
Particle Filter Algorithm Algorithm Particle_filter( u0:k-1, y1:k, P(x0), set of N samples M = {xj, wj} )
1. for i=1:k
2. for j=1:N
3. compute a new state x by sampling according to P( x | ui-1, xj )
4. xj = x
5. end
6. h=0
7. for j=1:N
8. wj = P( zi | xj )
9. h += wj
10. end
11. for j=1:N
12. wj = wj / h
13. end
14. resample (M ) according to weights wj
15. end