[1227] | 1 | #include "ekf_example/matrix_vs.h" |
---|
| 2 | #include "ekf_example/ekf_obj.h" |
---|
| 3 | int 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 | |
---|
| 84 | cout << "PSICh: " << imat(PSICh,5,5) << endl; |
---|
| 85 | cout << "Qf: " << imat(Qf,5,5) << endl; |
---|
| 86 | |
---|
| 87 | Ch = U*diag(sqrt(D)); |
---|
| 88 | |
---|
| 89 | |
---|
| 90 | householder(PSICh,Qf,5); |
---|
[1230] | 91 | // givens(PSICh,Qf,5); |
---|
[1227] | 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); |
---|
[1230] | 106 | |
---|
| 107 | D = pow(diag(Ch2),2); |
---|
| 108 | U = sqrt(diag(1./D)); |
---|
| 109 | U = Ch2*U; |
---|
| 110 | |
---|
[1227] | 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); |
---|
[1230] | 166 | |
---|
[1227] | 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 | { |
---|
[1230] | 172 | imat Chcmp(Chf,5,5); |
---|
| 173 | cout << "Delat Ch: " << round_i(Ch*multip-Chcmp) << endl; |
---|
| 174 | cout << "Delat Ch: " << Chcmp << endl; |
---|
[1227] | 175 | } |
---|
| 176 | |
---|
| 177 | return 0; |
---|
| 178 | } |
---|