cos 495 - lecture 17 autonomous robot...
TRANSCRIPT
![Page 1: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/1.jpg)
1
COS 495 - Lecture 17 Autonomous Robot Navigation
Instructor: Chris Clark Semester: Fall 2011
Figures courtesy of Siegwart & Nourbakhsh
![Page 2: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/2.jpg)
2
Control Structure
Perception
Localization Cognition
Motion Control
Prior Knowledge Operator Commands
![Page 3: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/3.jpg)
3
Extended Kalman Filter Localization
1. EKF Localization Overview 2. EKF Prediction 3. EKF Correction 4. Algorithm Summary
![Page 4: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/4.jpg)
4
Extended Kalman Filter Localization
§ Robot State Representation § State vector to be estimated, x
e.g. x x = y θ
§ Associated Covariance, P σxx
σxy σxθ
P = σyx
σyy σyθ
σθx σθy σθθ
![Page 5: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/5.jpg)
5
Extended Kalman Filter Localization
1. Robot State Representation
![Page 6: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/6.jpg)
6
Extended Kalman Filter Localization
§ Iterative algorithm 1. Prediction – Use a motion model and
odometry to predict the state of the robot and its covariance
x’t P’ t 2. Correction - Use a sensor model and
measurement to predict the state of the robot and its covariance xt Pt
![Page 7: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/7.jpg)
7
Extended Kalman Filter Localization
1. EKF Localization Overview 2. EKF Prediction 3. EKF Correction 4. Algorithm Summary
![Page 8: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/8.jpg)
8
EKFL Prediction Step
§ Motion Model § Lets use a general form of a motion model
as a discrete time equation that predicts the current state of the robot given the previous state xt-1 and the odemetry ut
x’t = f(xt-1, ut )
![Page 9: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/9.jpg)
9
EKFL Prediction Step
§ Motion model § For our differential drive robot…
xt-1 xt-1 = yt-1 θt-1
ut = Δsr,t
Δsl,t
![Page 10: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/10.jpg)
10
EKFL Prediction Step
§ Motion model § And the model we derived…
xt-1 Δst cos(θt-1 +Δθt/2 ) x’t = f( xt-1 , ut ) = yt-1 + Δst sin(θt-1 +Δθt/2 )
θt-1 Δθt
Δst = (Δsr,t+Δsl,t )/2 Δθt = (Δsr,t - Δsl,t )/b
![Page 11: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/11.jpg)
11
EKFL Prediction Step
§ Covariance § Recall, the propagation of error equation…
![Page 12: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/12.jpg)
12
EKFL Prediction Step
§ Covariance § Our equation f() is not linear, so to use the
property we will linearize with first order approximation
x’t = f( xt-1 , ut ) ≈ Fx,t xt-1 + Fu,t ut
where Fx,t = Derivative of f with respect to state xt-1
Fu,t = Derivative of f with respect to control ut
![Page 13: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/13.jpg)
13
EKFL Prediction Step
§ Covariance § Here, we linearize the motion model f to obtain
P’t = Fx,tPt-1Fx,tT + Fu,tQtFu,t
T
where Qt = Motion Error Covariance Matrix Fx,t = Derivative of f with respect to state xt-1
Fu,t = Derivative of f with respect to control ut
![Page 14: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/14.jpg)
14
EKFL Prediction Step
§ Covariance
Qt = k |Δsr,t | 0 0 k |Δsl,t | Fx,t = df/dxt df/dyt df/dθt
Fu,t = df/dΔsr,t df/dΔsl,t
![Page 15: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/15.jpg)
15
EKFL Prediction Step
1. Motion Model
x’t xt-1
![Page 16: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/16.jpg)
16
Extended Kalman Filter Localization
1. EKF Localization Overview 2. EKF Prediction 3. EKF Correction 4. Algorithm Summary
![Page 17: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/17.jpg)
17
EKFL Correction Step
§ Innovation § We correct by comparing current measurements zt
with what we expect to observe zexp,t given our predicted location in the map M.
§ The amount we correct our state is proportional to the innovation vt vt = zt - zexp,t
![Page 18: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/18.jpg)
18
EKFL Correction Step
§ The Measurement § Assume our robot measures
the relative location of a wall i extracted as line
zit = αi
t Rit = σi
αα,t σiαr,t
ri
t σirα,t
σirr,t
![Page 19: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/19.jpg)
19
EKFL Correction Step
§ The Measurement § Assume our robot measures the relative
location of a wall i extracted as line zi
t = αit =g(ρ1, ρ2,…, ρn, β1, β2,…, βn)
rit
β β
β β
β
β
β
![Page 20: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/20.jpg)
20
EKFL Correction Step
§ The Measurement Ri
t = σiαα,t
σiαr,t
σi
rα,t σi
rr,t
= Gρβ,t Σz,t Gρβ,t T
where Σz,t = Sensor Error Covariance Matrix Gρβ,t = Derivative of g() wrt measurements ρt, βt
![Page 21: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/21.jpg)
21
EKFL Correction Step
zi
exp,t = hi( x’t , M) = αiM – θ’t
ri – x’t cos(αiM) – y’ t sin(αi
M)
![Page 22: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/22.jpg)
22
EKFL Correction Step
xcos(α)
x ysin(α)
y
![Page 23: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/23.jpg)
23
EKFL Correction Step
§ The covariance associate with the innovation is
ΣIN,t = Hix,tP’t Hi
x,tT + Ri
t
where Ri
t = Line Measurement Error Covariance Matrix Hi
x,t = Derivative of h with respect to state xt
![Page 24: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/24.jpg)
24
EKFL Correction Step
§ Final updates § Update the state estimate
xt = x’t + Kt vt
§ Update the associated covariance matrix Pt = P’t – Kt ΣIN,t Kt
T
§ Both use the Kalman gain Matrix Kt = P’t Hx’,t
T (ΣIN,t )-1
![Page 25: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/25.jpg)
25
EKFL Correction Step
§ Compare with single var. KF § Update the state estimate
xt = xt-1 + Kt (zt - xt-1 )
§ Update the associated covariance matrix σt
2= σt-12 -Kt σt-1
2
Both use the Kalman gain Matrix
Kt = σt-12
σt-12 + σt
2
⌃ ⌃ ⌃
![Page 26: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/26.jpg)
26
EKFL Correction Step
§ Final updates § By fusing the
prediction of robot position (magenta) with the innovation gained by the measurements (green) we get the updated estimate of the robot position (red)
![Page 27: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/27.jpg)
27
Extended Kalman Filter Localization
1. EKF Localization Overview 2. EKF Prediction 3. EKF Correction 4. Algorithm Summary
![Page 28: COS 495 - Lecture 17 Autonomous Robot Navigationweb.cecs.pdx.edu/~mperkows/temp/KALMAN/KALMAN-PAPERS/CO… · COS 495 - Lecture 17 Autonomous Robot Navigation Instructor: Chris Clark](https://reader034.vdocuments.mx/reader034/viewer/2022050206/5f59739580777c6fea5630b0/html5/thumbnails/28.jpg)
28
EKFL Summary
Prediction 1. x’t = f( xt-1 , ut ) 2. P’t = Fx,tPt-1Fx,t
T + Fu,tQtFu,tT
Correction 3. zi
exp,t = hi( x’t , M) 4. vt = zt - zexp,t 5. ΣIN,t = Hi
x’,tP’tHix’,t
T + Rit
6. xt = x’t + Kt vt
7. Pt = P’t – Kt ΣIN,t KtT
8. Kt = P’t Hx’,tT (ΣIN,t )-1