[1202] | 1 | #include "ekf_example/matrix_vs.h" |
---|
| 2 | #include "ekf_example/ekf_obj.h" |
---|
| 3 | int main(){ |
---|
[1245] | 4 | int16 i; |
---|
[1202] | 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); |
---|
[1245] | 15 | for (int16 i=0; i<5;i++) { |
---|
| 16 | for (int16 j=i+1; j<5;j++) U(i,j)=2*randu(1)(0)-1; |
---|
[1202] | 17 | } |
---|
| 18 | mat Q = 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 | |
---|
[1245] | 24 | int16 PSI[25]; |
---|
| 25 | int16 PSIU[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}; |
---|
| 26 | int16 Uf[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}; |
---|
| 27 | int16 Gf[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 | int16 Df[5]={0,0,0,0,0}; |
---|
| 29 | int16 Dfold[5]={0,0,0,0,0}; |
---|
[1202] | 30 | |
---|
[1245] | 31 | int16 multip=1<<15; |
---|
[1202] | 32 | |
---|
| 33 | /////////// COPY |
---|
| 34 | imat Af=round_i(A*multip); |
---|
[1245] | 35 | mat_to_int16(Af, PSI); |
---|
| 36 | mat_to_int16(round_i(U*multip),Uf); |
---|
| 37 | vec_to_int16(round_i(D*multip), Df); |
---|
| 38 | int16 Qf[25]; |
---|
| 39 | mat_to_int16(round_i(Q*multip), Qf); |
---|
| 40 | int16 Rf[2]; |
---|
| 41 | vec_to_int16(round_i(R*multip), Rf); |
---|
[1202] | 42 | |
---|
| 43 | |
---|
| 44 | ////////////// Test mmultAU |
---|
| 45 | mmultAU(PSI,Uf,PSIU,5,5); |
---|
| 46 | |
---|
| 47 | mat PhiU =A*U; |
---|
| 48 | /* cout << "A*U" << round_i(PhiU*multip) <<endl; |
---|
| 49 | cout << "PSIU: "; for (i=0; i<25;i++) cout << PSIU[i] << ","; cout <<endl;*/ |
---|
| 50 | |
---|
| 51 | imat PUcmp(PSIU,5,5); |
---|
[1228] | 52 | cout << "Delta PSI: " << round_i(PhiU*multip-(1<<(15-qAU))*PUcmp) <<endl; |
---|
[1202] | 53 | |
---|
[1245] | 54 | mat_to_int16(round_i(PhiU*multip/(1<<(15-qAU))),PSIU); //<< make is same |
---|
[1202] | 55 | |
---|
| 56 | /////////// Test Thorton: |
---|
[1245] | 57 | int16 dim=5;double sigma; int16 j,k; |
---|
[1202] | 58 | vec Din = D; |
---|
| 59 | mat G=eye(5); |
---|
| 60 | for (i=dim-1; i>=0;i--){ |
---|
| 61 | sigma = 0.0; |
---|
| 62 | for (j=0; j<dim; j++) { |
---|
| 63 | sigma += PhiU(i,j)*PhiU(i,j) *Din(j); |
---|
| 64 | sigma += G(i,j)*G(i,j) * Q(j,j); |
---|
| 65 | } |
---|
| 66 | D(i) = sigma; |
---|
| 67 | for (j=0;j<i;j++){ |
---|
| 68 | sigma = 0.0; |
---|
| 69 | for (k=0;k<dim;k++){ |
---|
| 70 | sigma += PhiU(i,k)*Din(k)*PhiU(j,k); |
---|
| 71 | } |
---|
| 72 | for (k=0;k<dim;k++){ |
---|
| 73 | sigma += G(i,k)*Q(k,k)*G(j,k); |
---|
| 74 | } |
---|
| 75 | // |
---|
| 76 | U(j,i) = sigma/D(i); |
---|
| 77 | for (k=0;k<dim;k++){ |
---|
| 78 | PhiU(j,k) = PhiU(j,k) - U(j,i)*PhiU(i,k); |
---|
| 79 | } |
---|
| 80 | for (k=0;k<dim;k++){ |
---|
| 81 | G(j,k) = G(j,k) - U(j,i)*G(i,k); |
---|
| 82 | } |
---|
| 83 | } |
---|
| 84 | } |
---|
| 85 | |
---|
| 86 | |
---|
[1225] | 87 | //thorton_fast(Uf,Df,PSIU,Qf,Gf,Dfold,5); |
---|
| 88 | thorton(Uf,Df,PSIU,Qf,Gf,Dfold,5); |
---|
[1202] | 89 | |
---|
| 90 | /////// disp |
---|
| 91 | |
---|
| 92 | cout << endl<<"after thorton " <<endl; |
---|
| 93 | // cout << "U: " << round_i(U*multip) << endl; |
---|
| 94 | // cout << "D: " << round_i(D*multip) << endl; |
---|
| 95 | // cout << "Uf: "; for (i=0; i<25;i++) {cout << Uf[i] << ","; ((i+1)%5==0)? cout << endl:cout<<""; } |
---|
| 96 | // cout << "Df: "; for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl; |
---|
| 97 | |
---|
| 98 | imat Ucmp(Uf,5,5); |
---|
| 99 | ivec Dcmp(Df,5); |
---|
| 100 | cout << "Delta U: " << round_i(U*multip-Ucmp) << endl; |
---|
| 101 | cout << "Delat D: " << round_i(D*multip-Dcmp) << endl; |
---|
| 102 | |
---|
| 103 | // synchronize |
---|
[1245] | 104 | mat_to_int16(round_i(U*multip),Uf); |
---|
| 105 | vec_to_int16(round_i(D*multip), Df); |
---|
[1202] | 106 | |
---|
| 107 | |
---|
| 108 | vec ydif = 2*randu(2)-1; |
---|
| 109 | vec xp = 2*randu(5)-1; |
---|
| 110 | |
---|
[1245] | 111 | int16 difz[2]; |
---|
| 112 | vec_to_int16(round_i(ydif*multip), difz); |
---|
| 113 | int16 xf[5]; |
---|
| 114 | vec_to_int16(round_i(xp*multip), xf); |
---|
[1202] | 115 | |
---|
| 116 | cout << "x: "<< round_i(xp*multip) <<endl; |
---|
| 117 | cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl; |
---|
| 118 | |
---|
| 119 | |
---|
[1245] | 120 | int16 xf_old[5]; |
---|
| 121 | vec_to_int16(ivec(xf,5),xf_old); |
---|
[1202] | 122 | |
---|
| 123 | /////// Test bierman |
---|
| 124 | double dz,alpha,gamma,beta,lambda; |
---|
| 125 | vec a; |
---|
| 126 | vec b; |
---|
| 127 | mat C = zeros(2,5); |
---|
| 128 | C(0,0)=1.0;C(1,1)=1.0; |
---|
[1245] | 129 | for (int16 iy=0; iy<2; iy++){ |
---|
[1202] | 130 | a = U.T()*C.get_row(iy); // a is not modified, but |
---|
| 131 | b = elem_mult(D,a); // b is modified to become unscaled Kalman gain. |
---|
| 132 | dz = ydif(iy); |
---|
| 133 | |
---|
| 134 | alpha = R(iy); |
---|
| 135 | gamma = 1/alpha; |
---|
| 136 | for (j=0;j<dim;j++){ |
---|
| 137 | beta = alpha; |
---|
| 138 | alpha = alpha + a(j)*b(j); |
---|
| 139 | lambda = -a(j)*gamma; |
---|
| 140 | gamma = 1.0/alpha; |
---|
| 141 | D(j) = beta*gamma*D(j); |
---|
| 142 | |
---|
| 143 | // cout << "a: " << alpha << "g: " << gamma << endl; |
---|
| 144 | for (i=0;i<j;i++){ |
---|
| 145 | beta = U(i,j); |
---|
| 146 | U(i,j) = beta + b(i)*lambda; |
---|
| 147 | b(i) = b(i) + b(j)*beta; |
---|
| 148 | } |
---|
| 149 | } |
---|
| 150 | double dzs = gamma*dz; // apply scaling to innovations |
---|
| 151 | xp = xp + dzs*b; // multiply by unscaled Kalman gain |
---|
| 152 | |
---|
| 153 | //cout << "Ub: " << U << endl; |
---|
| 154 | //cout << "Db: " << D << endl <<endl; |
---|
| 155 | |
---|
| 156 | } |
---|
| 157 | |
---|
| 158 | bierman_fast(difz,xf, Uf, Df, Rf, 2, 5); |
---|
| 159 | cout << endl<<"after Bierman" <<endl; |
---|
| 160 | /* cout << "U: " << round_i(U*multip) << endl; |
---|
| 161 | cout << "D: " << round_i(D*multip) << endl; |
---|
| 162 | cout << "Uf: "; for (i=0; i<25;i++) {cout << Uf[i] << ","; ((i+1)%5==0)? cout << endl:cout<<""; } |
---|
| 163 | cout << "Df: "; for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;*/ |
---|
| 164 | cout << "x: "<< round_i(xp*multip) <<endl; |
---|
| 165 | cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl; |
---|
| 166 | |
---|
| 167 | { |
---|
| 168 | imat Ucmp(Uf,5,5); |
---|
| 169 | ivec Dcmp(Df,5); |
---|
| 170 | cout << "Delta U: " << round_i(U*multip-Ucmp) << endl; |
---|
| 171 | cout << "Delat D: " << round_i(D*multip-Dcmp) << endl; |
---|
| 172 | } |
---|
| 173 | |
---|
| 174 | return 0; |
---|
| 175 | } |
---|