Self-Driving Car ND - Sensor Fusion - Extended Kalman Filters

Transcription

1 Self-Driving Car ND - Sensor Fusion - Extended Kalman Filters Udacity and Mercedes February 7, 07 Introduction Lesson Ma 3 Estimation Problem Refresh 4 Measurement Udate Quiz 5 Kalman Filter Equations In C++ The state transition function is x f(x) + ν F x + }{{} Bu +ν () 0 The motion control, Bu, is zero in our case, so our state transition function is simlified to: The measurement function is x F x + ν () z h(x ) + ω Hx + ω (3) where, ν is the rocess noise. It is a Gaussian with zero mean and covariance matrix Q: ν N(0, Q) (4) ω is the measurement noise. It is a Gaussian with zero mean and covariance matrix R: ω N(0, R) (5) The state vector: Linear motion: x ( ) v (6) + v t (7) v v (8)

2 The state rediction function: The measurement function: ( ) v ( t 0 ) ( v z ( 0 ) ( v ) ) (9) (0) 5. Kalman Filter Algorithm Prediction: x F x + u () P F P F T + Q () Measurement Udate: y z Hx (3) S HP H T + R (4) K P H T S (5) x x + Ky (6) P (I KH)P (7) 6 Kalman Filter Equation in C++ Programming Assignment 7 Measurement Udate Quiz 8 State Prediction - 4D Case x y v x (8) 9 Uncertainty Deending on Time Delay Quiz 0 Uncertainty Deending on Acceleration Quiz

3 Process Covariance Matrix Linear Motion Model - D: Can be exressed in a matrix form as: x 0 t 0 y v x 0 0 t y v x x F x + ν (9) x + v x t + ν x y y + t + ν y v x (0) v x + ν vx + ν vy. Deriving the D velocity with a constant acceleration ν x ν y ν vx ν vy () From the kinematic formulas we can define the acceleration as the change in the velocity over the elased time, so the current velocity minus revious velocity over the elased time. a v t v k+ v k () t Then we can derive the current velocity as the revious velocity lus the acceleration a times t. v k+ v k + a t (3) Generalized in D we have both seed comonents described as follows: { v x v x + a x t v y + a y t. Deriving the relation between the revious location and current osition in D when having a motion with constant acceleration Similarly we know that the dislacement equals the average velocity v avg times t. (4) k+ k v avg t (5) which is the revious velocity v k+ lus current velocity v k over two. k+ k v k+ + v k t (6) k+ k + v k+ + v k t (7) And as we already saw, the current velocity v k+ is our old velocity v k lus the acceleration a times t. v k+ v k + a t (8) so we can relace the v k+ with v k + a t in revious equations: k+ k + v k + a t + v k t (9) k+ k + v k t + a t (30) And finally we get the full relation between the revious location k and current location k+ : k+ k + v k t + a t Going in D, we ll have both horizontal and vertical directions: { x + v x t + ax t y y + t + ay t (3) (3) 3

4 .3 The D motion model with constant acceleration Combining both D osition and D velocity equations reviously deducted formulas we have: x + v x t + ax t y y + t + ay t (33) v x v x + a x t + a y t.4 Process Noise in D Since the acceleration is unknown we can add it to the noise comonent, and this random noise would be exressed analytically as the last terms in the equation derived above. So, we have a random acceleration vector ν in this form: ν ν x ν y ν vx ν vy a x t a y t a x t a y t which is described by a zero mean and a covariance matrix Q. (34) ν N(0, Q) (35) The vector ν can be decomosed into two comonents a 4 by matrix G which does not contain random variables and a by matrix a which contains the random acceleration comonents: a x t t a y t ν 0 ( ) a x t t 0 ax t 0 Ga (36) a y }{{} a y t 0 t a }{{} G t is comuted at each Kalman Filter ste and the acceleration is a random vector with zero mean and standard deviations σ ax and σ ay. Based on our noise vector we can define now the new covariance matrix Q. The covariance matrix is defined as the exectation value of the noise vector ν times the noise vector ν transose. So let s write this down: Q E[νν T ] E[Gaa T G T ] (37) As G does not contain random variables, we can ut it outside the exectation calculation. ( ) σ Q GE[aa T ]G T G ax σ axy G T GQ ν G T (38) This leaves us with three statistical moments: σ axy the exectation of ax times ax, which is the variance of ax: σ ax. the exectation of ay times ay, which is the variance of ay: σ ay squared. And the exectation of ax times ay, which is the covariance of ax and ay: σ axy. a x and a y are assumed uncorrelated noise rocesses. This means that the covariance σ axy in Q ν is zero: ( ) ( ) σ Q ν ax σ axy σ σ axy σay ax 0 0 σay (39) So after combining everything in one matrix we obtain our 4 by 4 Q matrix: Q GQ ν G T σ ay t 4 4 σ t ax 0 3 t 0 4 σ ax 0 4 σ t ay 0 3 σ ay t 3 σ ax 0 t σax 0 0 t 3 σ ay 0 t σay 4 (40)

5 .5 other equations Note: Some authors describe Q as the comlete rocess noise covariance matrix. And some authors describe Q as the covariance matrix of the individual noise rocesses. In our case, the covariance matrix of the individual noise rocesses matrix is called Q ν. So we should be aware of that. Process Covariance Matrix Text 3 Laser Measurements 4 Find Out the H Matrix 4. Quiz question 4. Quiz answer ( x ( x ) y y ) (? ) x y v x (4) ( ) x y v x (4) 5 Find out the Dimension of R 5. Quiz question Now, can you determine what is the dimensionality of the measurement noise covariance matrix R. 5. Quiz answer ( ) σ R E[ωω T ] x 0 0 σy (43) 6 Programming Assignment ( ) 0. 0 R 0 0. ( ) 0 R 0 (44) (45) 6. Other Equations a x N(0, σ ax) (46) a y N(0, σ ay) (47) 5

6 7 Radar Measurements The state transition function: The measurement function: x f(x) + ν (48) z h(x ) + ω (49) The state transition function will be exactly what you ve designed in the lidar case. However our new Radar sees the world differently. Instead of a D ose (, y ), the radar can directly measure the object range ρ, bearing ϕ and range rate ρ: ρ z ϕ (50) ρ The range ρ is the radial distance from the origin to our edestrian. So we can always define a ray which extends from the origin to our object osition. The bearing ϕ is the angle between the ray and x direction. And the range rate ρ, also known as Doler or radial velocity is the velocity along this ray. And similar to our motion model, the radar observations are corruted by a zero-mean random noise ω N(0, R). Considering that the three measurement comonents of the measurement vector z are not cross-correlated, the radar measurement covariance matrix R becomes a 3 by 3 diagonal matrix: σ ρ 0 0 R 0 σϕ 0 (5) 0 0 σ ρ And the next question is what is the measurement function h(x ) that mas the redicted state x into the measurement sace? ρ x h(x ϕ ) y v x ρ (5) And that s how I defined the h function which basically secifies how the redicted osition and seed can be related to the observed range ρ, bearing ϕ and range rate ρ: x + y h(x ) arctan( y/ x) (53) x v x + y x + y 7. Deriving the Radar Measurement Function The measurement function is comosed of three comonents which show how the redicted state x ( x, y, v x, v y) T is maed into the measurement sace z (ρ, ϕ, ρ) T :. The range ρ is the distance to the edestrian which can be defined as: ρ x + y (54). ϕ is the angle between ρ and x direction and can be defined as: 3. Range rate ρ(t) derivation: ϕ arctan( y / ) (55) 6

7 Method : Generally we can exlicitly describe the range ρ as a function of time: ρ(t) (t) + y (t) (56) The range rate ρ(t) is defined as time rate of change of the range ρ, and it can be described as the time derivative of ρ: ρ (t) t t (t) + y (t) (t) + y (t) ( t (t) + t y(t) ) (t) + y (t) ((t) t (t) + y (t) t y(t)) t (t) is nothing else than v x (t), similarly t y(t) is (t). So we have: ρ (t) t (t) + y (t) ((t)v x (t)+ y (t) (t)) ((t)v x (t) + y (t) (t)) (t) + y (t) (t)v x (t) + y (t) (t) x (t) + y (t) For the simlicity we just use the following notation: ρ v x + y v y x + y (57) Method : The range rate ρ can be seen as a scalar rojection of the velocity vector v onto ρ. Both ρ and v are D vectors defined as: ( ) ( ) x vx ρ, v (58) y The scalar rojection of the velocity vector v onto ρ is defined as: ( ) ( ) vx x ρ v ρ ρ y v x + y v y x + y x + y (59) where ρ is the length of ρ. In our case it is actually the range, so ρ ρ 8 Maing with a Nonlinear Function Quiz 9 Extended Kalman Filter 0 Linearization Examle 0.0. Linearization of a function The Extended Kalman Filter uses a method called first order Taylor exansion h(x) h(µ) + h(µ) (x µ) (60) x 7

8 0. 0_quiz_linearization_examle 0.. quiz answer In our examle µ 0, therefore: h(x) x h(x) arctan(x) (6) arctan(x) x + x (6) h(x) arctan(x) (63) h(x) h(µ) + h(µ) (x µ) arctan(µ) + (x µ) (64) x + µ h(x) arctan(0) + (x 0) x (65) + 0 So, the function h(x) arctan(x) will be aroximated by a line: Jacobian Matrix State transition function: Measurement function: h(x) x (66) x f(x) + ν (67) z h(x ) + ω (68) where we consider that f(x) and h(x) are nonlinear and can be linearized as: f(x) f(µ) + f(µ) (x µ) (69) } x {{} F j h(x) h(µ) + h(µ) (x µ) (70) } x {{} H j The derivative of f(x) and h(x) with resect to x are called Jacobians: f(µ) x F j (7) h(µ) x H j (7) and it is going to be a matrix containing all the artial derivatives: H j h h x h h x. h m x h x n h x n x... x h m x... To be more secific, I know that my function h describes three comonents: range ρ, bearing ϕ and range-rate ρ, and my state is a vector with four comonents, y, v x,. Then the Jacobian matrix H j is going to be a matrix with 3 rows and four columns: H j y y y h m x n (73) (74) 8

9 After comuting all these artial derivatives we have: H j y x + y x + y y x + y x + y y(v x y ) ( v x y) ( x + y )3/ ( x + y )3/ x + y y x + y (75) If you re interested to see more details about how I got this results have a look at details below..0. Derivation of the Jacobian H j We re going to calculate, ste by ste, all the artial derivatives in H j : H j y y y So all the matrix H j elements are calculated as follows: (76). ( x + y) x x x + y x + y (77). y ( x + y) y y x + y y x + y (78) ( v x + y) 0 (79) x ( v x + y) 0 (80) y arctan( y / ) arctan( y / ) y y ( y ( y ) + ( y x ) y x + y ) + ( ) x + y x + y (8) (8) arctan( y / ) 0 (83) arctan( y / ) 0 (84) v x + y x + y (85) In order to calculate the derivative of this function we use the quotient rule. Given a function z that is quotient of two other functions f and g: z f g (86) 9

10 its derivative with resect to x is defined as: in our case: Their derivatives are: f z x x g g x f g (87) f v x + y (88) g x + y (89) f ( v x + y ) v x (90) g ( ) x + y x x + y (9) Putting everything together into the derivative quotient rule we have: v x x + y x ( x v x + y ) x + y x + y(v x y ) (9) y ( x + y) 3/ 0. Similarly y y v x + y x + y ( v x y ) ( x + y) 3/ (93).. v x + y x + y v x + y x + y x + y y x + y (94) (95) So now, after calculating all the artial derivatives our resulted Jacobian H j is: y 0 0 x + y x + y H j y x + y 0 0 x + y y(v x y ) ( v x y) ( x + y )3/ ( x + y )3/ EKF Algorithm Generalization 3 EKF with Linear Functions Quiz x + y 4 Sensor Fusion General Processing Flow 5 General EKF Algorithm y x + y (96) 0

11 6 Evaluating the Performance 6. RMSE RMSE n n t (x est t x true t ) (97) 7 3_State rediction - 4D case // T_SC_ Linear Motion - D: State transition: Prediction: Measurement Udate: x y v x }{{} x x + v x t y y + t v x v x 0 t t } 0 0 {{ } F y v x }{{} x (98) (99) x f(x, u) (00) P F j P F T j + Q (0) y z h(x ) (0) 8 Outro z (03) z ( x y ) (04) References