v are uncorrelated, zero-mean, white

Transcription

1 6.0 EXENDED KALMAN FILER 6.1 Introduction One of the underlying assumptions of the Kalman filter is that it is designed to estimate the states of a linear system based on measurements that are a linear function of the states. Unfortunately, in many of the situations where we would lie to use a Kalman filter, we have a non-linear system model and/or a non-linear measurement equation. Specifically, the system model is a non-linear function of the states and/or the measurements are non-linear functions of the states. Usually, the non-linearities don t etend to the system disturbances and measurement noise. Because of the attractiveness of the Kalman filter, designers have developed a set of mathematics to etend Kalman filter theory to situations where the system model and/or measurement model are non-linear functions of state. he resultant Kalman filter is referred to as the etended Kalman filter. As we will see, the etended Kalman filter uses the non-linear system ˆ 1, and the non-linear model for computing the predicted state estimate, measurement model to form the predicted measurement, y ˆ 1. he smoothed state then taes on the standard Kalman filter form similar to Equation Etended Kalman Filter Development 6..1 Problem Definition We are interested in designing a Kalman filter for a system defined by the state equation 1 f G w (6-1) and the measurement equation y 1 h 1 v 1. (6-) In Equations 6-1 and 6- w and 1 v are uncorrelated, zero-mean, white random processes. hey are also uncorrelated with the initial state, G is a nown matri and f and 1 h are nown, vector, nonlinear functions of the state. hey are of the forms n n f1 f1 1,, f f,, f f,, 0. 1 f (6-3) and n n 1 n M. C. Budge - merv@thebudges.com 89

2 n n h1 1 h1 1 1, 1, 1 h 1 h 1 1, 1, 1 h 1. (6-4) h 1 h 1, 1, 1 m m 1 n 6.. Filter Development We begin by separating and y 0 and 1 y into two parts as (6-5) 1 1 y 1 y. (6-6) 0 0 and y 0 1 are termed the nominal values of and 1 respectively, while and y 1 are termed perturbations. about Net we epand f and 0 and 0 1 as 0 y, h into a aylor series epansion f f f 0 H.O.. 0 (6-7) and h h h 0 H.O.. 0. (6-8) 0 We net drop the high order terms (H.O..), recognize that and write 0 f f F (6-9) and 0 h h H. (6-10) In Equations 6-9 and 6-10, F and H are defined by 0 F f1 1 f1 f1 n f f f f 1 n 0 fn 1 fn fn n (6-11) M. C. Budge - merv@thebudges.com 90

3 and H h1 1 h1 h1 n h h h h 1 n 0 hm 1 hm hm n where the notation gi j is interpreted as n 1 10 gi 1,,, g i j j n n0 (6-1). (6-13) We want to digress to discuss the notation we have used. One of the misconceptions associated with Equations 6-5 and 6-6 is that we have some nowledge of the nominal state and measurement, y, and can thus insure that the perturbations, 0 and 0 1 and y 1, are small. his, in turn, allows us to claim that the H.O.. of the aylor series epansions of h 1 are small and can thus be ignored. In fact, we have f and no nowledge of the nominal states or measurements (if we did, we would not need to build a Kalman filter!). We introduce the notation as a mathematical tool that facilitates our derivation. We temporarily assume we now the nominal state and measurement and that they are close to the actual state and measurement. his allows us to claim that the perturbations are small and thus that the first order approimations to the aylor series epansions are valid. We cannot determine the validity of our assumption and its ramifications beforehand. Because of this we cannot always be sure that an etended Kalman filter that we design will wor well. While this may be discouraging we should tae solace from the fact that designers have been designing etended Kalman filters for the past forty or so years and that most of these filters wor very well. Returning to our development, we use Equations 6-5, 6-6, 6-9 and 6-10 to rewrite the system and measurement equations as f F G w (6-14) and 0 0 y 1 y 1 h 1 H 1 1 v 1. (6-15) We net separate Equations 6-14 and 6-15 into two sets of equations. One of these we term the nominal equations and the other we call the perturbation equations. he nominal equations are f (6-16) M. C. Budge - merv@thebudges.com 91

4 and y h, (6-17) and the perturbation equations are 1 F G w (6-18) and y H v. (6-19) For now we set the nominal equations aside and build a Kalman filter for the perturbation model. his results in the equations ˆ yˆ 1 ˆ F, (6-0) 1 1ˆ 1 H, (6-1) ˆ ˆ K y yˆ, (6-) K P H H P H R, (6-3) 1 P F P F G Q G and (6-4) P I K H P. (6-5) With Equations 6-0 through 6-5 we have a means of estimating the perturbation part of the state. While this is an interesting result, it is not what we want. We want ˆ ˆ, the estimate of., the estimate of ; not 1 ˆ as ˆ Let us consider ˆ 0 ˆ. Based on our results from earlier, we can write. (6-6) In this equation estimate of 0 is the nominal state discussed earlier and ˆ is the that we just derived. Equation 6-6 tells us that to form an estimate of the state we add an estimate of the perturbed state to the nominal state (we conveniently ignore the fact that we don t really now the nominal state). If we consider the mean-squared error between and ˆ we get M. C. Budge - merv@thebudges.com 9

5 ˆ ˆ e E. (6-7) E ˆ ˆ e Recall that the Kalman filter results in a e e ˆ that minimizes e. Since we tend to jump to the conclusion that we have also minimized his is not a totally correct conclusion. We can say that we have minimized under the constraint that ˆ is given by Equation 6-6 and ˆ e. e is defined by Equation 6-0 through 6-5. If we were to remove these constraints we could conceivably find an ˆ such that e. However, we don t have a general means of finding min e min ˆ that truly minimizes e. herefore, for want of anything better, we will adopt Equation 6-6. As indicated earlier, the quality of this decision can only be tested by constructing the etended Kalman filter and testing it. ˆ If we combine Equation 6-6 with our previous results we get ˆ 1 0 ˆ 1 y 1 yˆ 1 f F K and if we epand f ˆ into a aylor series about we get (6-8) f ˆ f F ˆ H.O... (6-9) Dropping the higher order terms (H.O..) and recognizing ˆ 0 ˆ we get ˆ 0 ˆ as f f F. (6-30) Using this we can replace the first two terms in the last part of Equation 6-8 to yield ˆ 1 f ˆ K 1 y 1 yˆ 1. (6-31) We net turn our attention to the last term in Equation If we use Equation 6-6 and write the predicted measurement as yˆ yˆ 1 y, (6-3) we can rewrite the last term as 1 ˆ 1 1 ˆ 1 y y y y (6-33) M. C. Budge - merv@thebudges.com 93

6 and the state estimation equation as ˆ 1 f ˆ K 1 y 1 yˆ 1. (6-34) We note that we have manipulated the state estimation equation into the form that we want. hat is, we have an equation for the state estimate at stage 1 in terms of the state estimate at stage, the measurement at stage 1 and the predicted measurement at stage 1. We now need to perform some more mathematical manipulations to complete our formulation of the etended Kalman filter. We start by eamining the function value of 1 given only ˆ f ˆ. If we wanted to predict the we would use the original state equation of Equation 6-1. However, this equation contains the system disturbance w, which we don t now. Using the logic from Chapter 5 we can say that since we don t now E w, which w, our best choice for its replacement would be is zero. hus we are left with the equation ˆ 1 ˆ f (6-35) for the predicted state at stage 1 given the state estimate at stage. As we did in Chapter 5, we can characterize the error between the predicted and actual state as 1 1 ˆ 1 ˆ F G w. (6-36) With this we can use the results of Chapter 4 to write the covariance of 1 as 1 P F P F G Q G. (6-37) We net consider the predicted measurement, y ˆ 1. Following the logic from Chapter 5, we can relate the predicted measurement to the predicted state through the measurement model of Equation 6-. However, we don t v 1. As in Chapter 5, for want of anything better we will use now E yˆ v 1 0 instead. hus we get 1 ˆ 1 h. (6-38) H he final topic we need to consider concerns the matrices 1. Recall that these matrices are given by F and M. C. Budge - merv@thebudges.com 94

7 f F (6-39) and h H 1. (6-40) o compute 0 1 F and H 1 we need 0 and 0 1 ˆ in place of 0 and ˆ 1 now. In lieu of these, we use 0 1, which we don t in place of. We can offer no rationale for these substitutions ecept to state that they are the only ones we have Summary We now summarize the results of this section into the following presentation of the etended Kalman filter. Given the state and measurement models of Equations 6-1 and 6-, along with the previously indicated properties of 1 0 an etended Kalman filter that can be used to w, v and estimate the state of the system is given by the equations ˆ ˆ K y yˆ, (6-41) ˆ yˆ 1 ˆ f, (6-4) 1 ˆ 1 h, (6-43) K P H H P H R, (6-44) 1 P F P F G Q G, (6-45) P I K H P, (6-46) f F and (6-47) ˆ 1 h H 1. (6-49) 1 ˆ 1 Equations 6-41 through 6-49 constitute the tracing form of the etended Kalman filter. he control theoretic form of the etended Kalman filter is defined by the equations M. C. Budge - merv@thebudges.com 95

8 1 ˆ f ˆ K y h ˆ, (6-50) K F P H H P H R, (6-51) 1 P F K H P F G Q G, (6-5) f F and (6-53) ˆ h H. (6-54) ˆ he derivation of the control theoretic form of the etended Kalman filter is left to the reader. Bloc diagrams of the tracing and control theoretic forms of the etended Kalman filter are contained in Figures 6-1 and 6-. It will be noted that their structure is very similar to the normal Kalman filter bloc diagrams of Figures 5-7 and 5-8. he difference is that the matrices H are replaced by their non-linear equivalents, 1 F and f and h M. C. Budge - merv@thebudges.com 96

9 6-1. racing Form of the Etended Kalman Filter M. C. Budge - merv@thebudges.com 97

10 Figure 6-. Control heoretic Form of the Etended Kalman Filter 6..4 Properties of the Etended Kalman Filter We now tae some time to discuss the properties of the etended Kalman filter. We start by noting that the etended Kalman filter is intuitively appealing. It has the basic structure of the normal Kalman filter but also incorporates the non-linear functions of the system and measurement models. Since it incorporates these non-linear functions, the etended Kalman filter is a non-linear filter. hat is, the state prediction equation and the predicted measurement equations are non-linear. Because of this the state estimate equation is also a non-linear function of the previous state estimate. Although not as obvious, the elements of F and H are also nonlinear functions of the state estimates. his means that the Kalman gain and covariance matrices are also non-linear functions of the state estimate. It should be obvious to the reader that the development of the etended Kalman filter is very heuristic. hat is, while the various steps are intuitively appealing, as are the resultant equations, they are not always mathematically rigorous. For eample, we have no rigorous mathematical (or physical) justification for ignoring the higher order terms in the various aylor series epansions. Nor can we mathematically or physically justify replacing M. C. Budge - merv@thebudges.com 98

11 by ˆ and 0 1 by ˆ 1 when evaluating F and 1 H. Because of this we must tae care in implementing etended Kalman filters. It is quite possible that we could design an unstable etended Kalman filter. In other instances, the etended Kalman filter may be stable but do a poor job of estimating the system states. Having made these statements, we also note that designers have been very successful at constructing etended Kalman filters that wor very well. hey are stable, they produce accurate state estimates, their covariance matrices are representative of the actual errors between the estimate and actual states, and they have good transient response. However, this is not accomplished by blind application of the etended Kalman filter equations. It results from very careful selection of the system and measurement models and tuning of the filter via the Q matri. 6.3 Applications As the reader has probably deduced by now, the main effort in designing Kalman filters is devoted to deriving system and measurement models, and in 1 ˆ 0 P 0. Once these are found the Kalman determining R, Q, and filter will tae on the established forms of Equations 6-41 through 6-49 or Equations 6-50 through 6-54 depending upon whether we are building a tracing or control theoretic type of Kalman filter. Because of this, we will devote the rest of this chapter to developing system and measurement models for some eamples. As mentioned before, the system model is usually derived from an understanding of the physics of the process or system, and the measurement model is based on our understanding of how the measurements tae place Eample 1: General arget racer in Cartesian Coordinates For our first eample we consider the system and measurement models that might be used to design a Kalman filter for a broad range of target tracing problems. We cast the problem in a Cartesian coordinate system (yz). We will limit ourselves to two dimensions ( and z) for ease of notation. he reader can etend the results to three dimensions. Since we are considering a general target tracer we have no detailed nowledge of the system since we don t now the target type or its motion characteristics. he target could be an aircraft, a ship, a ballistic missile, an artillery round or a bicycle. In this case, our best choice for a system model would be to use the aylor series epansion discussed in Chapter. Specifically, if we use position and velocity as our states we would have 1 1 z z z z 1 z z z z 1. (6-55) M. C. Budge - merv@thebudges.com 99

12 Recall that since we don t now the second and higher order derivatives we lump them into the model disturbance terms, w. With this we have z 1 w w z z z w z z w z If we write this in matri form we get w 1 where. (6-56) F G (6-57) F, G I, w1 w w w w. w3 wz w w 4 z 1 and 3 z z 4 We have transitioned to representing the states and disturbances as random processes as acnowledgment of the fact that we are now treating the higher order terms of the aylor series epansions as random processes to be consistent with the requirements of the Kalman filter formulation. he model described by the above equations is now as a constant 1 z 1 z. In velocity model because of the fact that and some instances it may be more appropriate to use a constant acceleration model. A constant acceleration model would be described by the Equation 6-57 with the various matrices replaced by the following M. C. Budge - merv@thebudges.com 100

13 F, G I, w1 w w w w w 3 w. w 4 wz w w 5 z w 6 wz 1 3, and 4 z z 5 6 z he derivation of the above matrices is a simple eercise that is left to the reader. If we consider a sensor that is remote from the target, the measurements made by this sensor will not be in the Cartesian coordinate system. Instead, they are most often in a polar coordinate system. hese types of sensors would include radars, lasers, sonar and the lie. he measurements and their relation to the Cartesian coordinate system are illustrated in Figure 6-3. In this figure, we assume that the Cartesian coordinate system is centered on the sensor. his is a standard convention. As illustrated, the standard measurements that a sensor maes are range and angle. In some cases the sensor can also measure range-rate via Doppler frequency. Figure 6-3. Coordinate System for arget racing Model M. C. Budge - merv@thebudges.com 101

14 Since our system model is in the Cartesian coordinate system we need to cast our measurement model in this coordinate system. From the geometry of Figure 6-3 the appropriate equations are z r v r (6-58) and tan z. (6-59) 1 v If we include range rate as a measurement, the appropriate equation would be r z z z v r (6-60) he reader can substitute the elements of the appropriate vector (for the constant velocity or constant accretion system model) into the above equations to epress them in terms of their elements. As an eample of one of the vector forms, the measurement vector for the case of range and angle measurement with the constant velocity system model is 1 3 v1 y h v 1 (6-61) tan 3 1 v where v v and v v 1 r. Recall that, for purposes of building the Kalman filter we need to H 1 matri. For the tracing filter formulation this equation generate the is as defined in Equation For the specific case associated with Equation H 1 becomes 6-61 the h1 1 h1 h1 3 h1 4 H 1 h 1 h h 3 h (6-6) 4 where the elements of the matri are defined by ˆ 1 1 h ˆ ˆ ˆ ˆ 3 1 h ˆ ˆ ˆ , (6-63), (6-64) 1 tan ˆ 3 1 h 1, (6-65) ˆ ˆ 1 1 ˆ M. C. Budge - merv@thebudges.com 10

15 1 tan ˆ1 1 h 3 ˆ ˆ 3 1 ˆ , (6-66) and the rest of the terms are zero. With the above, we can rewrite H 1 as H ˆ ˆ ˆ ˆ ˆ 1 1 ˆ ˆ ˆ (6-67) ˆ ˆ ˆ ˆ he derivation of the other forms of for the reader. h and 1 H are left as eercises With the above we are in a position to proceed to the net phase of designing the Kalman filter. We will consider this through a specific eample Eample : Application to a Ballistic Missile racing Problem Purely Ballistic rajectory We now want to apply some of the developments of the previous subsection to the development of a Kalman filter that can be used to estimate the states of a ballistic missile based on measurements of range and angle. We will start with the unrealistic, but instructive, case where the missile is flying a purely ballistic trajectory across a non-rotating, flat earth. Consistent with the developments in subsection 6.3.1, we will assume that the trajectory lies in the -z plane to simplify the system model and measurement equations. In this coordinate system, the origin is located at the radar, which generates the range and angle measurements, and includes the Kalman filter. he continuous-time equations of motion for this ideal trajectory are 0 0 0, , 0 t z t g z z z 0 (6-68) where g is the acceleration of gravity (9.8 m/s ),,0 0 is the launch position of the missile and z is the launch velocity. 0, 0 Figure 6-4 contains a plot of the missile trajectory for the case where Km and 0, z0 775,63 m/s. In the figure, the solid red curve denotes the portion of the trajectory over which the radar (and Kalman filter) tracs the target. rac is initiated when the missile is at a range of about 43 Km and an angle of about 7º. We will start with the system model of Equation We deduce that this model is representative in the coordinate because we now that our real system has a constant velocity in this coordinate (see Equation 6-68) M. C. Budge - merv@thebudges.com 103

16 However, the model is not good in the z coordinate because we now that the real system has a constant acceleration, and not a constant velocity, in this coordinate. We will try to account for the inaccuracy of the model through the Q matri. Figure 6-4 Ballistic arget rajectory Since we are measuring range and angle the appropriate measurement model is specified in Equations 6-58 and 6-59; and Equation 6-61 if we use the H 1 matri is given by state assignment of Equation he appropriate Equation In general, the range and angle measurement errors, vr 1 and v 1, will depend on signal-to-noise ratio (SNR), the radar compressed pulse width, p, and the radar antenna beam width, B. here will also be a lower limit on these errors that depends upon various radar design parameters. he measurement error model we will assume is 1 1 r p SNR 1 1 B SNR 0 0. (6-69) he terms SNR and SNR were obtained from Solni s radar tet (with r 1 p see Solni). he B 1 0 factor limits the minimum errors to 1/0 th of the pulse width and beam width. We will assume a pulse width of 1 s (150 m) and a beam width of ( rad). p he SNR would be obtained from the radar range equation (see Solni 4 reference) and is inversely proportional to R, where R is the range from the B M. C. Budge - merv@thebudges.com 104

17 radar to the target. For this eample we will assume that the radar range equation results in a SNR given by 0 10 SNR. (6-70) 4 R his provides an SNR of about 15 db at the start of tracing. o implement the Kalman filter we need to specify and R 1. he system model matrices, ˆ 0, P 0, Q F and specified. We will also need to discuss the computation of H 1. G have been We now that tracing starts at a missile range of approimately 43 Km and an angle of about 7º. From this we can compute the initial position as ˆ yˆ 0 ˆ cos 7 0 ˆ sin 7. (6-71) We assume that we don t now the initial velocity but we assume it is in the order of 1000 m/s. With this we let 4 ˆ 0 ˆ (6-7) yˆ 0 ˆ We reflect our uncertainty in these initial estimates by using P 0. (6-73) his selection of the P 0 matri says that we now the position to within about 00 m and the velocity to within about 500 m/s. hese are guesses. Although we now we shouldn t, we will start with a Q of all zeros. his says that we believe that our system model is perfect, which we now it is not. We will assume that the range and angle measurements are independent, which is a reasonable assumption. With this we get r 0 R 1 (6-74) 0 where the range and angle variances are as defined in Equations We note that the measurement covariance matri is not constant since the variances R 1 depend upon target range, which varies with time. his means that must be computed inside of the Kalman filter loop. Furthermore, to compute M. C. Budge - merv@thebudges.com 105

18 R 1 we will need to use a prediction of target range based on the predicted state estimate. Since H 1 depends upon the predicted states, it must also be computed inside of the Kalman loop. It should be initialized to H (6-75) or a X4 matri of zeros outside of the Kalman filter loop. he MALAB script entitled MissileKfilt1.m contained in the Program Files folder contains a script that implements the Kalman filter. he script is well commented and should be easy to follow. he script reads a file entitled Missile1.tt 1 that contains the true state. he data in the file is stored at intervals of 0.1 seconds. We must down sample the data because we want the update period of the Kalman filter to be 0. s. he results of a simulation run are shown in Figures 6-5 through 6-9. Figure 6-5 contains a plot of actual and estimated y position vs. position. his plot seems to indicate that the filter is woring well. However, the scale maes the errors difficult to see. More specific information on the errors is provided in Figures 6-6 and 6-7. Figure 6-6 contains plots of and y position errors between the estimates and actual values (top plot) and the measured and actual values (bottom plot). he measured and y positions were obtained from y r cos meas meas meas r sin meas meas meas. (6-76) It will be noted that the errors between the estimated and actual positions is large, and much larger than the measured values. his tells us that the filter is not woring very well. In fact, direct use of the measurements provides better position estimates! Figure 6-7 shows that the errors between the estimated and actual velocities are also large. here is no curve of errors between measured and estimated velocities because the measured velocities were not computed. Figure 6-8 contains plots of what the Kalman filter thins the rms position and velocity errors are. hese curves were obtained by plotting the square root of the diagonal terms of the covariance matri ( P ). As we have previously discussed, the rms errors indicated in these curves should be in agreement with the actual errors of Figures 6-6 and 6-7. As can be seen, they are not. he Kalman filter thins it is doing a good job of estimating the positions and velocities and is thus assigning small rms errors to them. his is a clear indication of the fact that we have not provided the Kalman filter with 1 he file Missile.tt is a MALAB.mat file. he.tt etension is used because many programs will not allow down load of files with a.mat etension because they often contain harmful viruses M. C. Budge - merv@thebudges.com 106

19 an adequate system model; and we have lied to the filter by telling it that the model is good my setting the Q matri to zero. Figure 6-5 Plot of Estimated and Actual Positions Figure 6-6 Plot of Position Errors M. C. Budge - merv@thebudges.com 107

20 Figure 6-7 Plot of Velocity Errors Figure 6-8 Plot of RMS Position and Velocity Error from Covariance M. C. Budge - merv@thebudges.com 108

21 he curves of Figure 6-9 contain plots of the eight Kalman gains. he top two curves are the gains that are applied to the range measurement error (difference between the actual range measurement and the estimated range measurement) to update the position estimates. he second two curves are the gains that are applied to the angle measurement error to update the position estimates. he third two curves are the gains that are applied to the range measurement error to update the position estimates and the fourth two curves are the gains that are applied to the angle measurement error to update the velocity estimates. It will be noted that all of the gains tend toward zero. his is epected and means that the Kalman filter is de-emphasizing the measurements and emphasizing the previous state estimates. his is the reason the position and velocity errors increase with time: the estimates are poor but the filter is not maing use of the measurements to try and improve the estimates. he reason we epect the gains to go to zero is that we have told the Kalman filter that the system model is good and that it can rely on the system model more than on the data, once the filter has been initialized by the first few measurements. Figure 6-9 Plot of Kalman Gains M. C. Budge - merv@thebudges.com 109

22 As we learned with the Humvee tracing problem, we can probably improve the performance of the missile tracing Kalman filter by choosing a non-zero Q matri. his is left as a homewor eercise. Another alternative is to include acceleration as a state and attempt to estimate it. his will also be considered as a homewor problem. We will note that this approach will be of considerable help when the trajectory is not purely ballistic but includes atmospheric drag. Still another way of improving the performance of the Kalman filter for the ballistic missile case is to recognize that the trajectory is influenced by gravity and include gravity in the system model by considering it as a nown input. his is discussed in Chapter 8. It should be noted that there are many other ways to improve the performance of the Kalman filter. As eamples: For the trajectory with atmospheric drag, we could include the drag term in the system model. For both trajectories we may be able to improve performance by including the measurement of range-rate. We could use the combined continuous-time, discrete-time Kalman filter discussed in Chapter 8. In all cases, a critical factor affecting the performance of the filter is a proper selection of the Q matri M. C. Budge - merv@thebudges.com 110

23 PROBLEMS 1. Derive the control theoretic form of the etended Kalman filter presented in Equations 6-50 through Derive the Cartesian coordinate, constant acceleration system model defined by the matricies and vectors above Figure Show that 0 is uncorrelated with w and 4. Derive Equation v. 5. Derive the H 1 matri given by Equations 6-6 through Derive the H 1 matri for the si-state, Cartesian-coordinate system model. 7. Etend the measurement model and H 1 matri of Section 6..1 to include Doppler M. C. Budge - merv@thebudges.com 111