16-311-Q I NTRODUCTION TO R OBOTICS F ALL ’17 L ECTURE 20: E XTENDED K ALMAN F ILTER I NSTRUCTOR : G IANNI A. D I C ARO
EKF FOR MAP-BASED ROBOT LOCALIZATION 1. Odometry measures: EKF with only motion Proprioceptive information 1. Action/prediction update: Proprioceptive sensing Exteroceptive sensing 2. Perception/measurement update: Exteroceptive sensing 2. Detection of landmarks: EKF with motion + observations 2
DISCRETE-TIME MOTION EQUATIONS From Runge-Kutta numeric integration 2 3 θ k + ∆ θ k � � x k + ∆ S k cos of pose evolution kinematic equations. 2 3 x k +1 2 6 7 Assume that the odometry model is 6 7 6 7 ξ k +1 = y k +1 θ k + ∆ θ k 5 = � � y k + ∆ S k sin 6 7 6 7 2 6 7 4 perfect, based on measured distance 4 5 θ k +1 θ k + ∆ θ k 𝛦 S, and heading variation 𝛦𝜾 Odometry measurements are noisy! ➔ Random noise is added to 𝛦 S and 𝛦𝜾 to model motion’s kinematics k ) cos( θ k + ∆ θ k Discrete-time ( ∆ S k + ν s 2 + ν θ k ) x k process (motion) ξ k +1 = + k ) sin( θ k + ∆ θ k ( ∆ S k + ν s 2 + ν θ y k k ) equations ∆ θ k + ν θ θ k k In absence of specific information, motion noise is modeled as Gaussian white noise (and the two noise components are assumed to be uncorrelated) " # σ 2 0 ⇤ T ∼ N (0 , V k ) , ks Process noise ⇥ ν s ν θ ν k = V k = k k σ 2 0 k θ 3
NON LINEARITY OF DISCRETE-TIME MOTION EQUATIONS k ) cos( θ k + ∆ θ k Discrete-time ( ∆ S k + ν s 2 + ν θ k ) x k process (motion) ξ k +1 = + k ) sin( θ k + ∆ θ k ( ∆ S k + ν s 2 + ν θ y k k ) equations ∆ θ k + ν θ θ k k ⇤ T ∼ N (0 , V k ) ν θ ⇥ ν s ξ k +1 = f ( ξ k , ∆ S k , ∆ θ k , ν k ) , ν k = k k Process’ dynamics function, f() , is not linear ➜ Process equations do not meet the linearity requirement for using the Kalman filter Linearize pose evolution f() in the neighborhood of [^ ξ k|k uk ( 𝝃 k = 0)] , the current state estimate, controls ( 𝛦 S k and 𝛦𝜾 k ), and mean of process noise ξ k | k , u k , 0 + ( ξ k − b f ( ξ k , u k , ν k ) = f ( ξ , u , ν ) | b ξ k | k ) F ξ | b ξ k | k , u k , 0 + ( ν k − 0 ) F ν | b ξ k | k , u k , 0 1st order = f k ( b ξ k | k , u k , 0 ) + ( ξ k − b ξ k | k ) F k ξ + ν k F k ν Linear in ξ k and 𝝃 k Taylor series 4
EXTENDED KALMAN FILTER (EKF): LINEARIZED MOTION MODEL Scenario (Prediction from motion) : The robot does move but no external observations are made. Proprioceptive measures from the on-board odometry sensors are used to model robot’s motion dynamics avoiding to consider the direct control inputs. Linear(ized) discrete-time ξ k +1 = f k ( b ξ k | k , u k , 0 ) + ( ξ k − b ξ k | k ) F k ξ + ν k F k ν process (motion) equations Linearization of motion dynamics using the Jacobians F k ξ and F k ν , that have to be evaluated in ( ξ k = b ξ k | k , u k , ν k = 0) ➔ Rules for linear transformations of mean and (co)variance of Gaussian variables can be applied Extended Kalman Filter (EKF) - Motion only ( b ξ k +1 | k = f k ( b ξ k | k , 0 ; ∆ S k , ∆ θ k ) + ( b ξ k | k − b ξ k | k ) F ξ | b (State prediction) ξ k , u k , 0 Prediction update = 0 P k +1 | k = F k ξ P k F T k ξ + F k ν V k F T (Covariance prediction) k ν 8 b ξ k +1 = b ξ k +1 | k + G k +1 ( z k +1 − C k +1 b > ξ k +1 | k ) (State update) > < P k +1 = P k +1 | k − G k +1 C k +1 P k +1 | k (Covariance update) Measurement correction > > : k +1 + W k +1 ) − 1 (Kalman gain) G k +1 = P k +1 | k C T k +1 ( C k +1 P k +1 | k C T 5
EKF JACOBIANS FOR THE LINEARIZED MOTION MODEL The Jacobian of the non-linear function f () is computed in [ ^ ξ k|k uk ( 𝝃 k = 0) ] , the current state estimate (the mean), the current controls, the mean of the Gaussian noise k ) cos( θ k + ∆ θ k x k + ( ∆ S k + ν s 2 + ν θ f kx = k ) f () is a vector function with k ) sin( θ k + ∆ θ k y k + ( ∆ S k + ν s 2 + ν θ f ky = k ) three function components: θ k + ∆ θ k + ν θ f k θ = k The Jacobian matrix of f: 2 3 ∂ f kx ∂ f kx ∂ f kx ∂ f kx ∂ f kx ∂ν s ∂ν θ ∂ x k ∂ y k ∂θ k 6 7 k k 6 7 6 7 ⇤ T = ∂ f ky ∂ f ky ∂ f ky ∂ f ky ∂ f ky 6 7 F k ( x k , y k , θ k , ν s k , ν θ ⇥ ⇥ ⇤ k ) = = r f kx r f ky r f k θ F k ξ F k ν 6 7 6 ∂ν s 7 ∂ν θ ∂ x k ∂ y k ∂θ k 6 7 k k 6 7 6 7 ∂ f k θ ∂ f k θ ∂ f k θ ∂ f k θ ∂ f k θ 4 5 ∂ν s ∂ν θ ∂ x k ∂ y k ∂θ k k k 2 � ∆ S k sin( θ k + ∆ θ k 3 2 cos( θ k + ∆ θ k � ∆ S k sin( θ k + ∆ θ k 3 1 0 2 ) 2 ) 2 ) 6 ∆ S k cos( θ k + ∆ θ k 7 6 sin( θ k + ∆ θ k ∆ S k cos( θ k + ∆ θ k 7 F k ξ = F k ν = 0 1 2 ) 2 ) 2 ) 6 7 6 7 4 5 4 5 0 0 1 0 1 b b ξ k | k , u k , ν =0 ξ k | k , u k , ν =0 6
R E C A P O N D E R I VAT I V E S , G R A D I E N T S , J A C O B I A N S Def. Derivative: Given a scalar function f : X ✓ R 7! R , if the limit f ( x 0 + h ) � f ( x 0 ) lim h h ! 0 exists and takes a finite value, f is di ff erentiable in x 0 2 X and the value of the limit is the derivative of the function in x 0 , which is also indicated with f 0 ( x 0 ) def df = dx ( x 0 ) Geometric interpretation: the derivative is the slope of the tangent to the graph of f in point ( x 0 , f ( x 0 )) . This can be shown considering that the line passing for two points ( x 0 , f ( x 0 )) and (( x 0 + h ) , f ( x 0 + h )) belonging to the graph f , is y = mx + f ( x 0 + h ) , where the slope is m = f ( x 0 + h ) � f ( x 0 ) . If h ! 0 , the secant to the curve overlaps with the tangent in x 0 . That ( x 0 + h ) � x 0 is, the equation of the tangent to f in x 0 is: y = f ( x 0 ) + f 0 ( x 0 )( x � x 0 ) , which is precisely the first-order Taylor series computed in x 0 . f(x 0 +h) f(x) f(x) h ! 0 f(x 0 ) f(x 0 ) x 0 x 0 +h x 0 7 x x
R E C A P O N D E R I VAT I V E S , G R A D I E N T S , J A C O B I A N S Gradient: “derivative” for scalar functions of multiple variables ! Normal to the tangent hyperplane to the function graph. Given a scalar, di ff erentiable, multi-variable function f : R n 7! R , its gradient is the vector of its partial derivatives: ⇣ ∂ f , ∂ f , . . . , ∂ f = ∂ f e 1 + ∂ f e 2 + . . . + ∂ f ⌘ def = r f ( x 1 ,x 2 ,...,x n ) e n ∂ x 1 ∂ x 2 ∂ x n ∂ x 1 ∂ x 2 ∂ x n For f : X ✓ R n 7! R , the Taylor series becomes: 1 X k ! ∂ k [ f ( x 0 )]( x � x 0 ) k f ( x ) | x 0 = | k | � 0 where k is a multi-index, an integer-valued vector, k = ( k 1 , k 2 , . . . , k n ) , k i 2 Z + , and ∂ k f j f = ∂ j f means ∂ k 1 1 f ∂ k 2 2 f · · · ∂ k n n f , where ∂ i . The 2nd order polynomial is: ∂ x j i f ( x ) = f ( x 0 ) + r f ( x 0 ) T ( x � x 0 ) + 1 2 ( x � x 0 ) T H ( f ( x 0 ))( x � x 0 ) Removing the quadratic part, the linear approximation is obtained, that is, the equation of the tangent hyperplane in x 0 , where the gradient is normal to the tangent hyperplane Jacobian: “gradient” for vector functions of multiple vari- ables ! Each function component has a tangent hyperplane to the function graph ! Map of tangent hyperplanes 8
E F F E C T O F L I N E A R I Z AT I O N : L I N E A R C A S E 9
E F F E C T O F L I N E A R I Z AT I O N : N O N L I N E A R C A S E 10
E F F E C T O F L I N E A R I Z AT I O N : N O N L I N E A R C A S E 11
E F F E C T O F L I N E A R I Z AT I O N : N O N L I N E A R C A S E 12
E F F E C T O F L I N E A R I Z AT I O N : N O N L I N E A R C A S E 13
E R R O R I N L O C A L I Z AT I O N K E E P S G R O W I N G The ellipses in the plot show the error in ( x, y ) , but also the error in θ (the third component of the covariance matrix) grows (but usually less than that in ( x, y ) ) 14
U N C E RTA I N T Y A S P R O C E S S VA R I A N C E The magnitude of the total uncertainty, including both position and heading, is quantified by q det ( ˆ P ) , shown in the plot for di ff erent values of V = α V 0 , α = { 0 . 5 , 1 , 2 } the 15
EKF FOR MAP-BASED ROBOT LOCALIZATION 1. Odometry measures: EKF with only motion Proprioceptive information 1. Action/prediction update: Proprioceptive sensing Exteroceptive sensing 2. Perception/measurement update: Exteroceptive sensing 2. Detection of landmarks: EKF with motion + observations 16
U S I N G M A P S T O R E D U C E T H E E R R O R Exteroceptive measures are needed in the filter to reduce pose uncertainty A map is provided to the robot: a list of objects in the environment along with their properties Let’s consider the case in which the map contains n fixed landmarks with their position. Each landmark is identifiable by the robot through a set of detectable features 17
Recommend
More recommend
