root/applications/pmsm/simulator_zdenek/test_UD.cpp

#include "ekf_example/matrix_vs.h"
#include "ekf_example/ekf_obj.h"
int main(){
        int16 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 (int16 i=0; i<5;i++) {
                for (int16 j=i+1; j<5;j++) U(i,j)=2*randu(1)(0)-1;
        }
        mat Q = diag(vec(" 0.2000 0.3000 0.4000 0.5000 0.6"));
        vec R = vec(" 0.2000 0.3000");
        
                vec D = randu(5)*0.4;
                vec xref = ones(5);

                int16 PSI[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};
                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};
                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};
                int16 Df[5]={0,0,0,0,0};
                int16 Dfold[5]={0,0,0,0,0};
                
                int16 multip=1<<15;
                
        /////////// COPY
        imat Af=round_i(A*multip);
        mat_to_int16(Af, PSI);
        mat_to_int16(round_i(U*multip),Uf);             
        vec_to_int16(round_i(D*multip), Df); 
        int16 Qf[25];
        mat_to_int16(round_i(Q*multip), Qf);
        int16 Rf[2];
        vec_to_int16(round_i(R*multip), Rf);
        
        
        ////////////// Test mmultAU
        mmultAU(PSI,Uf,PSIU,5,5);       
        
        mat PhiU =A*U;
        cout << "A*U" << round_i(PhiU*multip) <<endl;
        cout << "PSIU: "; for (i=0; i<25;i++) cout << PSIU[i] << ","; cout <<endl;
        
        Mat<int16> PUcmp(PSIU,5,5);
        imat PUcmpi=PUcmp;
        cout << "Delta PSI: " << round_i(PhiU*multip-(1<<(15-qAU))*PUcmp) <<endl;
        
        mat_to_int16(round_i(PhiU*multip/(1<<(15-qAU))),PSIU); //<< make is same

/////////// Test Thorton:
        int16 dim=5;double sigma; int16 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); 
                        }
                }
        }
        
        
        //thorton_fast(Uf,Df,PSIU,Qf,Gf,Dfold,5);
        thorton(Uf,Df,PSIU,Qf,Gf,Dfold,5);

        /////// disp
        
        cout << endl<<"after thorton " <<endl;
        cout << "U: " << round_i(U*multip) << endl;
        cout << "D: " << round_i(D*multip) << endl;
        cout << "Uf: "; for (i=0; i<25;i++) {cout << Uf[i] << ","; ((i+1)%5==0)? cout << endl:cout<<""; }
        cout << "Df: "; for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;

        Mat<int16> Ucmp(Uf,5,5);
        Vec<int16> Dcmp(Df,5);
        cout << "Delta U: " << round_i(U*multip-Ucmp) << endl;
        cout << "Delat D: " << round_i(D*multip-Dcmp) << endl;
        
        // synchronize
        mat_to_int16(round_i(U*multip),Uf);             
        vec_to_int16(round_i(D*multip), Df); 
        

        vec     ydif = 2*randu(2)-1;
        vec     xp = 2*randu(5)-1;
        
        int16 difz[2];
        vec_to_int16(round_i(ydif*multip), difz);
        int16 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;
        
        
        int16 xf_old[5];
        for (i=0;i<5;i++) xf_old[i]=xf[i];
        
        /////// Test bierman
        double dz,alpha,gamma,beta,lambda;
        vec a;
        vec b;
        mat C = zeros(2,5);
        C(0,0)=.05;C(0,1)=-.01;
        C(1,0)=0.; C(1,1) = .07;
        
        int16 Cf[10];
        imat iC=round_i(C*multip);
        mat_to_int16(iC, Cf);
        
        
        for (int16 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;
                
        }
        
        bierman_fastC(difz,xf, Uf, Df, Cf, Rf, 2, 5);
        cout << endl<<"after Bierman" <<endl;
/*      cout << "U: " << round_i(U*multip) << endl;
        cout << "D: " << round_i(D*multip) << endl;
        cout << "Uf: "; for (i=0; i<25;i++) {cout << Uf[i] << ","; ((i+1)%5==0)? cout << endl:cout<<""; }
        cout << "Df: "; for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;*/
        cout << "x: "<< round_i(xp*multip) <<endl;
        cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;

        {
        Mat<int16> Ucmp(Uf,5,5);
        Vec<int16> Dcmp(Df,5);
        cout << "Delta U: " << round_i(U*multip-Ucmp) << endl;
        cout << "Delat D: " << round_i(D*multip-Dcmp) << endl;
        }
        
return 0;       
}