Estimation in Robotics
slides are from different sources, e.g. D. Lee(UPenn), D. Kelin & P. Abbeel (UC Berkeley), UW (D. Fox), Wikipedia, …
p(x)~ N(μ,s2):
Copyright By PowCoder代写 加微信 powcoder
1 -1 (x-μ)2
Univariate
p(x) ~ Ν (μ,Σ) :
p(x)= 1 e-1(x-μ)t Σ-1(x-μ)
2 (2p)d/2 Σ1/2
Multivariate
Properties of Gaussians
X ~ N(μ,s2)ü
Y = aX + b
22 Þ Y~N(aμ+b,as)
X~N(μ,s2)ü
s2 1 öú 1 μ ,
1 ýÞ p(X1)×p(X2)~ N
çs 2 +s 2
s 2 +s 2 2 s -2 +s -2 ú
1 222þè121212ø
~ N(μ ,s 2)
Multivariate Gaussians
X ~ N(μ,S) ü Y=AX+B ýþ
Þ Y~N(Aμ+B,ASAT)
11121 X~N(μ,S)üÞp(X)×p(X)~Nç S μ+ S μ, 1 úö
X ~ N(μ ,S )ý 1 222þè121212ø
S +S 2 S-1 +S-1 ú
• We stay in the “Gaussian world” as long as we start with Gaussians and perform only linear transformations.
Estimates the state x of a discrete-time controlled process that is governed by the linear stochastic difference equation
xt =Atxt-1+Btut +et
with a measurement
zt =Ctxt +dt
Components of a
Matrix (nxn) that describes how the state evolves from t-1 to t without controls or noise.
Matrix (nxl) that describes how the control ut changes the state from t to t-1.
Matrix (kxn) that describes how to map the state xt to an observation zt.
Random variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance Rt and Qt respectively.
Linear Gaussian Systems: Initialization
• Initial belief is normally distributed: bel(x0) = N(x0;μ0,S0 )
Linear Gaussian Systems: Dynamics
• Dynamics are linear function of state and control plus additive noise:
xt =Atxt-1+Btut +et
p(xt |ut,xt-1)=N(xt;Atxt-1+Btut,Rt)
Linear Gaussian Systems: Observations
• Observations are linear function of state plus additive noise:
zt =Ctxt +dt
p(zt |xt)=N(zt;Ctxt,Qt)
5. 6. 7. 8.
Algorithm Kalman_filter(μt-1,St-1,ut ,zt): Prediction:
μt =Atμt-1+Btut St=ASAT+R
tt-1t t Correction:
K=StCT(CStCT+Q)-1 ttttt
μt =μt +Kt(zt -Ctμt) St =(I-KtCt)St
Returnμt,St
•Highly efficient: Polynomial in
measurement dimensionality k and
state dimensionality n: O(k2.376 + n2)
•Optimal for linear Gaussian systems! •Most robotics systems are nonlinear!
Going non-linear
EXTENDED KALMAN FILTER
Nonlinear Dynamic Systems
•Most realistic robotic problems involve nonlinear functions
xt =g(ut,xt-1)
• Linearize around the transition function
• Jacobian represents the derivative in matrix form
• Calculate the uncertainty update via linearization
non-linear Motion 𝑥𝑡+1 = g 𝑥𝑡
non-linear Observation 𝑧𝑡+1 = h 𝑥𝑡
Dynamical System: &
• Covariance prediction
𝑝(𝑥𝑥 𝜕𝑔𝜕𝑔%
𝑡+1|𝑡)=𝒩(𝑔𝑥$, 𝑃$ +Σ&) 𝜕𝑥 𝜕𝑥
!” Represents the Jacobian of g !#
• Overall update
𝒙$ = 𝒈 𝒙 + 𝑲 ( 𝒛 − 𝒉 ( 𝒈 ( 𝒙 ) ) ) 𝒕 𝒕3𝟏 𝒕 𝒕
𝑷. = 𝑷 − 𝑲 𝝏 𝒈 𝑷 𝒕 𝝏𝒙
Nonlinear Dynamic Systems
•Most realistic robotic problems involve nonlinear functions
xt =g(ut,xt-1)
Linearity Assumption Revisited
Non-linear Function
EKF Linearization (1)
EKF Linearization (2)
EKF Linearization (3)
EKF Linearization:
We use First Order Expansion for linearization.
Recap:Taylor series expansion
• In mathematics, the Taylor series of a function is an infinite sum of terms that are expressed in terms of the
function’s derivatives at a single point.
• For most common functions, the function and the sum of its Taylor series are equal near this point. Taylor’s series are named after Brook Taylor who introduced them in 1715.
The Taylor series of a real or complex-valued function f (x) that is infinitely differentiable at a real or complex number a is the power series
EKF Linearization: First Order Expansion
• Prediction: g(ut,xt-1)»g(ut,μt-1)+¶g(ut,μt-1)(xt-1 -μt-1)
¶xt-1 g(ut,xt-1)»g(ut,μt-1)+Gt (xt-1 -μt-1)
• Correction: h(xt)»h(μt)+¶h(μt)(xt -μt)
¶xt h(xt)»h(μt)+Ht (xt -μt)
EKF Algorithm
1. Extended_Kalman_filter(μt-1,St-1,ut,zt ): 2. Prediction:
3. μt =g(ut,μt-1) 4. St=GSGT+R
μt =Atμt-1+Btut St=ASAT+R
t t-1 t t 5. Correction:
9. Returnμt,St
K =StHT(HStHT +Q)-1
ttttt ttttt
K =StCT(CStCT +Q)-1 μt =μt +Kt(zt -Ctμt)
St =(I-KtCt)St
G =¶g(ut,μt-1)
μt =μt +Kt(zt -h(μt)) St =(I-KtHt)St
Localization
“Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.” [Cox ’91]
• Map of the environment.
• Sequence of sensor measurements.
• Estimate of the robot’s position.
• Problem classes • Position tracking
• Global localization
• Kidnapped robot problem (recovery)
Landmark-based Localization
1. EKF_localization(μt-1,St-1,ut,zt ):
EKF Summary
•Highly efficient: Polynomial in
measurement dimensionality k and
state dimensionality n: O(k2.376 + n2)
• Not optimal!
•Can diverge if nonlinearities are large!
•Works surprisingly well even when all assumptions are violated!
程序代写 CS代考 加微信: powcoder QQ: 1823890830 Email: powcoder@163.com