Lọc Kalman - lý thuyết và thực hành bằng cách sử dụng MATLAB (P5)

Kalman Filtering: Theory and Practice Using MATLAB, Second Edition, Mohinder S. Grewal, Angus P. Andrews Copyright # 2001 John Wiley & Sons, Inc. ISBNs: 0-471-39254-5 (Hardback); 0-471-26638-8 (Electronic) 5 Nonlinear Applications The principal uses of linear ®ltering theory are for solving nonlinear problems. Harold W. Sorenson, in a private conversation 5.1 CHAPTER FOCUS 5.1.1 Nonlinear Estimation Problems Linear estimators for discrete and continuous systems were derived in Chapter 4. The combination of functional linearity, quadratic performance criteria, and Gaus-sian statistics is essential to this development. The resulting optimal estimators are simple in form and powerful in effect. Many dynamic systems and sensors are not absolutely linear, but they are not far from it. Following the considerable success enjoyed by linear estimation methods on linear problems, extensions of these methods were applied to such nonlinear problems. In this chapter, we investigate the model extensions and approximation methods used for applying the methodology of Kalman ®ltering to these ``slightly nonlinear`` problems. More formal derivations of these nonlinear ®lters and predictors can be found in references [1, 21, 23, 30, 36, 75, 112]. 5.1.2 Main Points to Be Covered Many estimation problems that are of practical interest are nonlinear but ``smooth.`` That is, the functional dependences of the measurement or state dynamics on the system state are nonlinear, but approximately linear for small perturbations in the values of the state variables. Methods of linear estimation theory can be applied to such nonlinear problems by linear approximation of the effects of small perturbations in the state of the nonlinear system from a ``nominal`` value. 169 170 NONLINEAR APPLICATIONS For some problems, the nominal values of the state variables are fairly well known beforehand. These include guidance and control applications for which operational performance depends on staying close to an optimal trajectory. For these applications, the estimation problem can often be effectively linearized about the nominal trajectory and the Kalman gains can be precomputed to relieve the real-time computational burden. The nominal trajectory can also be de®ned ``on the ¯y`` as the current best estimate of the actual trajectory. This approach is called extended Kalman ®ltering. It has the advantage that the perturbations include only the state estimation errors, which are generally smaller than the perturbations from any prede®ned nominal trajectory and therefore better conditioned for linear approximation. The major disadvantage of extended Kalman ®ltering is the added real-time computational cost of linearization about an unpredictable trajectory, for which the Kalman gains cannot be computed beforehand. Extensions of the linear model to include quadratic terms yield optimal ®lters of greater applicability but increased computational complexity. 5.2 PROBLEM STATEMENT Suppose that a continuous or discrete stochastic system can be represented by nonlinear plant and measurement models as shown in Table 5.1, with dimensions of the vector and matrix quantities as shown in Table 5.2 and where the symbols Dk ` stand for the Kronecker delta function and the symbols dt s stand for the Dirac delta function (actually, a generalized function). The function f is a continuously differentiable function of the state vector x, and the function h is a continuously differentiable function of the state vector. Whereas af®ne (i.e., linear and additive) transformations of Gaussian RVs have Gaussian distributions, the same is not always true in the nonlinear case. Conse-quently, it is not necessary that w and v be Gaussian. They may be included as arguments of the nonlinear functions f and h, respectively. However, the initial value TABLE 5.1 Model Plant Nonlinear Plant and Measurement Models Continuous Time x fx;twt Discrete Time xk fxk1;k 1wk1 Measurement Plant noise Measurement noise zt hxt;tvt Ewt 0 EwtwTs dt sQt Evt 0 EvtvTs dt sRt zk hxk;kvk Ewk 0 Ewkwi Dk iQk Evk 0 Evkvi Dk iRk 5.4 LINEARIZATION ABOUT A NOMINAL TRAJECTORY 171 TABLE 5.2 Dimensions of Vectors and Matrices in Nonlinear Model Symbol Dimensions Symbol Dimensions x;f;w n 1 Q n n z;h;v ` 1 R ` ` D;d Scalars x0 may be assumed to be a Gaussian random variate with known mean and known n n covariance matrix P0. The objective is to estimate xk or xt to satisfy a speci®ed performance criterion as given in Chapter 4. 5.3 LINEARIZATION METHODS Applying linearization techniques to get simple approximate solutions to nonlinear estimation problems requires that f and h be twice-continuously differentiable [112, 133]. 5.4 LINEARIZATION ABOUT A NOMINAL TRAJECTORY 5.4.1 Nominal Trajectory A trajectory is a particular solution of a stochastic system, that is, with a particular instantiation of the random variates involved. The trajectory is a vector-valued sequence xkk 0;1;2;3;... for discrete-time systems and a vector-valued function xt;0 t, for continuous-time systems. The term ``nominal`` in this case refers to that trajectory obtained when the random variates assume their expected values. For example, the sequence xnom obtained as a solution of the equation xnom fxnom;k 1 5:1 with zero process noise and with the mean xnom as the initial condition would be a nominal trajectory for a discrete-time system. 5.4.2 Perturbations about a Nominal Trajectory The word ``perturbation`` has been used by astronomers to describe a minor change in the trajectory of a planet (or any free-falling body) due to secondary forces, such as those produced by other gravitational bodies. Astronomers learned long ago that the actual trajectory can be accurately modeled as the sum of the solution of the two-body problem (which is available in closed form) and a linear dynamic model for the 172 NONLINEAR APPLICATIONS perturbations due to the secondary forces. This technique also works well for many other nonlinear problems, including the problem at hand. In this case, the perturba-tions are due to the presence of random process noise and errors in the assumed initial conditions. If the function f in the previous example is continuous, then the state vector xk at any instant on the trajectory will vary smoothly with small perturbations of the state vector xk1 at the previous instant. These perturbations are the result of ``off-nominal`` (i.e., off-mean) values of the random variates involved. These random variates include the initial value of the state vector (x0), the process noise (wk), and (in the case of the estimated trajectory) the measurement noise (vk). If f is continuously differentiable in®nitely often, then the in¯uence of the perturbations on the trajectory can be represented by a Taylor series expansion about the nominal trajectory. The likely magnitudes of the perturbations are determined by the variances of the variates involved. If these perturbations are suf®ciently small relative to the higher order coef®cients of the expansion, then one can obtain a good approximation by ignoring terms beyond some order. (However, one must usually evaluate the magnitudes of the higher order coef®cients before making such an assumption.) Let the symbol d denote perturbations from the nominal, dxk xk xnom; dzk zk hxnom;k; so that the Taylor series expansion of fx;k 1 with respect to x at x xnom is xk fxk1;k 1 5:2 fxnom;k 1 @fx;k 1 dxk1 xxk1 higher order terms 5:3 nom @fx;k 1 k k1 xxk1 higher order terms; 5:4 or dxk xk xnom 5:5 @fx;k 1 k1 xxnom higher order terms. 5:6 5.4 LINEARIZATION ABOUT A NOMINAL TRAJECTORY 173 If the higher order terms in dx can be neglected, then dxk F1 1dxk1 wk1; 5:7 where the ®rst-order approximation coef®cients are given by 1 @fx;k 1 k1 @x nom k1 @f1 @f1 @f1 6@x1 @x2 @x3 6 @f2 @f2 @f2 6@x1 @x2 @x3 6 @f3 @f3 @f3 6@x1 @x2 @x3 6 . . . ... 4 @fn @fn @fn @x1 @x2 @x3 an n n constant matrix. 5:8 @f 3 @xn 7 @f2 7 @xn 7 @f3 7 ; 5:9 @xn 7 . 7 @fn 5 @xn xxnom 5.4.3 Linearization of h about a Nominal Trajectory If h is suf®ciently differentiable, then the measurement can be represented by a Taylor series: hxk;k hxnom;k @hx;k dxk higher order terms, 5:10 xxnom or dzk @hx;k dxk higher order terms. 5:11 xxnom If the higher-order terms in this expansion can be ignored, then one can represent the perturbation in zk as dzk H1dxk; 5:12 ... - tailieumienphi.vn