Extended Kalman Filter Lecture Notes EEE 581-Spring 1997 Darryl Morrell 1 Introduction In this lecture note, we extend the Kalman Filter to non-linear system models to obtain an approximate filter–the Extended Kalman Filter. We will do this by finding an approximate error system that is linear, and applying the Kalman filter to this error system. Because the EKF is obtained using a linear approximation of a non-linear system, it offers no guarantees of optimality in a mean squared error sense (or in any other sense). However, for many systems, the EKF has proven to be a useful method of obtaining good estimates of the system state. 2 Discrete/Discrete EKF The system model that we use is the following discrete/discrete model: Xk+1 = f (Xk ; k ) + GWk (1) Zk = h (Xk ; k ) + Vk (2) In this model, Wk is a discrete-time white noise process with mean zero and covariance matrix Q, Vk is a discrete-time white noise process with mean zero and covariance matrix R, and Wj , Vk , and X0 are uncorrelated for all j and k . We develop the Extended Kalman filter by starting with a nominal reference trajectory R denoted xR k ; xk is obtained as the solution to the difference equation (1) without the process noise Wk : R = ; k xR f x (3) k +1 k This difference equation has an initial condition xR 0 . We define the error between Xk and xR as k k = Xk 0 xR (4) k We now find an approximate linear model that describes the dynamics of k : k+1 = Xk+1 0 xR k +1 = f (Xk ; k ) + GWk 1 0f xR k ;k To obtain a linear approximation of this equation, we make a Taylor series expansion1 of f (Xk ; k ) about the value xR k and drop all but the constant and linear terms: f (Xk ; k ) f xR k ;k @ f (x; k ) + We make the definition Xk 0 xR k @x x=xRk @ f (x; t) Ak = ; @x x=xRk and note that Ak is an n 2 n matrix of the following form: 2 @f1 (x;k ) @x1 6 .. . @fn (x;k ) .. . @fn (x;k ) ::: @x1 6 6 4 Ak = 3 @f1 (x;k ) @xn 7 ::: 7 7 5 x=xRk @xn With this definition, k+1 f xR k;k = Ak k + GWk + Ak Xk 0 xRk + GW k 0f xR k ;k (5) Note that (5) is a linear difference equation. We follow a similar approach to obtain an (approximate) linear relationship from (2). We expand h(Xk ; k ) in a Taylor series about the nominal trajectory xR k: h(Xk ; k ) h(xR k ; k) + We define Hk as @ h (x; k ) @x x=xRk X k 0 xR k Hk = @ h (x; k ) @x x=xRk Then we can write (2) as Zk h(xRk; k) + Hk Xk 0 xR k + Vk : Define k as k = = = 1 Zk 0 h(xR k ; k) h(Xk ; k ) + Vk 0 h(xRk; k) Hk Xk + Vk 0 xRk Hk k + Vk A Taylor series expansion of a vector function f (x) about the point x0 is the following: f (x) = f (x0 ) + @ f (x) (x 0 x0 ) + Higher Order Terms @ x x=x0 2 (6) So we have an (approximate) linear relationship between the quantity k (which can be computed from Zk ) and k . Assuming that k obeys the difference equation (5) and the observation equation (6), ^ we can use a discrete/discrete Kalman filter to compute ^ k+1jk , Pk+1jk , k +1jk +1 , and Pk +1jk +1 . R Note that the actual error between Xk and xk does not obey the linear equations (5) and (6). Thus, ^ k+1jk and ^ k+1jk+1 are not optimal MMSE estimates of the actual error; we hope, however, that they are still good estimates of this error. How do we obtain an estimate of Xk+1 from ^ k+1jk or ^ k+1jk ? Since ^ k+1jk and ^ k+1jk are estimates of the error between Xk and xR k , reasonable estimates of Xk +1 are ^ X k +1jk = ^ xR k +1 + k +1jk ^ X k +1jk +1 = ^ xR k +1 + k +1jk +1 How do we choose the reference trajectory? A reasonable value for xR 0 would be mX0 , the mean of the initial state. Using this value gives h ^ 0j0 = E [ 0 ] = E X0 0 xR 0 i =0 P0j0 = PX0 Thus, ^ 1j0 = A0^ 0j0 = 0 The error covariance for the estimate ^ 1j0 is T P1j0 = A0P0j0 AT 0 + GQG The estimate for X1 is ^ X 1j0 = ^ xR 1 + k +1jk = f (mX0 ; 0) ^ : Note that P1j0 is also approximately the error covariance of the estimate X 1j0 P1j0 = = At time k = 1, E E E 1 0 ^ j 1 0 10 X1 0 x1 R 0 ^ X1 0 X 1j0 ^ X j ^ j 10 0x 0 X^ j R 10 X1 T 1 T X1 0 x1 R 0 = 0 1 K1 = P1j0H1T H1 P1j0 H1T + R = ^ 1j0 + K1 1 0 H1 ^ 1j0 K1 1 ; 3 j 10 0x R T 1 (7) we process the observation Z1 to obtain ^ 1j1 ^ X 10 where we have used the fact that ^ 1j0 = 0. P1j1 = (I 0K H )P j : 1 1 10 ^ The estimate X 1j1 is obtained as ^ X 1j1 = = = = ^ xR 1 + 1j1 xR 1 + K1 1 h h R ^ X 1j0 + K1 Z1 0 h x1 ; 1 i i ^ ^ X 1j0 + K1 Z1 0 h X1j0 ; 1 ^ ; this can be shown using a Note that P1j1 is approximately the error covariance of X 1j1 derivation similar to (7). Now, to compute the estimate for k = 2, we could continue to use the reference trajectory that we used for k = 1; however, we now have a (hopefully) better estimate of ^ . Thus, to get the reference trajectory for k = 2, we set the state at time 1, namely X 1j1 R ^ ^ , where 3 represents x1 = X1j1. With this new reference trajectory, we have 31 = X1 0 X 1j1 1 3 the error with the new reference trajectory. Assuming that 1 has a mean of zero 2 , the estimate of 31 is ^3 1j1 = 0 and ^ 2j1 = A1^3 1j1 = 0 Thus, ^ X 2j1 = ^ xR 2 + 2j1 = ^ ;1 f X 1j1 The approximate error covariance for this estimate is P2j1 = A1P1j1 A1T + GQGT At k = 2, we process the observation Z2 in the same way as we processed the observation Z1 . Further prediction and filter updates follow this same pattern. To summarize, the Extended Kalman Filter equations are the following: Predictor Update: ^ X k +1jk = ^ ;k f X k jk T Pk+1jk = Ak Pkjk AT k + GQG Ak = @ f (x; t) @x 2 ^ x= X kk : j This assumption would be true if the system model were linear; since the system model is not linear, we hope that the assumption is approximately true and that the mean of 31 is approximately zero. 4 Filter Update: Hk+1 = @ h (x; tk ) @x ^ x=X k+1 k j ^ X k +1jk +1 ^ =X k +1jk + Kk +1 Pk+1jk+1 = (I 0 1 Kk+1 = Pk+1jk HkT+1 Hk+1 Pk+1jk HkT+1 + R h i ^ Zk+1 0 h X k +1jk ; k + 1 0 Kk : j +1 Hk +1 ) Pk +1 k ^ ^ Note that in the filter update equation for X k +1jk +1 , the residual Zk +1 0 h Xk +1jk ; k + 1 plays the same role as the innovations in the standard Kalman filter. This residual sequence can be monitored to verify correct filter operation, much in the same way that the innovation sequence can be monitored in the standard Kalman filter. 3 Continuous/Discrete EKF The development of the continuous/discrete EKF is very similar to the development of the discrete/discrete EKF. The system model that we use is the following continuous/discrete model: X_ (t) = f (X(t); t) + GW(t) (8) Z (tk ) = h (X(tk ); tk ) + Vk (9) In this model, W(t) is a continuous-time white noise process with mean zero and intensity Q, Vk is a discrete-time white noise process with covariance R, and W(t), Vk , and X(t0) are uncorrelated for all t and k . As in the derivation of the discrete/discrete Kalman filter, we develop the continuous/discrete Extended Kalman filter by starting with a nominal reference trajectory denoted xR (t); xR (t) is obtained as the solution to the differential equation (8) without the process noise W(t): x_ R (t) = f xR (t); t (10) This differential equation has some initial condition xR (t0). We denote the error between X(t) and xR (t) as (t) = X(t) 0 xR (t) (11) We now find an approximate linear model that describes the dynamics of (t) by taking the derivative of (11): _ (t) = = X_ (t) 0 x_ R (t) f (X(t); t) + GW(t) 0 f xR (t); t 5 To obtain a linear approximation of this equation, we make a Taylor series expansion of f (X(t); t) about the value xR (t) and drop all but the constant and linear terms: f (X(t); t) f xR (t); t + Denote @ f (x; t) @x x=xR (t) X(t) 0 xR (t) @ f (x; t) A(t) = ; @x x=xR (t) and note that A(t) is an n 2 n matrix of the following form: 2 A(t) = @f1 (x;t) @x1 6 6 6 4 3 @f1 (x;t) @xn 7 ::: .. . @fn (x;t) .. . @fn (x;t) ::: @x1 7 7 5 x=xR (t) @xn With this definition, _ (t) f xR (t); t = A(t) (t) + GW(t) + A(t) X(t) 0 xR (t) + GW(t) 0f xR (t); t (12) Note that (12) is now a linear differential equation. We follow a similar approach to obtain an (approximate) linear relationship from (9). We expand h(X(tk ); tk ) in a Taylor series about the nominal trajectory xR (tk ): h(X(tk ); tk ) h(x R (tk ) ; tk ) + We define Hk as @ h (x; tk ) @x x=xR (tk ) X(tk ) 0 xR (tk ) Hk = @ h (x; tk ) @x x=xR (tk ) Then we can write (9) as Z (tk ) h(xR (tk ) ; tk ) + Hk X(tk ) 0 xR (tk ) + Vk : Define (tk ) as (tk ) = Z (tk ) 0 h(xR (tk ) ; tk ) h(xR (tk ) ; tk ) + Hk X(tk ) 0 xR (tk ) 0 xR (tk) = Hk X(tk ) = Hk (tk ) + Vk + Vk 0 h(xR (tk ) ; tk ) + Vk (13) So we have an (approximate) linear relationship between the quantity (tk ) (which can be computed from Zk ) and (tk ). 6 We can now use a continuous/discrete Kalman filter to compute ^ (tjk ) and P (tjk ) using the linear dynamics equation (12) and the linear observation equation (13). An ^ (tjk ) is obtained as estimate of X(t) from ^ (tjk ) = xR (t) + ^ (tjk ) : X X How do we choose the reference trajectory? Let us first consider the interval t0 t t1; a reasonable value for xR (t0) would be mX (t0) = E [X (t0)]. Using this value gives E [ (t0)] = 0 j P (t0 0) = PX0 Thus, for t0 t t , ^ (tj0) = 0. The estimate for X (t) is ^ (tj0) = xR (t) ; X 1 where xR (t) is the solution to the differential equation (10) with an initial condition of mX (t0 ). The error covariance is j j P (t 0) = (t; t0)P (t0 0) T (t; t0) + Z t t0 (t; )GQGT T (t; )d; where (t; ) is the state transition matrix of the linear system model (12). At time t1, we process the observation Z (t1 ) to obtain j j K1 = P (t1 0) H1T H1 P (t1 0) H1T + R 0 1 ^ (t1 j1) = ^ (t1j0) + K1 (t1) 0 H1 ^ (t1 j0) = K1 (t1); where we have used the fact that ^ (t1 j0) = 0. j P (t1 1) = (I 0 K H ) P (t j0) : 1 1 1 ^ (t j1) is obtained as The estimate X 1 ^ (t j1) = xR (t ) + K (t ) = X ^ (t j0) + K Z (t ) 0 h X ^ (t j0) ; t X 1 1 1 1 1 1 1 1 1 : ^ (tj1) for t t t , we could continue to use the reference traNow, to obtain X 1 2 jectory that we used for the interval [t0; t1]; however, we now have a (hopefully) better ^ (t j1). Thus, to get the reference trajectory for estimate of the state at time t1, namely X 1 ^ (t j1). If our t1 t t2, we find the solution of (10) with initial condition xR (t1 ) = X 1 7 dynamics and observation models for the linearized error system were exact, we would expect E [ (t)] = 0; making the assumption that the mean of the error is small, we get ^ (tj1) = xR (t) ; X where xR (t) is the solution of the differential equation (10) with an initial condition of ^ (t j1). The error covariance for this estimate is xR (t1) = X 1 j j P (t 1) = (t; t1)P (t1 1) T (t; t1) + Z t t1 (t; )GQGT T (t; )d: At t2, we process the observation Z (t2) in the same way as we processed the observation Z (t1 ). To summarize, the Extended Kalman Filter equations are the following: ^ (tjk ) is the solution of (10) with X ^ (t jk ) as the initial Predictor Step: For tk t tk+1 , X k condition. The corresponding error covariance update is j j T P (t k ) = (t; tk )P (tk k ) (t; tk ) + Z t tk (t; )GQGT T (t; )d; where (t; ) is the state transition matrix obtained from A(t) = @ f (x; t) @x ^ (tjk ) x=X : Filter Step At tk+1 , the filter step is the following: Hk+1 = @ h (x; tk+1 ) j @x ^ (tk +1 jk ) x=X h 0 1 j Kk+1 = P (tk+1 k ) HkT+1 Hk+1 P (tk+1 k ) HkT+1 + R i ^ (t ^ ^ X k +1 jk + 1) = X (tk +1 jk ) + Kk +1 Z (tk +1 ) 0 h X (tk +1 jk ) j (tk+1 k + 1) = (I 0 Kk 8 +1 Hk +1 ) P j (tk+1 k ) :