Context Navigation

libKF.h @ 338

Revision 338, 12.8 kB (checked in by smidl, 15 years ago)
Multiple Models + changes in loggers
Property svn:eol-style set to `native`

Line
1	/*!
2	\file
3	\brief Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions
4	\author Vaclav Smidl.
5
6	-----------------------------------
7	BDM++ - C++ library for Bayesian Decision Making under Uncertainty
8
9	Using IT++ for numerical operations
10	-----------------------------------
11	*/
12
13	#ifndef KF_H
14	#define KF_H
15
16
17	#include "../stat/libFN.h"
18	#include "../stat/libEF.h"
19	#include "../math/chmat.h"
20
21	namespace bdm
22	{
23
24	/*!
25	* \brief Basic Kalman filter with full matrices (education purpose only)! Will be deleted soon!
26	*/
27
28	class KalmanFull
29	{
30	protected:
31	int dimx, dimy, dimu;
32	mat A, B, C, D, R, Q;
33
34	//cache
35	mat _Pp, _Ry, _iRy, _K;
36	public:
37	//posterior
38	//! Mean value of the posterior density
39	vec mu;
40	//! Variance of the posterior density
41	mat P;
42
43	bool evalll;
44	double ll;
45	public:
46	//! Full constructor
47	KalmanFull ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0 );
48	//! Here dt = [yt;ut] of appropriate dimensions
49	void bayes ( const vec &dt );
50	//! print elements of KF
51	friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf );
52	//! For EKFfull;
53	KalmanFull() {};
54	};
55
56
57	/*!
58	* \brief Kalman filter with covariance matrices in square root form.
59
60	Parameter evolution model:\f[ x_t = A x_{t-1} + B u_t + Q^{1/2} e_t \f]
61	Observation model: \f[ y_t = C x_{t-1} + C u_t + Q^{1/2} w_t. \f]
62	Where $e_t$ and $w_t$ are independent vectors Normal(0,1)-distributed disturbances.
63	*/
64	template<class sq_T>
65
66	class Kalman : public BM
67	{
68	protected:
69	//! Indetifier of output rv
70	RV rvy;
71	//! Indetifier of exogeneous rv
72	RV rvu;
73	//! cache of rv.count()
74	int dimx;
75	//! cache of rvy.count()
76	int dimy;
77	//! cache of rvu.count()
78	int dimu;
79	//! Matrix A
80	mat A;
81	//! Matrix B
82	mat B;
83	//! Matrix C
84	mat C;
85	//! Matrix D
86	mat D;
87	//! Matrix Q in square-root form
88	sq_T Q;
89	//! Matrix R in square-root form
90	sq_T R;
91
92	//!posterior density on $x_t$
93	enorm<sq_T> est;
94	//!preditive density on $y_t$
95	enorm<sq_T> fy;
96
97	//! placeholder for Kalman gain
98	mat _K;
99	//! cache of fy.mu
100	vec& _yp;
101	//! cache of fy.R
102	sq_T& _Ry;
103	//!cache of est.mu
104	vec& _mu;
105	//!cache of est.R
106	sq_T& _P;
107
108	public:
109	//! Default constructor
110	Kalman ( );
111	//! Copy constructor
112	Kalman ( const Kalman<sq_T> &K0 );
113	//! Set parameters with check of relevance
114	void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const sq_T &Q0,const sq_T &R0 );
115	//! Set estimate values, used e.g. in initialization.
116	void set_est ( const vec &mu0, const sq_T &P0 )
117	{
118	sq_T pom ( dimy );
119	est.set_parameters ( mu0,P0 );
120	P0.mult_sym ( C,pom );
121	fy.set_parameters ( C*mu0, pom );
122	};
123
124	//! Here dt = [yt;ut] of appropriate dimensions
125	void bayes ( const vec &dt );
126	//!access function
127	const epdf& posterior() const {return est;}
128	const enorm<sq_T>* _e() const {return &est;}
129	//!access function
130	mat& __K() {return _K;}
131	//!access function
132	vec _dP() {return _P->getD();}
133	};
134
135	/*! \brief Kalman filter in square root form
136
137	Trivial example:
138	\include kalman_simple.cpp
139
140	*/
141	class KalmanCh : public Kalman<chmat>
142	{
143	protected:
144	//! pre array (triangular matrix)
145	mat preA;
146	//! post array (triangular matrix)
147	mat postA;
148
149	public:
150	//! copy constructor
151	BM* _copy_() const
152	{
153	KalmanCh* K=new KalmanCh;
154	K->set_parameters ( A,B,C,D,Q,R );
155	K->set_statistics ( _mu,_P );
156	return K;
157	}
158	//! Set parameters with check of relevance
159	void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &Q0,const chmat &R0 );
160	void set_statistics ( const vec &mu0, const chmat &P0 )
161	{
162	est.set_parameters ( mu0,P0 );
163	};
164
165
166	/*!\brief Here dt = [yt;ut] of appropriate dimensions
167
168	The following equality hold::\f[
169	\left[\begin{array}{cc}
170	R^{0.5}\\
171	P_{t\|t-1}^{0.5}C' & P_{t\|t-1}^{0.5}CA'\\
172	& Q^{0.5}\end{array}\right]<\mathrm{orth.oper.}>=\left[\begin{array}{cc}
173	R_{y}^{0.5} & KA'\\
174	& P_{t+1\|t}^{0.5}\\
175	\\\end{array}\right]\f]
176
177	Thus this object evaluates only predictors! Not filtering densities.
178	*/
179	void bayes ( const vec &dt );
180	};
181
182	/*!
183	\brief Extended Kalman Filter in full matrices
184
185	An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
186	*/
187	class EKFfull : public KalmanFull, public BM
188	{
189	protected:
190	//! Internal Model f(x,u)
191	diffbifn* pfxu;
192	//! Observation Model h(x,u)
193	diffbifn* phxu;
194
195	enorm<fsqmat> E;
196	public:
197	//! Default constructor
198	EKFfull ( );
199	//! Set nonlinear functions for mean values and covariance matrices.
200	void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const mat Q0, const mat R0 );
201	//! Here dt = [yt;ut] of appropriate dimensions
202	void bayes ( const vec &dt );
203	//! set estimates
204	void set_statistics ( vec mu0, mat P0 ) {mu=mu0;P=P0;};
205	//!dummy!
206	const epdf& posterior() const{return E;};
207	const enorm<fsqmat>* _e() const{return &E;};
208	const mat _R() {return P;}
209	};
210
211	/*!
212	\brief Extended Kalman Filter
213
214	An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
215	*/
216	template<class sq_T>
217	class EKF : public Kalman<fsqmat>
218	{
219	//! Internal Model f(x,u)
220	diffbifn* pfxu;
221	//! Observation Model h(x,u)
222	diffbifn* phxu;
223	public:
224	//! Default constructor
225	EKF ( RV rvx, RV rvy, RV rvu );
226	//! copy constructor
227	EKF<sq_T>* _copy_() const { return new EKF<sq_T> ( this ); }
228	//! Set nonlinear functions for mean values and covariance matrices.
229	void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0 );
230	//! Here dt = [yt;ut] of appropriate dimensions
231	void bayes ( const vec &dt );
232	};
233
234	/*!
235	\brief Extended Kalman Filter in Square root
236
237	An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
238	*/
239
240	class EKFCh : public KalmanCh
241	{
242	protected:
243	//! Internal Model f(x,u)
244	diffbifn* pfxu;
245	//! Observation Model h(x,u)
246	diffbifn* phxu;
247	public:
248	//! copy constructor duplicated - calls different set_parameters
249	BM* _copy_() const
250	{
251	EKFCh* E=new EKFCh;
252	E->set_parameters ( pfxu,phxu,Q,R );
253	E->set_statistics ( _mu,_P );
254	return E;
255	}
256	//! Set nonlinear functions for mean values and covariance matrices.
257	void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const chmat Q0, const chmat R0 );
258	//! Here dt = [yt;ut] of appropriate dimensions
259	void bayes ( const vec &dt );
260	};
261
262	/*!
263	\brief Kalman Filter with conditional diagonal matrices R and Q.
264	*/
265
266	class KFcondQR : public Kalman<ldmat>
267	{
268	//protected:
269	public:
270	void condition ( const vec &QR )
271	{
272	it_assert_debug ( QR.length() == ( dimx+dimy ),"KFcondRQ: conditioning by incompatible vector" );
273
274	Q.setD ( QR ( 0, dimx-1 ) );
275	R.setD ( QR ( dimx, -1 ) );
276	};
277	};
278
279	/*!
280	\brief Kalman Filter with conditional diagonal matrices R and Q.
281	*/
282
283	class KFcondR : public Kalman<ldmat>
284	{
285	//protected:
286	public:
287	//!Default constructor
288	KFcondR ( ) : Kalman<ldmat> ( ) {};
289
290	void condition ( const vec &R0 )
291	{
292	it_assert_debug ( R0.length() == ( dimy ),"KFcondR: conditioning by incompatible vector" );
293
294	R.setD ( R0 );
295	};
296
297	};
298
299	//////// INstance
300
301	template<class sq_T>
302	Kalman<sq_T>::Kalman ( const Kalman<sq_T> &K0 ) : BM ( ),rvy ( K0.rvy ),rvu ( K0.rvu ),
303	dimx ( K0.dimx ), dimy ( K0.dimy ),dimu ( K0.dimu ),
304	A ( K0.A ), B ( K0.B ), C ( K0.C ), D ( K0.D ),
305	Q ( K0.Q ), R ( K0.R ),
306	est ( K0.est ), fy ( K0.fy ), _yp ( fy._mu() ),_Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() )
307	{
308
309	// copy values in pointers
310	// _mu = K0._mu;
311	// _P = K0._P;
312	// _yp = K0._yp;
313	// _Ry = K0._Ry;
314
315	}
316
317	template<class sq_T>
318	Kalman<sq_T>::Kalman ( ) : BM (), est ( ), fy (), _yp ( fy._mu() ), _Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() )
319	{
320	};
321
322	template<class sq_T>
323	void Kalman<sq_T>::set_parameters ( const mat &A0,const mat &B0, const mat &C0, const mat &D0, const sq_T &Q0, const sq_T &R0 )
324	{
325	dimx = A0.rows();
326	dimy = C0.rows();
327	dimu = B0.cols();
328
329	it_assert_debug ( A0.cols() ==dimx, "Kalman: A is not square" );
330	it_assert_debug ( B0.rows() ==dimx, "Kalman: B is not compatible" );
331	it_assert_debug ( C0.cols() ==dimx, "Kalman: C is not square" );
332	it_assert_debug ( ( D0.rows() ==dimy ) \|\| ( D0.cols() ==dimu ), "Kalman: D is not compatible" );
333	it_assert_debug ( ( R0.cols() ==dimy ) \|\| ( R0.rows() ==dimy ), "Kalman: R is not compatible" );
334	it_assert_debug ( ( Q0.cols() ==dimx ) \|\| ( Q0.rows() ==dimx ), "Kalman: Q is not compatible" );
335
336	A = A0;
337	B = B0;
338	C = C0;
339	D = D0;
340	R = R0;
341	Q = Q0;
342	}
343
344	template<class sq_T>
345	void Kalman<sq_T>::bayes ( const vec &dt )
346	{
347	it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
348
349	sq_T iRy ( dimy );
350	vec u = dt.get ( dimy,dimy+dimu-1 );
351	vec y = dt.get ( 0,dimy-1 );
352	//Time update
353	_mu = A* _mu + B*u;
354	//P = APA.transpose() + Q; in sq_T
355	_P.mult_sym ( A );
356	_P +=Q;
357
358	//Data update
359	//_Ry = CPC.transpose() + R; in sq_T
360	_P.mult_sym ( C, _Ry );
361	_Ry +=R;
362
363	mat Pfull = _P.to_mat();
364
365	_Ry.inv ( iRy ); // result is in _iRy;
366	_K = PfullC.transpose() ( iRy.to_mat() );
367
368	sq_T pom ( ( int ) Pfull.rows() );
369	iRy.mult_sym_t ( C*Pfull,pom );
370	( _P ) -= pom; // P = P -PC'iRy*CP;
371	( _yp ) = C* _mu +D*u; //y prediction
372	( _mu ) += _K* ( y- _yp );
373
374
375	if ( evalll==true ) //likelihood of observation y
376	{
377	ll=fy.evallog ( y );
378	}
379
380	//cout << "y: " << y-(*_yp) <<" R: " << _Ry->to_mat() << " iR: " << _iRy->to_mat() << " ll: " << ll <<endl;
381
382	};
383
384	/*! \brief (Switching) Multiple Model
385	The model runs several models in parallel and evaluates thier weights (fittness).
386
387	The statistics of the resulting density are merged using (geometric?) combination.
388
389	The next step is performed with the new statistics for all models.
390	*/
391	class MultiModel: public BM
392	{
393	protected:
394	//! List of models between which we switch
395	Array<EKFCh*> Models;
396	//! vector of model weights
397	vec w;
398	//! cache of model lls
399	vec _lls;
400	//! type of switching policy [1=maximum,2=...]
401	int policy;
402	//! internal statistics
403	enorm<chmat> est;
404	public:
405	void set_parameters ( Array<EKFCh*> A, int pol0=1 )
406	{
407	Models=A;//TODO: test if evalll is set
408	w.set_length ( A.length() );
409	_lls.set_length ( A.length() );
410	policy=pol0;
411
412	est.set_rv(RV("MM",A(0)->posterior().dimension(),0));
413	est.set_parameters(A(0)->_e()->mean(), A(0)->_e()->_R());
414	}
415	void bayes ( const vec &dt )
416	{
417	int n = Models.length();
418	int i;
419	for ( i=0;i<n;i++ )
420	{
421	Models ( i )->bayes ( dt );
422	_lls ( i ) = Models ( i )->_ll();
423	}
424	double mlls=max ( _lls );
425	w=exp ( _lls-mlls );
426	w/=sum ( w ); //normalization
427	//set statistics
428	switch ( policy )
429	{
430	case 1:
431	{
432	int mi=max_index ( w );
433	const enorm<chmat>* st=(Models(mi)->_e());
434	est.set_parameters(st->mean(), st->_R());
435	}
436	break;
437	default: it_error ( "unknown policy" );
438	}
439	// copy result to all models
440	for ( i=0;i<n;i++ )
441	{
442	Models ( i )->set_statistics ( est.mean(),est._R());
443	}
444	}
445	//all posterior densities are equal => return the first one
446	const enorm<chmat>* _e() const {return &est;}
447	//all posterior densities are equal => return the first one
448	const enorm<chmat>& posterior() const {return est;}
449	};
450
451
452	//TODO why not const pointer??
453
454	template<class sq_T>
455	EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<sq_T> ( rvx0,rvy0,rvu0 ) {}
456
457	template<class sq_T>
458	void EKF<sq_T>::set_parameters ( diffbifn* pfxu0, diffbifn* phxu0,const sq_T Q0,const sq_T R0 )
459	{
460	pfxu = pfxu0;
461	phxu = phxu0;
462
463	//initialize matrices A C, later, these will be only updated!
464	pfxu->dfdx_cond ( _mu,zeros ( dimu ),A,true );
465	// pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );
466	B.clear();
467	phxu->dfdx_cond ( _mu,zeros ( dimu ),C,true );
468	// phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
469	D.clear();
470
471	R = R0;
472	Q = Q0;
473	}
474
475	template<class sq_T>
476	void EKF<sq_T>::bayes ( const vec &dt )
477	{
478	it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
479
480	sq_T iRy ( dimy,dimy );
481	vec u = dt.get ( dimy,dimy+dimu-1 );
482	vec y = dt.get ( 0,dimy-1 );
483	//Time update
484	_mu = pfxu->eval ( _mu, u );
485	pfxu->dfdx_cond ( _mu,u,A,false ); //update A by a derivative of fx
486
487	//P = APA.transpose() + Q; in sq_T
488	_P.mult_sym ( A );
489	_P +=Q;
490
491	//Data update
492	phxu->dfdx_cond ( _mu,u,C,false ); //update C by a derivative hx
493	//_Ry = CPC.transpose() + R; in sq_T
494	_P.mult_sym ( C, _Ry );
495	( _Ry ) +=R;
496
497	mat Pfull = _P.to_mat();
498
499	_Ry.inv ( iRy ); // result is in _iRy;
500	_K = PfullC.transpose() ( iRy.to_mat() );
501
502	sq_T pom ( ( int ) Pfull.rows() );
503	iRy.mult_sym_t ( C*Pfull,pom );
504	( _P ) -= pom; // P = P -PC'iRy*CP;
505	_yp = phxu->eval ( _mu,u ); //y prediction
506	( _mu ) += _K* ( y-_yp );
507
508	if ( evalll==true ) {ll+=fy.evallog ( y );}
509	};
510
511
512	}
513	#endif // KF_H
514

Note: See TracBrowser for help on using the browser.

Context Navigation

root/bdm/estim/libKF.h @ 338

Download in other formats: