Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&&...
Transcript of Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&&...
![Page 1: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/1.jpg)
Kalman Filters: Examples
CS 4758 Ashutosh Saxena
![Page 2: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/2.jpg)
Methods
Bayes Filter
Par@cle Filter
Unscented Kalman Filter
Kalman Filter
Extended Kalman Filter
![Page 3: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/3.jpg)
PR2 localiza@on: Door Detector
![Page 4: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/4.jpg)
Example 2: Simple 1D Linear System with Erroneous Start
Given: F=G=H=1, u=cos(t/5) Ini@al state es@mate = 20 Linear system:
xt+1 = xt + cos(t / 5)+wt
zt+1 = xt+1 + nt+1Propaga@on: Update: (no change)
xt+1 = xt + cos(t / 5)Pt+1− = Pt +Qt
Unknown noise parameters
zt+1 = xt+1Kt+1 = P
−t+1(P
−t+1 + Rt+1)
−1
xt+1 = x−t+1 +Kt+1(zt+1 − x
−t+1)
Pt+1 = (I −Kt+1)P−t+1
![Page 5: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/5.jpg)
State Es@mate
![Page 6: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/6.jpg)
State Es@ma@on Error vs 3σ Region of Confidence
![Page 7: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/7.jpg)
Sensor Residual vs 3σ Region of Confidence
![Page 8: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/8.jpg)
Kalman Gain and State Covariance
![Page 9: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/9.jpg)
Approxima@ng Robot Mo@on Uncertainty with a Gaussian
![Page 10: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/10.jpg)
Robot Mo@ons are not linear
• Extended Kalman Filter
![Page 11: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/11.jpg)
Linearized Mo@on Model for PR2 / aerial robot.
X
Y
ω
x y
G
v
tt
t
tt
yVx
ωφ =
=
=
0From a robot-‐centric perspec@ve, the veloci@es look like this:
From the global perspec@ve, the veloci@es look like this:
tt
ttt
ttt
VyVx
ωφ
φ
φ
=
=
=
sincos
The discrete @me state es@mate (including noise) looks like this:
tw
twVyy
twVxx
t
t
t
ttt
tVttt
tVttt
δωφφ
φδ
φδ
ω )(ˆˆ
ˆsin)(ˆˆ
ˆcos)(ˆˆ
1
1
1
++=
++=
++=
+
+
+ Problem! We don’t know linear and rota@onal velocity errors. The state es@mate will rapidly diverge if this is the only source of informa@on!
![Page 12: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/12.jpg)
Linearized Mo@on Model for PR2/aerial robot
111
111
111
~ˆ
~ˆ
~ˆ
+++
+++
+++
=−
=−
=−
ttt
ttt
ttt
yyyxxx
φφφ
The indirect Kalman filter derives the pose equa@ons from the es@mated error of the state:
In order to linearize the system, the following small-‐angle assump@ons are made:
φφ
φ~~sin
1~cos
≅
≅
Now, we have to compute the covariance matrix propaga@on equa@ons.
![Page 13: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/13.jpg)
Linearized Mo@on Model for PR2/aerial robot
xt+1yt+1ϕ t+1
!
"
####
$
%
&&&&
=
1 0 −Vmδt sinϕ0 1 Vmδt cosϕ0 0 1
!
"
####
$
%
&&&&
xtytϕ t
!
"
####
$
%
&&&&
+
−δt cosϕR 0−δt sinϕR 0
0 −δt
!
"
####
$
%
&&&&
wVt
wωt
!
"
##
$
%
&&
Xt+1 = At Xt +GtWt
From the error-‐state propaga@on equa@on, we can obtain the State propaga@on and noise input func@ons F and G :
From these values, we can easily compute the standard covariance propaga@on equa@on:
Pt+1− = AtPtAt
T +GtQtGtT
![Page 14: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/14.jpg)
Sensor Model for a Robot with a Perfect Map
X
Y
x y
G
L
z
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
+⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
=
+
+
+
+
φφ nnn
yx
z y
x
L
L
L
t
t
t
t
1
1
1
1
From the robot, the measurement looks like this:
From a global perspec@ve, the measurement looks like:
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
+⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
−
−
−
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡ −
=
+
+
+
++
++
+
+
+
+
φφφ
φφ
φφ
nnn
yyxx
z y
x
tL
tL
tL
tt
tt
t
t
t
t
1
1
1
11
11
1
1
1
1
1000cossin0sincos
The measurement equa@on is nonlinear and must also be linearized!
![Page 15: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/15.jpg)
Sensor Model for a Robot with a Perfect Map
Now, we have to compute the linearized sensor func@on. Once again, we make use of the indirect Kalman filter where the error in the reading must be es@mated.
In order to linearize the system, the following small-‐angle assump@ons are made:
φφ
φ~~sin
1~cos
≅
≅
The final expression for the error in the sensor reading is:
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
+
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
−
−−−−−
−+−−−−
=⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
+
+
+
+++++
++++ +
+
+
+
φφ
φφφφ
φφφφ
φ nnn
yx
yyxxyyxx
yx
y
x
t
t
t
tLttLttt
tLttLtt
L
L
L t
t
t
t
1
1
1
11111
1111
~~~
100)ˆ(ˆsin)ˆ(ˆcosˆcosˆsin)ˆ(ˆcos)ˆ(ˆsinˆsinˆcos
~~~
1
1
1
1
![Page 16: Kalman&Filters:&Examples& - Cornell University• Extended&Kalman&Filter& Linearized&Mo@on&Model&& for&PR2&/&aerial&robot.& X Y ω y x G v t t t t t y x V φ = ω = = 0 From&arobotcentric&](https://reader030.fdocuments.in/reader030/viewer/2022040409/5ec11538d8993e7bd5313989/html5/thumbnails/16.jpg)
Upda@ng the State Vector
Propaga@on only Propaga@on and update