root/applications/pmsm/simulator_zdenek/test_Ch.cpp @ 1240

Revision 1230, 4.0 kB (checked in by smidl, 14 years ago)

givens + new Q for UD

Line 
1#include "ekf_example/matrix_vs.h"
2#include "ekf_example/ekf_obj.h"
3int main(){
4        int i;
5        mat A = 0.99*eye(5);
6        A(0,3) = 0.06;
7        A(0,2) = 0.01;
8        A(1,2) = 0.01;
9        A(1,3) = -0.07;
10        A(3,2) = 0.0001;
11
12//      RNG_randomize();
13       
14        mat U=eye(5);
15        for (int i=0; i<5;i++) {
16                for (int j=i+1; j<5;j++) U(i,j)=2*randu(1)(0)-1;
17        }
18        mat Q = 0.1*diag(vec(" 0.2000 0.3000 0.4000 0.5000 0.6"));
19        vec R = vec(" 0.2000 0.3000");
20       
21                vec D = randu(5)*0.9;
22                vec xref = ones(5);
23
24                mat Ch = U*diag(sqrt(D));
25               
26                int PSI[25];
27                int PSICh[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
28                int Chf[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
29                int multip=1<<15;
30               
31        /////////// COPY
32        imat Af=round_i(A*multip);
33        mat_to_int(Af, PSI);
34        mat_to_int(round_i(Ch*multip),Chf);             
35        int Qf[25];
36        mat_to_int(round_i(sqrt(Q)*multip), Qf);
37        int Rf[2];
38        vec_to_int(round_i(R*multip), Rf);
39       
40       
41        ////////////// Test mmultAU
42        mmultACh(PSI,Chf,PSICh,5,5);   
43       
44        mat PhiCh =round(A*32768)*round(Ch*32768)/32768/32768;
45/*      cout << "A*U" << round_i(PhiU*multip) <<endl;
46        cout << "PSIU: "; for (i=0; i<25;i++) cout << PSIU[i] << ","; cout <<endl;*/
47       
48        imat PChcmp(PSICh,5,5);
49        cout << "Delta PSI: " << round_i(PhiCh*multip-PChcmp) <<endl;
50       
51        mat_to_int(round_i(PhiCh*multip),PSICh); //<< make is same
52
53        mat PhiU= A*U;
54/////////// Test Thorton:
55        int dim=5;double sigma; int j,k; 
56        vec Din = D;
57        mat G=eye(5);
58        for (i=dim-1; i>=0;i--){
59                sigma = 0.0; 
60                for (j=0; j<dim; j++) {
61                        sigma += PhiU(i,j)*PhiU(i,j) *Din(j); 
62                        sigma += G(i,j)*G(i,j) * Q(j,j); 
63                }
64                D(i) = sigma; 
65                for (j=0;j<i;j++){ 
66                        sigma = 0.0; 
67                        for (k=0;k<dim;k++){ 
68                                sigma += PhiU(i,k)*Din(k)*PhiU(j,k); 
69                        }
70                        for (k=0;k<dim;k++){ 
71                                sigma += G(i,k)*Q(k,k)*G(j,k); 
72                        }
73                        //
74                        U(j,i) = sigma/D(i); 
75                        for (k=0;k<dim;k++){ 
76                                PhiU(j,k) = PhiU(j,k) - U(j,i)*PhiU(i,k); 
77                        }
78                        for (k=0;k<dim;k++){ 
79                                G(j,k) = G(j,k) - U(j,i)*G(i,k); 
80                        }
81                }
82        }
83
84cout << "PSICh: " << imat(PSICh,5,5) << endl;
85cout << "Qf: " << imat(Qf,5,5) << endl;
86
87Ch = U*diag(sqrt(D));
88
89       
90        householder(PSICh,Qf,5);
91//      givens(PSICh,Qf,5);
92
93        /////// disp
94       
95        cout << endl<<"after householder " <<endl;
96
97        imat Chcmp(PSICh,5,5);
98        cout << "Ch(UD): " << round_i(Ch*multip) << endl;
99        cout << "Ch: " << (Chcmp) << endl;
100        cout << "Delta Ch: " << round_i(Ch*multip-Chcmp) << endl;
101       
102        // synchronize
103        vec d=diag(Ch*Ch.T());
104        mat Ch2 = 0.9*diag(1./sqrt(d))*Ch;
105        mat_to_int(round_i(Ch2*multip),Chf);           
106
107        D = pow(diag(Ch2),2);
108        U = sqrt(diag(1./D));
109        U = Ch2*U;
110               
111        vec     ydif = 2*randu(2)-1;
112        vec     xp = 2*randu(5)-1;
113       
114        int difz[2];
115        vec_to_int(round_i(ydif*multip), difz);
116        int xf[5];
117        vec_to_int(round_i(xp*multip), xf);
118
119        cout << "x: "<< round_i(xp*multip) <<endl;
120        cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
121       
122       
123        int xf_old[5];
124        vec_to_int(ivec(xf,5),xf_old);
125       
126        /////// Test bierman
127        double dz,alpha,gamma,beta,lambda;
128        vec a;
129        vec b;
130        mat C = zeros(2,5);
131        C(0,0)=1.0;C(1,1)=1.0;
132        for (int iy=0; iy<2; iy++){
133                a     = U.T()*C.get_row(iy);    // a is not modified, but
134                b     = elem_mult(D,a);                          // b is modified to become unscaled Kalman gain.
135                dz    = ydif(iy); 
136               
137                alpha = R(iy); 
138                gamma = 1/alpha; 
139                for (j=0;j<dim;j++){
140                        beta   = alpha; 
141                        alpha  = alpha + a(j)*b(j); 
142                        lambda = -a(j)*gamma; 
143                        gamma  = 1.0/alpha; 
144                        D(j) = beta*gamma*D(j); 
145                       
146                        //                      cout << "a: " << alpha << "g: " << gamma << endl;
147                        for (i=0;i<j;i++){
148                                beta   = U(i,j); 
149                                U(i,j) = beta + b(i)*lambda; 
150                                b(i)   = b(i) + b(j)*beta; 
151                        }
152                }
153                double dzs = gamma*dz;  // apply scaling to innovations
154                xp   = xp + dzs*b; // multiply by unscaled Kalman gain
155               
156                //cout << "Ub: " << U << endl;
157                //cout << "Db: " << D << endl <<endl;
158               
159        }
160       
161        Ch = U*diag(sqrt(D));
162
163        cout << "bef Carlson: " << imat(Chf,5,5) << endl;
164       
165        carlson(difz,xf, Chf, Rf, 2, 5);
166       
167        cout << endl<<"after Carlson" <<endl;
168        cout << "x: "<< round_i(xp*multip) <<endl;
169        cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
170
171        {
172                imat Chcmp(Chf,5,5);
173                cout << "Delat Ch: " << round_i(Ch*multip-Chcmp) << endl;
174                cout << "Delat Ch: " << Chcmp << endl;
175        }
176       
177return 0;       
178}
Note: See TracBrowser for help on using the browser.