Context Navigation

libKF.h @ 358

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

Note: See TracBrowser for help on using the browser.

Context Navigation

root/bdm/estim/libKF.h @ 358

Download in other formats: