Context Navigation

← Previous Change
Next Change →

Changeset 37 for tests

Timestamp:

03/14/08 18:11:21 (17 years ago)

Author:

smidl

Message:

Matrix in Cholesky decomposition, Square-root Kalman and many bug fixes

Location:

Files:

: 4 modified

pmsm_unkQ.cpp (modified) (3 diffs)
pmsm_unkQpf.cpp (modified) (3 diffs)
test0.cpp (modified) (4 diffs)
testKF.cpp (modified) (4 diffs)

Legend:

: Unmodified
: Added
: Removed

tests/pmsm_unkQ.cpp

r33	r37
21	21	/*
22	22	// PMSM with Q on Ia and Ib given externally
23		class EKF_unQ : public EKF<ldmat> , public BMcond {
	23	class EKF_unQ : public EKF<chmat> , public BMcond {
24	24	public:
25		EKF_unQ( rx,ry,ru,rQ):EKF<ldmat>(rx,ry,ru),BMcond(rQ){};
	25	EKF_unQ( rx,ry,ru,rQ):EKF<chmat>(rx,ry,ru),BMcond(rQ){};
26	26	void condition(const vec &Q0){};
27	27	};*/
28	28
	29
29	30	int main() {
30	31	// Kalman filter
31		int Ndat = 10000;
	32	int Ndat = 1000;
32	33
33	34	// cout << KF;
…	…
40	41
41	42	vec mu0= "100 100 100 1";
42		vec Qdiag ( "0.1 0.1 0.01 0.0~~000~~1" );
	43	vec Qdiag ( "0.1 0.1 0.01 0.01" );
43	44	vec Rdiag ( "0.02 0.02" );
44	45	vec vQ = "0.01:0.01:100";
45		ldmat Q = ldmat ( Qdiag );
46		ldmat R = ldmat ( Rdiag );
47		EKF<ldmat> KFE ( rx,ry,ru );
	46	chmat Q ( Qdiag );
	47	chmat R ( Rdiag );
	48	EKFCh KFE ( rx,ry,ru );
	49	KFE.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) );
48	50	KFE.set_parameters ( &fxu,&hxu,Q,R );
49		~~KFE.set_est ( mu0, ldmat ( 1000*ones ( 4 ) ) );~~
50	51
51	52	mat ll(100,Ndat);
52	53
53		EKF~~<ldmat>~~* kfArray[100];
	54	EKFCh* kfArray[100];
54	55
55	56	for ( int i=0;i<100;i++ ) {
56	57	vec Qid ( Qdiag );
57	58	Qid ( 0 ) = vQ ( i ); Qid ( 1 ) = vQ ( i );
58		kfArray[i]= new EKF~~<ldmat>~~ ( rx,ry,ru );
59		kfArray[i]->set_~~parameters ( &fxu,&hxu,ldmat ( Qid ),R~~ );
60		kfArray[i]->set_~~est ( mu0, ldmat ( 1000*ones ( 4 ) )~~ );
	59	kfArray[i]= new EKFCh ( rx,ry,ru );
	60	kfArray[i]->set_est ( mu0, chmat ( 1000*ones ( 4 ) ) );
	61	kfArray[i]->set_parameters ( &fxu,&hxu,chmat ( Qid ),R );
61	62	}
62	63
…	…
89	90	//estimator
90	91	KFE.bayes ( concat ( dt,ut ) );
91		for ( int i=0;i<100;i++ ) {kfArray[i]->bayes( concat ( dt,ut ) );ll(i,t)+=kfArray[i]->_ll();}
	92	for ( int i=0;i<100;i++ ) {kfArray[i]->bayes( concat ( dt,ut ) );ll(i,t)=ll(i,t-1) + kfArray[i]->_ll();
	93	}
92	94
93	95	Xt.set_col ( t,xt );

tests/pmsm_unkQpf.cpp

r33	r37
21	21
22	22	//!Extended Kalman filter with unknown \c Q
23		class EKF_unQ : public EKF~~<ldmat>~~ , public BMcond {
	23	class EKF_unQ : public EKFCh , public BMcond {
24	24	public:
25	25	//! Default constructor
26		EKF_unQ (RV rx, RV ry,RV ru,RV rQ ) :EKF<ldmat> ( rx,ry,ru ),BMcond ( rQ ) {};
27		void condition ( const vec &Q0 ) {Q.setD ( Q0,0 ); }; //set first two diag(Q)
	26	EKF_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {};
	27	void condition ( const vec &Q0 ) {
	28	Q.setD ( Q0,0 );
	29	//from EKF
	30	preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
	31	};
28	32	};
29	33
…	…
45	49	vec vQ = "0.01:0.01:100";
46	50
47		~~ldmat Q = ldmat~~ ( Qdiag );
48		~~ldmat R = ldmat~~ ( Rdiag );
	51	chmat Q ( Qdiag );
	52	chmat R ( Rdiag );
49	53
50	54	RV rQ ( "100","{Q}","2","0" );
51	55	EKF_unQ KFE ( rx,ry,ru,rQ );
52	56	KFE.set_parameters ( &fxu,&hxu,Q,R );
53		KFE.set_est ( mu0, ldmat ( 1000*ones ( 4 ) ) );
	57	KFE.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) );
54	58
55	59	mgamma evolQ ( rQ,rQ );
56		evolQ.set_parameters ( 10000.0 ); //sigma = 1/10 mu
	60	//evolQ.set_parameters ( 10000.0 ); //sigma = 1/10 mu
57	61
58	62	MPF<EKF_unQ> M ( rx,rQ,evolQ,evolQ,100,KFE );
…	…
61	65	epdf& Mep = M._epdf();
62	66	// initialize
	67	evolQ.set_parameters ( 1.0 ); //sigma = 1/10 mu
63	68	evolQ.condition ( "0.5 0.5" );
64	69	epdf& pfinit=evolQ._epdf();
65	70	M.set_est ( pfinit );
	71	evolQ.set_parameters ( 1000.0 ); //sigma = 1/10 mu
66	72
67	73	//simulator values
68		vec dt ( 2 );
69		vec wt ( 2 );
70		vec ut ( 2 );
71		vec xt=mu0;
72		vec et ( 4 );
	74	vec dt ( 2 ); // output (isa isb)
	75	vec wt ( 2 ); // noise on dt
	76	vec ut ( 2 ); //
	77	vec xt=mu0; // initial state
	78	vec et ( 4 ); // noise on xt
73	79
74		mat Xt=zeros ( 4,Ndat );
75		mat XtE=zeros ( 4,Ndat );
76		mat XtM=zeros ( 6,Ndat );
77		Xt.set_col ( 0,~~KFEep.mean()~~ );
	80	mat Xt=zeros ( 4,Ndat ); // True trajetory of xt
	81	mat XtE=zeros ( 4,Ndat ); // Estimate of xt using EKF (known Q)
	82	mat XtM=zeros ( 6,Ndat ); // Estimate of xt using EKF-MPF
	83	Xt.set_col ( 0,mu0 );
78	84	XtM.set_col ( 0,Mep.mean() );
79	85
80	86	for ( int t=1;t<Ndat;t++ ) {
81	87	//simulator
82		UniRNG.sample_vector ( 2,wt );
	88	// UniRNG.sample_vector ( 2,wt );
83	89
84	90	if ( rem ( t,500 ) <200 ) ut = rem ( t,500 ) *ones ( 2 );

tests/test0.cpp

r35	r37
2	2	#include "../bdm/stat/libBM.h"
3	3	#include "../bdm/math/libDC.h"
	4	#include "../bdm/math/chmat.h"
4	5
5	6	using namespace itpp;
…	…
11	12	int main()
12	13	{
13
14	14	RV th = RV ( "1 2","{a b }","1 1","0 0");
15	15	RV r = RV ( "3 4" );
…	…
38	38	ld.inv(Il); //
39	39	mat I = Il.to_mat()*ld.to_mat();
40		cout << "ld:"<<Il.to_mat() << "eye:"<< I <<endl;
	40	cout << "ld:"<<Il.to_mat() << "eye:"<< I <<endl;
41	41
42	42	cout << "Test ldform()"<<endl;
…	…
47	47	cout << "ld:" << lV << "eye:"<< V*(ilV.to_mat()) <<endl;
48	48
	49	cout << "Test logdet()"<<endl;
	50	chmat chV(V);
	51	cout << "ch:" << chV.to_mat() <<endl;
	52	cout << "log(det(V)):"<< chV.logdet() <<endl;
	53	cout << "qform : " << chV.qform(ones(2)) <<endl;
	54
49	55
50	56	//Exit program:

tests/testKF.cpp

r33	r37
9	9
10	10	int main() {
11
12
13	11	// Kalman filter
14	12	mat A, B,C,D,R,Q,P0;
…	…
18	16	it_file fin( "testKF.it" );
19	17
20		mat Dt~~, Xt,Xt2,XtE,Xtf~~;
	18	mat Dt;
21	19	int Ndat;
22	20
…	…
40	38
41	39	Ndat = Dt.cols();
42		Xt=zeros( 2,Ndat );
43		Xt2=zeros( 2,Ndat );
44		Xtf=zeros( 2,Ndat );
45		XtE=zeros( 2,Ndat );
	40	int dimx = A.rows();
	41	int dimy = C.rows();
	42	int dimu = B.cols();
	43
	44	// Prepare for Kalman filters in BDM:
	45	ivec tmpsize(1);
	46	tmpsize(0) = A.cols();
	47	RV rx("1","{x}",tmpsize,"0");
	48	tmpsize(0) = B.cols();
	49	RV ru("2","{u}",tmpsize,"0");
	50	tmpsize(0) = C.rows();
	51	RV ry("3","{y}",tmpsize,"0");
	52
	53	// // LDMAT
	54	// Kalman<ldmat> KF(rx,ry,ru);
	55	// KF.set_parameters(A,B,C,D,ldmat(R),ldmat(Q));
	56	// KF.set_est(mu0,ldmat(P0) );
	57	// epdf& KFep = KF._epdf();
	58	// mat Xt(2,Ndat);
	59	// Xt.set_col( 0,KFep.mean() );
46	60
47		// cout << KF;
48		RV rx("1","{x}","2","0");
49		RV ru("2","{u}","1","0");
50		RV ry("3","{y}","1","0");
51		//
52		Kalman<ldmat> KF(rx,ry,ru);
53		KF.set_parameters(A,B,C,D,ldmat(R),ldmat(Q));
54		KF.set_est(mu0,ldmat(P0) );
55		//
56		Kalman<fsqmat> KFf(rx,ry,ru);
57		KFf.set_parameters(A,B,C,D,fsqmat(R),fsqmat(Q));
58		KFf.set_est(mu0,fsqmat(P0) );
59		//
	61	//Chol
	62	KalmanCh KF(rx,ry,ru);
	63	KF.set_parameters(A,B,C,D,chmat(R),chmat(Q));
	64	KF.set_est(mu0,chmat(P0) ); //prediction!
	65	epdf& KFep = KF._epdf();
	66	mat Xt(dimx,Ndat);
	67	Xt.set_col( 0,KFep.mean() );
	68
	69	//
	70	// FSQMAT
	71	Kalman<ldmat> KFf(rx,ry,ru);
	72	KFf.set_parameters(A,B,C,D,ldmat(R),ldmat(Q));
	73	KFf.set_est(mu0,ldmat(P0) );
	74	epdf& KFfep = KFf._epdf();
	75	mat Xtf(dimx,Ndat);
	76	Xtf.set_col( 0,KFfep.mean() );
	77
	78	// FULL
60	79	KalmanFull KF2( A,B,C,D,R,Q,P0,mu0 );
61		//
	80	mat Xt2(dimx,Ndat);
	81	Xt2.set_col( 0,mu0);
	82
	83
	84	// EKF
62	85	bilinfn fxu(rx,ru,A,B);
63	86	bilinfn hxu(rx,ru,C,D);
64		EKF~~<ldmat>~~ KFE(rx,ry,ru);
	87	EKFCh KFE(rx,ry,ru);
65	88	KFE.set_parameters(&fxu,&hxu,Q,R);
66		KFE.set_est(mu0,P0);
	89	KFE.set_est(mu0,chmat(P0));
	90	epdf& KFEep = KFE._epdf();
	91	mat XtE(dimx,Ndat);
	92	XtE.set_col( 0,KFEep.mean() );
67	93
68		~~epdf& KFep = KF._epdf();~~
69		~~epdf& KFfep = KFf._epdf()~~;
70		~~epdf& KFEep = KFE._epdf();~~
	94	//test performance of each filter
	95	Real_Timer tt;
	96	vec exec_times(4); // KF, KFf KF2, KFE
71	97
72		Xt.set_col( 0,KFep.mean() );
73		Xtf.set_col( 0,KFfep.mean() );
74		Xt2.set_col( 0,KF2.mu );
75		XtE.set_col( 0,KFEep.mean() );
	98	tt.tic();
	99	for ( int t=1;t<Ndat;t++ ) {
	100	KF.bayes( Dt.get_col( t ));
	101	Xt.set_col( t,KFep.mean() );
	102	}
	103	exec_times(0) = tt.toc();
	104
	105	tt.tic();
76	106	for ( int t=1;t<Ndat;t++ ) {
77	107	KFf.bayes( Dt.get_col( t ));
78		KF.bayes( Dt.get_col( t ));
	108	Xtf.set_col( t,KFfep.mean() );
	109	}
	110	exec_times(1) = tt.toc();
	111
	112	tt.tic();
	113	for ( int t=1;t<Ndat;t++ ) {
79	114	KF2.bayes( Dt.get_col( t ));
	115	Xt2.set_col( t,KF2.mu);
	116	}
	117	exec_times(2) = tt.toc();
	118
	119	tt.tic();
	120	for ( int t=1;t<Ndat;t++ ) {
80	121	KFE.bayes( Dt.get_col( t ));
81		~~Xt.set_col( t,KFep.mean() );~~
82		~~Xtf.set_col( t,KFfep.mean() );~~
83		~~Xt2.set_col(t,KF2.mu);~~
84	122	XtE.set_col( t,KFEep.mean() );
85	123	}
	124	exec_times(3) = tt.toc();
	125
86	126
87	127	it_file fou( "testKF_res.it" );
…	…
90	130	fou << Name("xth2") << Xt2;
91	131	fou << Name("xthE") << XtE;
	132	fou << Name("exec_times") << exec_times;
92	133	//Exit program:
93	134	return 0;

Download in other formats: