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

#include "ekf_example/matrix_vs.h"
#include "ekf_example/ekf_obj.h"
int main(){
        int i;
        mat A = 0.99*eye(5);
        A(0,3) = 0.06;
        A(0,2) = 0.01;
        A(1,2) = 0.01;
        A(1,3) = -0.07;
        A(3,2) = 0.0001;

//      RNG_randomize();
        
        mat U=eye(5);
        for (int i=0; i<5;i++) {
                for (int j=i+1; j<5;j++) U(i,j)=2*randu(1)(0)-1;
        }
        mat Q = 0.1*diag(vec(" 0.2000 0.3000 0.4000 0.5000 0.6"));
        vec R = vec(" 0.2000 0.3000");
        
                vec D = randu(5)*0.9;
                vec xref = ones(5);

                mat Ch = U*diag(sqrt(D));
                
                int PSI[25];
                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};
                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};
                int multip=1<<15;
                
        /////////// COPY
        imat Af=round_i(A*multip);
        mat_to_int16(Af, PSI);
        mat_to_int16(round_i(Ch*multip),Chf);           
        int Qf[25];
        mat_to_int16(round_i(sqrt(Q)*multip), Qf);
        int Rf[2];
        vec_to_int16(round_i(R*multip), Rf);
        
        
        ////////////// Test mmultAU
        mmultACh(PSI,Chf,PSICh,5,5);    
        
        mat PhiCh =round(A*32768)*round(Ch*32768)/32768/32768;
/*      cout << "A*U" << round_i(PhiU*multip) <<endl;
        cout << "PSIU: "; for (i=0; i<25;i++) cout << PSIU[i] << ","; cout <<endl;*/
        
        imat PChcmp(PSICh,5,5);
        cout << "Delta PSI: " << round_i(PhiCh*multip-PChcmp) <<endl;
        
        mat_to_int16(round_i(PhiCh*multip),PSICh); //<< make is same

        mat PhiU= A*U;
/////////// Test Thorton:
        int dim=5;double sigma; int j,k; 
        vec Din = D;
        mat G=eye(5);
        for (i=dim-1; i>=0;i--){
                sigma = 0.0; 
                for (j=0; j<dim; j++) {
                        sigma += PhiU(i,j)*PhiU(i,j) *Din(j); 
                        sigma += G(i,j)*G(i,j) * Q(j,j); 
                }
                D(i) = sigma; 
                for (j=0;j<i;j++){ 
                        sigma = 0.0; 
                        for (k=0;k<dim;k++){ 
                                sigma += PhiU(i,k)*Din(k)*PhiU(j,k); 
                        }
                        for (k=0;k<dim;k++){ 
                                sigma += G(i,k)*Q(k,k)*G(j,k); 
                        }
                        //
                        U(j,i) = sigma/D(i); 
                        for (k=0;k<dim;k++){ 
                                PhiU(j,k) = PhiU(j,k) - U(j,i)*PhiU(i,k); 
                        }
                        for (k=0;k<dim;k++){ 
                                G(j,k) = G(j,k) - U(j,i)*G(i,k); 
                        }
                }
        }

cout << "PSICh: " << imat(PSICh,5,5) << endl;
cout << "Qf: " << imat(Qf,5,5) << endl;

Ch = U*diag(sqrt(D));

        
        householder(PSICh,Qf,5);
//      givens(PSICh,Qf,5);

        /////// disp
        
        cout << endl<<"after householder " <<endl;

        imat Chcmp(PSICh,5,5);
        cout << "Ch(UD): " << round_i(Ch*multip) << endl;
        cout << "Ch: " << (Chcmp) << endl;
        cout << "Delta Ch: " << round_i(Ch*multip-Chcmp) << endl;
        
        // synchronize
        vec d=diag(Ch*Ch.T());
        mat Ch2 = 0.9*diag(1./sqrt(d))*Ch;
        mat_to_int16(round_i(Ch2*multip),Chf);          

        D = pow(diag(Ch2),2);
        U = sqrt(diag(1./D));
        U = Ch2*U;
                
        vec     ydif = 2*randu(2)-1;
        vec     xp = 2*randu(5)-1;
        
        int difz[2];
        vec_to_int16(round_i(ydif*multip), difz);
        int xf[5];
        vec_to_int16(round_i(xp*multip), xf);

        cout << "x: "<< round_i(xp*multip) <<endl;
        cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
        
        
        int xf_old[5];
        vec_to_int16(ivec(xf,5),xf_old);
        
        /////// Test bierman
        double dz,alpha,gamma,beta,lambda;
        vec a;
        vec b;
        mat C = zeros(2,5);
        C(0,0)=1.0;C(1,1)=1.0;
        for (int iy=0; iy<2; iy++){
                a     = U.T()*C.get_row(iy);    // a is not modified, but 
                b     = elem_mult(D,a);                          // b is modified to become unscaled Kalman gain. 
                dz    = ydif(iy); 
                
                alpha = R(iy); 
                gamma = 1/alpha; 
                for (j=0;j<dim;j++){
                        beta   = alpha; 
                        alpha  = alpha + a(j)*b(j); 
                        lambda = -a(j)*gamma; 
                        gamma  = 1.0/alpha; 
                        D(j) = beta*gamma*D(j); 
                        
                        //                      cout << "a: " << alpha << "g: " << gamma << endl;
                        for (i=0;i<j;i++){
                                beta   = U(i,j); 
                                U(i,j) = beta + b(i)*lambda; 
                                b(i)   = b(i) + b(j)*beta; 
                        }
                }
                double dzs = gamma*dz;  // apply scaling to innovations 
                xp   = xp + dzs*b; // multiply by unscaled Kalman gain 
                
                //cout << "Ub: " << U << endl;
                //cout << "Db: " << D << endl <<endl;
                
        }
        
        Ch = U*diag(sqrt(D));

        cout << "bef Carlson: " << imat(Chf,5,5) << endl;
        
        carlson(difz,xf, Chf, Rf, 2, 5);
        
        cout << endl<<"after Carlson" <<endl;
        cout << "x: "<< round_i(xp*multip) <<endl;
        cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;

        {
                imat Chcmp(Chf,5,5);
                cout << "Delta Ch: " << round_i(Ch*multip-Chcmp) << endl;
                cout << "Delta Ch: " << Chcmp << endl;
        }
        
return 0;       
}