FRAMEWORK FOR IMPLEMENTING AND TESTING NONLINEAR FILTERS

FRAMEWORK FOR IMPLEMENTING AND TESTING NONLINEAR FILTERS Miroslav Fl´ıdr, Jindˇrich Dun´ık, Ondˇrej Straka, ˇ acha and Miroslav Simandl ˇ Jaroslav Sv´ Department of Cybernetics & Research Centre Data - Algorithms - Decision Making University of West Bohemia in Pilsen Univerzitn´ı 8, 30614 Plzeˇn, Czech Republic e-mail: [email protected], [email protected]

Abstract: The aim of this paper is to present a software framework facilitating implementation, testing and use of various nonlinear estimation methods. This framework is designed to offer an easy to use tool for state estimation of discrete time dynamic stochastic systems. Besides implementation of various local and global state estimation methods it contains procedures for system design and simulation. Its strength is in the fact that it provides means that help students get acquainted with nonlinear state estimation problem and to be able to test features of various estimation methods. Another considerable advantage of proposed framework is its high modularity and extensibility. The paper briefly describes nonlinear estimation problem and its general solution using the Bayesian approach leading to the Bayesian recursive relations. Then it presents key features of the software framework designed in MATLAB environment that supports straightforward implementation of estimation methods based on the Bayesian approach. The strengths of the framework are demonstrated on implementation of the Divided c difference filter 1st order. Copyright 2006 IFAC Keywords: stochastic systems, nonlinear state and parameter estimation, Bayesian recursive relations, nonlinear filters, software framework.

1. INTRODUCTION State estimation of discrete-time stochastic dynamic systems plays crucial role in many areas such as digital measurement processing, adaptive and optimal control problems. It constitutes an essential part of any decision-making process. The main task of the state estimation is to find conditional probability density function (pdf) of the state conditioned by available measurements. The closed form general solution can be found employing the Bayesian recursive relations (BRR). However, the exact solution can be found only for a few special cases ˇ ˇ (Simandl, 1996; Simandl, 1997). Such typical case is

state estimation of a linear Gaussian system where the solution of the BRR leads to the well known Kalman filter. Unfortunately, it is usually necessary to make use of suitable approximations. These approximate methods can be divided with respect to the validity of the resulting estimates into two groups. The first group of methods provides results with validity within some neighborhood of a point estimate only and thus can be called local methods. The second group of methods provides results valid in almost whole state space and so they can be called global methods. The principal idea of the local methods is to approximate the pdf’s with Gaussian pdf’s and thus to ensure analytical solvability of the BRR. This approxi-

mation can be addressed twofold. The first possibility is to substitute the nonlinear functions describing the system with the first few elements of the Taylor expansion (Jazwinski, 1970) or the Stirling’s interpolation formula (N¨orgaard et al., 2000; Ito and Xiong, 2000). The second way to achieve the objective is to solve the problem numerically. In this case the integrals occurring in the BRR are numerically solved using an appropriate quadrature rule e.g GaussHermite quadrature rule or unscented transformation (Julier et al., 2000; Ito and Xiong, 2000). On the other hand the global methods usually don’t approximate the nonlinear state and measurement functions directly. They are essentially based on a suitable approximation of conditional pdf of the state estimate. The global approaches can be divided into three categories: analytical, numerical and simulation. The main advantage of the global methods is global validity of the estimates but drawback of these methods lies in growth of theoretical and computational demands. Prominent among the analytical approaches is the Gaussian sum method (Sorenson and Alspach, 1971; ˇ Simandl and Fl´ıdr, 1997). In this method all the considered pdf’s are generally expected in the form of a mixture of Gaussians. The sought conditional pdf’s are then also represented by such a mixture. It should be noted that this method may be also understood as a result of a multi-point approximation of nonlinear functions in the state and the measurement equations.

insight into the nonlinear estimation problem and to get acquainted with various methods and their applicability in different fields of study. The goal of this paper is to introduce advanced possibilities of a software framework developed for easy estimator design and testing called Nonlinear Filtering ˇ Toolbox (NFT) (Simandl et al., 2004). This software framework is grounded in the basic concepts of the nonlinear estimation problem and its general solution provided by the BRR. The toolbox includes all the tools for building up whole chain of tasks necessary to estimate state of some arbitrary nonlinear and nonGaussian system. The NFT allows to set up a system, simulate its trajectory and to choose and apply a suitable estimator. However, this is not the only role of the toolbox. A not less important role is to facilitate the design and implementation of new methods or modifications of the present ones. The paper is organized as follows. Section 2 deals with the nonlinear estimation problem statement and its general solution represented by the BRR. Subsequently the basic philosophy behind the NFT is described highlighting its main strength, i.e. flexibility and extensibility in Section 3. Afterwards in Section 4 an example of nonlinear estimator implementation is provided in order to show the ease of use of the framework. Finally, Section 5 contains the conclusion of the paper.

The numerical approach to the BRR solution is represented by the point-mass method. The fundamental principle of this method constitutes the substitution of the state space by a grid of a finite number of points which is usually orthogonal and equally spaced. The Bayesian inference is then performed numerically over this grid only instead of the continuous state ˇ space (Sorenson, 1988; Simandl et al., 2002).

The topic of this section is a statement of the nonlinear estimation problem and presentation of its solution by means of the BRR. The following discrete-time nonlinear stochastic system is considered:

Cornerstone of the last mentioned simulation approach to nonlinear estimation, referred as particle filter, is exploitation of Monte Carlo (MC) methods ˇ (Gordon et al., 1993; Simandl and Straka, 2004). This approach approximates the conditional pdf’s with the so called empirical pdf’s consisting of a certain number of random samples and associated weights. Similarly to the point-mass method it produces a grid point approximation of the state space, however, the state space is approximated by randomly spread grid points.

where the multivariate quantities x k ∈ Rn x , z k ∈ Rn z and uk ∈ Rnu represent the state of the system, the measurement and the known input at the time instant k, respectively, w k ∈ Rn x and v k ∈ Rn z are state and measurement white noises. Both noises are mutually independent and they are also independent of the known initial state x 0 pdf p(x 0 ). The vector mappings f : Rn x → Rn x , h : Rn x → Rn z are known .

As it can be seen the nonlinear estimation is dynamically evolving field of study with a broad family of methods. In the course of developing and testing it is always necessary to spend precious time on software implementation of the estimator. Some software package facilitating system specification and appropriate estimator design would greatly lighten procedures of estimator design, testing and comparing its quality with different estimation methods. Such a framework could also provide aid for students to get a better

2. NONLINEAR STATE ESTIMATION

x k+1 = f (x k , uk , wk ), z k = h(x k , v k ),

k = 0, 1, 2, . . . k = 0, 1, 2, . . .

(1) (2)

The system given by the equations (1) and (2) can be alternatively described by the conditional pdf’s p(x k+1 |x k , uk ), k = 0, 1, . . . p(z k |x k ), k = 0, 1, . . . ,

(3) (4)

where the pdf (3) is called the transient pdf of the state and the pdf (4) is called the measurement pdf. The estimate of the state x k is represented by the posterior pdf p(x k |z ` , uk−1 ), where z ` is the sequence of measurements up to time instant l, i.e. z ` ,[z 0 , z 1 , . . . , z ` ].

The estimation problem can be divided according to the relation of k and ` to the following three special cases. • If ` = k the problem is called filtering. • If ` < k the problem is called prediction. • If ` > k the problem is called smoothing. The general solution of the estimation problem can be obtained by the Bayesian approach. The general solution of the filtering problem is given by the Bayesian relation (5): p(x k |z k , uk−1 ) = R

p(x k |z k−1 , uk−1 ) p(z k |x k ) , p(x k |z k−1 , uk−1 ) p(zk |x k )d x k (5)

where the prediction pdf p(x k |z k−1 , uk−1 ) is given by the relation (6): Z p(x k |z k−1 , uk−1 ) = p(x k−1 |z k−1 , uk−2 )× (6) × p(x k |x k−1 , uk−1 )d x k−1 . The multistep prediction pdf p(x ` |z k , u`−1 ) with ` k can be described by the relation (8): p(x k |z ` , uk−1 ) = p(x k |z k , uk−1 )× Z p(x k+1 |z ` , uk ) × p(x k+1 |x k , uk )d x k+1 . p(x k+1 |z k , uk )

(8)

From (8) it is clear that, in order to evaluate the smoothing pdf p(x k |z ` , uk−1 ), it is necessary to firstly evaluate the filtering pdf p(x m |z m , um−1 ) for m = k, k + 1, . . . l. The one-step ahead predictive pdf of the initial state x 0 is given by p(x 0 |z −1 ) = p(x 0 ). The closed-form solution of the relations (5), (7) and (8) is available only for a few special cases. These involve the linear Gaussian systems where the solution of the BRR represents the Kalman filter or the linear systems with noises and prior pdf described by Gaussian mixture where the solution is represented by the Gaussian sum method. These methods can be called exact. For the sake of this paper it is not necessary to continue with presentation of details of various methods that solve the BRR for specific types of systems described by relations (1) and (2). However, this problem formulation gives a sufficient insight in what tools should be provided by a software framework in order to be able to facilitate utilization of nonlinear estimation methods.

3. NONLINEAR FILTERING TOOLBOX This section is devoted to presentation of the Nonlinear filtering toolbox. This toolbox is written in MATˇ LAB. The first version of the toolbox (Simandl and Fl´ıdr, 1997) provided only a set of scripts that just implemented the well known estimation methods and sufficed for simple testing of the methods. However, it quickly became evident that it is necessary to develop an elaborated framework with respect to ease of use and future extensibility. The second version of the ˇ toolbox (Simandl et al., 2004) was built using principles of object oriented design i.e. inheritance, polymorphism and encapsulation. Those principles enable high modularity and reusability of the code making it possible to ease developing new or enhancing currently implemented estimation methods. As it is obvious from previous section, the framework should provide a tool for description of the system, the pdf’s and implementation of the estimators itself. The structure of the toolbox reflects these needs. 3.1 Description of probability density functions A very important NFT feature is the possibility to describe various pdf’s. All the random quantities are represented by an instance of one of the classes that describe some supported type of pdf. All of those classes are descendants of the generic class called general pdf. This class essentially describes the interface that all the child pdf classes should implement. The pdf class interface requires that the child classes implement methods such as setting and reading of the pdf parameters, evaluation of the pdf at an arbitrary point of state space, generating random samples of the pdf, etc. As an example consider the following two-dimensional stochastic quantity x specified by the Gaussian pdf      −1 0.1 0 p(x) = N x : , , (9) 5 0 0.2 Commands required for creating an object that corresponds to the pdf (9) and subsequent generation of one sample look like this px = gpdf([-1;5],diag([.1,.2])); x = sample(px); 3.2 Description of the system Essential part of the NFT forms the classes describing objects corresponding to several abstract system types accompanied with auxiliary classes required for representation of generally nonlinear vector functions f (·) and h(·) occurring in relations (1) and (2), respectively. Similarly as in case of representation of pdf’s there are classes defining necessary interfaces. The first one

called nfFunction serves for description of nonlinear vector function objects that should implement methods for creation and evaluation of the function and its first and possible second derivation in certain point of the state space. The toolbox offers one child class nfSymFunction. This class take advantage of the Symbolic toolbox and is useful for rapid prototyping of nonlinear systems. Although, the use of the Symbolic toolbox alleviates the burden of differentiations the seamy side of its use is slower computation times and insufficient flexibility. The solution is to create own child class implementing the nfFunction interface. The second general interface is defined by the class nlng describing a system given by relations (1) and (2) (the name is chosen as an abbreviation of the words NonLinear and Non-Gaussian which describe the ”nature” of this system). Attributes of this object are instances of some nfFunction child classes. The most important method of the nlng class is the method performing simulation of the system trajectory. Unlike previously mentioned general classes that describe necessary interfaces this class is fully usable. The toolbox provides also a few child classes describing special cases of the system given by relations (1) and (2) e.g. lga defining Linear system with Gaussian Additive noises. The creation of system object will be illustrated on the system given by the relations     x 1,k+1 x 1,k · x 2,k = + wk (10) x 2,k+1 x 2,k z k = x 1,k · x 2,k + vk

(11)

where the stochastic quantities w k and vk are described by the following pdf’s     0.49 0 0 p(wk ) = N , (12) 0 0 10−5 p(vk ) = N {0, 0.01} (13) and the pdf of the initial state x 0 is     9 0 10 p(x 0 ) = N , . −0.85 0 10−4

(14)

The design of the system begins with specification of the random quantities pw = gpdf([0;0],diag([0.49 1e-5])); pv = gpdf(0,0.01); px0 = gpdf([10;-0.85], ... diag([9 1e-4])); The next steps it to declare the state and measurement functions f (·) and h(·), respectively. f =

h =

nfSymFunction( ... ’[x1*x2+w1;x2+w2]’, ... ’’,’x1,x2’,’w1,w2’); nfSymFunction(’x1*x2+v’, ... ’’,’x1,x2’,’v’);

Having created objects representing the random quantities and vector functions, it is now possible to create an entire model issuing the following constructor of the nlga class referring to NonLinear system with Gaussian Additive noises system = nlga(f,h,pw,pv,px0) The object system represents an instance of the nlga class. The trajectory of the system can be simulated using the simulate method for n time instants issuing the following command [z,x,system] = simulate(system,u,n) where the method parameter u represents know input of the system.

3.3 Design of the estimators The last part of this section deals with the third and central component of the framework i.e. the classes implementing various estimators. Currently the NFT provides methods listed in Table 1. All the estimators provide methods for evaluation of filtering and predictive pdf defined by (5) and (7), respectively. Some of the implementations support also evaluation of smoothing pdf (8). Method

NFT class

Kalman filter Extended Kalman filter Iterating Kalman filter Second order Kalman filter Gaussian sums filter Particle filter Point mass filter Divide difference filter 1st order Unscented Kalma filter

kalman extkalman itekalman seckalman gsm pf pmf dd1 ukf

Table 1. Estimation methods implemented in NFT However, the main strength of the whole NFT is its core parent class estimator. Using virtual methods it determines the interface that all the implementations of estimators should meet. To implement a new designed estimator it is mostly necessary to define relevant filtering, prediction and possibly even smoothing methods. Many of the estimator rely on application of discrete Ricatti equation and require evaluation of Kalman gain. This general class is thus ideal for implementation of corresponding methods. It reduces code redundancy and provides means for any future optimized implementation of those methods that would be automatically used by all the relevant estimator classes. The estimator class doesn’t only define the estimators interface moreover it provides a method estimate for controlling the whole estimation procedure. The purpose of this method is to control the processing of the input data and to choose an adequate sequence of estimation steps (filtering, prediction and

smoothing) and to store all the result in suitable data structure. This data structure is implemented using the class container. This class provides a mean for creation and management of dynamic lists. Even though originally it was supposed that the data type stored using the object of container class would be only a pdf object, it can store any arbitrary object and/or structure thus not restraining estimator designer. Utilization of the estimator classes is quite simple. To create an instance of some estimator object it is necessary to call the class constructor with two parameter. The first one specifies the system and the second parameter determines type of the problem. In case of choosing the 1st order Divided difference filter method and filtering problem the MATLAB command looks like filter = dd1(system,0); The filtering problem is specified by zero value of the second parameter. In case the second constructor parameter is given by the variable n, than for n>0 the n-step predictive problem is chosen. Similarly for n<0 the smoothing problem is considered. The estimation process itself can be accomplished by issuing the command [est,filter] = estimate(filter,z,NaN) For most of the implemented estimators the variable est contains a list of conditional pdf’s of the state x k for k = 0, 1, . . . n, i.e. p(x k |z k ); k = 0, 1, . . . n. In this case it should be naturally a list of objects of the gpdf class. However, in order to reduce the complexity and increase speed of the estimation algorithm, the possibility to store an arbitrary structure within the list is used. The DD1 filter needs the knowledge of Cholesky factorization of the conditional pdf’s variances. The estimator recursively repeats the filtering and prediction methods and it would normally pass only the conditional pdf’s. Thus it would unnecessary compute the factorizations which were already available in previous estimation step. Hence, it is convenient to store also the Cholesky factors. This is one example that shows the openness and flexibility of the NFT framework. Further details concerning the structure and usage of ˇ the toolbox can be found in (Simandl et al., 2003).

4. EXAMPLE OF ESTIMATOR IMPLEMENTATION The aim of this section is to demonstrate how easy it is to implement one particular estimator. For the demonstration the DD1 filter was chosen. The example will demonstrate the constructor and filtering method only because prediction and smoothing methods are designed similarly and differ only in the relations they implement.

Firstly the constructor of the class will be created. function p = dd1(varargin) switch nargin case 1 if isa(varargin{1},’dd1’) p = varargin{1}; else error(’Input is not a dd1 object’); end case {2,3} p.system = varargin{1}; if ˜isa(p.system,’nlga’) error(’The 1st argument is ... not a valid system’); end pw = get(p.system,’pw’); pv = get(p.system,’pv’); Q = var(pw); R = var(pv); pwmean = mean(pw); pvmean = mean(pv); p.scaling = sqrt(3); p.Sq = chol(Q); p.Sr = chol(R); if nargin == 2 ESTIMATOR = estimator( ... varargin{1},varargin{2}); else ESTIMATOR = estimator( ... varargin{1},varargin{2}, ... varargin{3}); end p = class(struct([]), ... ’dd1’,ESTIMATOR); otherwise error(’Incompatible arguments’); end The method firstly checks the input arguments and if there is no problem, it proceeds with dd1 object creation. It extracts necessary information about the system noises and prepares auxiliary variables. Lastly it creates the dd1 object as a child of the estimator class. The relations defining the filtering, predictive and smoothing pdf’s for DD1 filter can be found in N¨orgaard et al. (2000). The filtering method is implemented in following way function [filt]=filtering(p,pred,z) Mp = get(pred.pp,’mean’); Sp = pred.chol; system=p.system;

6. ACKNOWLEDGEMENT

Szx1 = []; for idx=1:1:system.nx sxj = Sp(:,idx); zd = nfeval(system.h, ... [(Mp + sxj*p.scaling)’ zeros(1, system.nv)]) zd_ = nfeval(system.h, ... [(Mp - sxj*p.scaling)’ zeros(1, system.nv)]) Szx1 = [Szx1 (zd - zd_)/ ... (2*p.scaling)]; end;

The work was supported by the Ministry of Education, Youth and Sports of the Czech Republic, project ˇ project 102/05/2075. 1M6798555601 and GACR ... ; ... ;

zp = nfeval(system.h, ... [Mp’ zeros(1, system.nv)]); Szp = triag([Szx1 p.Sr]); Pxzp = Sp*(Szx1)’; K = Pxzp*inv(Szp*Szp’); if (sum(sum(isnan(K))) == 0) ... & (sum(sum(isinf(K))) == 0) M = Mp + K * (z - zp); S = triag([Sp-K*Szx1 K*p.Sr]); else M = Mp; S = zeros(size(Sp)); end filt.pf = gpdf(M,S*S’); filt.chol = S; The body of the method implements the DD1 algorithm. The input arguments are the filter object, a structure containing the predictive pdf and Cholesky factor of its covariance matrix and the last argument is the measurement. Output is, in this case, Gaussian filtering pdf and Cholesky factor of filtering pdf covariance matrix.

5. CONCLUSION The description of the NFT and the example of the filter implementation should make it obvious that the usage of the framework is very simple. Also the design of some arbitrary user-defined estimator is easy as well. To design an estimator it is necessary to choose the type of conditional pdf of the state and to program the three mandatory methods i.e. the constructor of the class, the filtering and the prediction methods. The rest of the estimation process, i.e. distribution of the state pdf’s among methods, is accomplished by the toolbox. It can be seen that a new estimator implementation for the toolbox was made as easy as possible. The framework seems to be a suitable tool not only for designing and testing new estimation methods but it also provides means for better understanding of the nonlinear estimation problem for the students.

REFERENCES Gordon, N., D. Salmond and A.F.M. Smith (1993). Novel approach to nonlinear/ non-Gaussian Bayesian state estimation. IEE proceedings-F 140, 107–113. Ito, K. and K. Xiong (2000). Gaussian Filters for Nonlinear Filtering Problems. Automatica 45(5), 910–927. Jazwinski, A.H. (1970). Stochastic Processes and filtering Theory. Academic Press. Julier, S.J., J.K. Uhlmann and H.F. Durrant-white (2000). A new method for the nonlinear transformation of means and covariances in filters and estimators. IEEE Transactions on Automatic Control 45(3), 477–482. N¨orgaard, M., N.K. Poulsen and O. Ravn (2000). New developments in state estimation for nonlinear systems. 36(11), 1627–1638. ˇ Simandl, M. (1996). State Estimation For NonGaussian Models. In: Preprints of the 13th world congress. Vol. 2. IFAC. Elsevier Science. San Francisco, USA. pp. 463–468. ˇ Simandl, M. (1997). Multi-process models and outliers elimination. In: Proceedings of IASTED Int. Conf. SIP’97. IASTED. New Orleans, USA. pp. 320–325. ˇ Simandl, M. and M. Fl´ıdr (1997). Nonlinear nonnormal dynnamic models: State estimation and software. In: Computer-Intensive Methods in Control and Signal Processing. Birkh¨auser. Boston. ˇ Simandl, M. and O. Straka (2004). Sampling density design for particle filters. In: Proceedings of the 13th IFAC Symposium on System Identification. Elsevier Ltd.. Oxford. pp. 175–180. ˇ Simandl, M., J. Kr´alovec and T. S¨oderstr¨om (2002). Anticipative Grid Design in Point-Mass Approach to Nonlinear State Estimation. IEEE Trans. on Automatic Control 47(4), 699–702. ˇ ˇ acha (2003). Simandl, M., M. Fl´ıdr, O. Straka and J. Sv´ Nonlinear filtering toolbox. Technical report. Department of Cybernetics, University of West Bohemia in Pilsen. Czech Republic. Sorenson, H.W. (1988). Bayesian Analysis of Time Series and Dynamic Models. Chap. Recursive Estimation for Nonlinear Dynamic Systems, pp. 127–165. Marcel Dekker. New York. Sorenson, H.W. and D.L. Alspach (1971). Recursive Bayesian Estimation Using Gaussian Sums. Automatica 7, 465–479. ˇ ˇ acha and J. Simandl, M., O. Straka, M. Fl´ıdr, J. Sv´ Dun´ık (2004). Nonlinear filtering methods: basic approaches and software package. In: Preprints of the CMP 2004. Prague, Czech Republic.