Homework: Ball Predictor for Robot Soccer

Transcription

1 Homework: Ball Predictor for Robot Soccer ECEn 483 November 17, Introduction The objective of this homework is to prepare you to implement a ball predictor in the lab An extended Kalman filter will be used to estimate the current position of the ball and to predict the future position of the ball T seconds into the future 2 Mathematical Model When the ball is not in contact with a wall or another robot, its dynamics are governed by Newton s second law: m b = F, where b = (b x, b y ) T is the position of the ball in world coordinates, m is the mass of the ball, and F = (F x, F y ) T is the external force exerted on the ball The external forces will be composed of a term due to static friction term and a variety of other forces, which we will assume to be small The force due to static friction is constant, assuming the ball has a non-zero velocity, and in the direction opposing motion The static friction term can be expressed as F s = µ ḃ ḃ Therefore the total force is F = µ ḃ + δf, ḃ where we assume that δf can be modeled as a zero mean, Gaussian random variable with covariance Q In component form we have ( bx by ) = µ m b x ḃ2 x +ḃ2 y b y ḃ2 x +ḃ2 y + 1 m ( ) δfx δf y 1

2 The constant µ m is unknown A common trick is to assume that this constant is also a state of the system and then use the Kalman filter to predict the constant, along with the other states The dynamics for a constant are Defining the state vector as and the process disturbance as d(µ/m) dt x 1 x = x 3 x 4 x 5 = = 0 b x b y ḃx ḃyµ m, ( ) q1 = 1 ( ) δfx, q 2 m δf y we can write the nonlinear state space equations as ẋ x 3 1 ẋ 2 x 4 ẋ 3 ẋ 4 = x3x x 4x (1) ẋ Assuming that an overhead camera is used to measure b, the output equation is ( ) ( ) ( ) bx 1 rx y = = x +, b y where r = (r x, r y ) T is the measurement noise and is assumed to be a zero mean Gaussian random variable with covariance R The application of the extended Kalman filter will require the computation of the Jacobian of f: f x = f x = r y Given f in Equation (1) we have x 5 + x2 3 x 5 x 3 x 4 x 5 x 3 + ( 4 3 +x2 4 )3/2 ( 3 3 +x2 4 )3/2 x 3 x 4 x 5 x ( 5 + x2 4 x 5 x 3 +x2 4 )3/2 3 + ( x2 4 )3/

3 3 Extended Kalman Filter The continuous-discrete extended Kalman filter is given by the following algorithm System Model where q N (0, Q) and r k N (0, R) ˆx = f(ˆx, u) + Gq z(kt ) = C ˆx(kT ) + r k, Initialization The first step is to initialize ˆx and P to ˆx 0 and P 0 Time update In between measurements, the state estimate ˆx and the covariance matrix P are propagated according to the differential equations ˆx = f(ˆx, u) A = f x (x=ˆx,u) P = AP + P A T + GQG T In simulation and in the lab, these differential equations will need to solved numerically This can be done by approximating the time derivative with an Euler approximation, and propagating the following difference equations ˆx[k + 1] = ˆx[k] + T p (f(ˆx, u[k])) (2) A = f (3) x (x= x[k+1],u[k]) ˆ P [k + 1] = P [k] + T p ( AP [k] + P [k]a T + GQG T ), (4) where T p is the sample rate of propagation, T p << T s, and T s is the sample rate of the measurement Measurement update When a measurement is received, we stop the time propagation and let ˆx and P be the values of ˆx as currently computed by Equations (2) and (4), respectively The values of ˆx and P are revised, according to the measurement, as follows: L = P C T [CP C T + R] 1 (5) P = (I LC)P (6) ˆx = ˆx + L(z k C ˆx ) (7) 3

4 4 Prediction Equations If at time t, we would like to predict the state at time t + T, where T > 0, then we need to add an additional step In particular we need to integrate the system equations ˆx = f(ˆx, û) with initial condition ˆx(t) for T seconds into the future Pseudo code for the complete algorithm is shown below Algorithm 1 Ball Predictor 1: Input: (1) Measurement z, (2) Prediction time T 2: In between measurements: 3: for i = 1 to N do 4: Compute ˆx = ˆx + T s N f(ˆx, u) 5: Correct ˆx to take into account wall bounces 6: Compute A = f(ˆx,u) x ( 7: Compute P = P + T s n AP + P A T + GQG ) T 8: end for 9: At a measurement 10: L = P C ( T CP C T + R ) 1 11: P = (I LC)P 12: ˆx = ˆx + L (z C ˆx) 13: Correct ˆx to take into account wall bounces 14: At each time t, predict ahead T seconds given the current estimate 15: ˆx p = ˆx 16: while t p < T do 17: Compute ˆx p = ˆx p + T s N f(ˆx p, u p ) 18: Correct ˆx p to take into account wall bounces 19: Update t p = t p + Ts N 20: end while 5 Application to Ball Prediction Download the matlab/simulink files associated with this homework set Problems 1 Implement the extended Kalman filter using the Matlab/Simulink code provided with this homework set Use matrix notation 2 Modify your code to allow you to return the predicted state T seconds into the future 3 Explicitly write out the formula for each element of the matrix update equations Ie, multiply out the matrix equations and derive an equation for each term 4

5 4 Modify your simulation so that it does not use any matrix multiplies Turn in your code for kalman filterm and sample plots of the estimation error for each state 5