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) Nonlinear Applications The principal uses of linear ®ltering theory are for solving nonlinear problems Harold W Sorenson, in a private conversation 5.1 5.1.1 CHAPTER FOCUS Nonlinear Estimation Problems Linear estimators for discrete and continuous systems were derived in Chapter The combination of functional linearity, quadratic performance criteria, and Gaussian 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 D…k À `† stand for the Kronecker delta function and the symbols d…t À 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 Consequently, 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 Nonlinear Plant and Measurement Models Model Plant Measurement Plant noise Measurement noise Continuous Time _ x ˆ f …x; t† ‡ w …t† z…t† ˆ h…x…t†; t† ‡ v …t† Discrete Time xk ˆ f …xkÀ1 ; k À 1† ‡ wkÀ1 zk ˆ h…xk ; k† ‡ vk Ehw…t†i ˆ Ehw …t†w T …s†i ˆ d…t À s†Q…t† Ehwk i ˆ Ehwk wiT i ˆ D…k À i†Qk Ehv …t†i ˆ Ehv …t†v T …s†i ˆ d…t À s†R…t† Ehvk i ˆ Ehvk viT i ˆ D…k À i†Rk 5.4 171 LINEARIZATION ABOUT A NOMINAL TRAJECTORY TABLE 5.2 Dimensions of Vectors and Matrices in Nonlinear Model Symbol Dimensions Symbol Dimensions x; f ; w Q D; d nÂ1 nÂn Scalars z; h; v R `Â1 `Â` 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 x…t† to satisfy a speci®ed performance criterion as given in Chapter 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 5.4.1 LINEARIZATION ABOUT A NOMINAL TRAJECTORY 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 fxk jk ˆ 0; 1; 2; 3; g for discrete-time systems and a vector-valued function x…t†; 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 fxnom g k obtained as a solution of the equation xnom ˆ f …xnom ; k À 1† k 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 twobody 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 perturbations 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 xkÀ1 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 ; k dzk ˆ zk À h…xnom ; k†; k so that the Taylor series expansion of f …x; k À 1† with respect to x at x ˆ xnom is kÀ1 xk ˆ f …xkÀ1 ; k À 1† @f …x; k À 1† ˆ f …xnom ; k À 1† ‡ kÀ1 nom dxkÀ1 @x xˆx …5:2† kÀ1 ‡ higher order terms @f …x; k À 1† nom ‰1Š FkÀ1 ˆ nom @x xˆxkÀ1 @f1 @f1 @f1 @f1 ÁÁÁ @x @x @x @xn 7 6 @f2 @f2 @f2 @f2 7 6 @x @x @x Á Á Á @x n 7 ˆ @f3 @f3 @f3 Á Á Á @f3 ... involved The trajectory is a vector-valued sequence fxk jk ˆ 0; 1; 2; 3; g for discrete-time systems and a vector-valued function x…t†; t, for continuous-time systems The term ``nominal'''' in... 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... 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