root/applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp @ 1233

/************************************
        Extended Kalman Filter
        Matrix operations

        V. Smidl

Rev. 30.8.2010

30.8.2010      Prvni verze

*************************************/
#include "fixed.h"
#include "stdio.h"
#include <math.h>

#include "matrix_vs.h"

/* Matrix multiply Full matrix by upper diagonal matrix; */
void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int columns) {
    unsigned int i, j, k;
    long tmp_sum=0L; //in 15+qAU
    int *m2pom;
    int *m1pom=m1;
    int *respom=result;

    for (i=0; i<rows; i++) //rows of result
    {
        for (j=0; j<columns; j++) //columns of result
        {
            m2pom=up+j;//??

            for (k=0; k<j; k++) //inner loop up to "j" - U(j,j)==1;
            {
                tmp_sum+=((long)(*(m1pom++))**m2pom)>>(15-qAU);
                m2pom+=columns;
            }
            // add the missing A(i,j)
            tmp_sum +=(long)(*m1pom)<<qAU; // no need to shift
            m1pom-=(j); // shift back to first element

            // saturation effect
            tmp_sum=tmp_sum>>15;
            if (tmp_sum>32767) {
                                printf("Au - saturated\n");
            }
            if (tmp_sum<-32768) {
                                printf("Au - saturated\n");
            }
//                      printf("Au - saturated\n");

            *respom++=tmp_sum;

            tmp_sum=0;
        }
        m1pom+=(columns);
    }
};

/* Matrix multiply Full matrix by upper diagonal matrix; */
void mmultACh(int *m1, int *up, int *result, unsigned int rows, unsigned int columns) {
    unsigned int i, j, k;
    long tmp_sum=0L;
    int *m2pom;
    int *m1pom=m1;
    int *respom=result;

    for (i=0; i<rows; i++) //rows of result
    {
        for (j=0; j<columns; j++) //columns of result
        {
            m2pom=up+j;//??

            for (k=0; k<=j; k++) //inner loop up to "j" - U(j,j)==1;
            {
                tmp_sum+=(long)(*(m1pom++))**m2pom;
                m2pom+=columns;
            }
            m1pom-=(j+1); // shift back to first element

            // saturation effect
            tmp_sum=tmp_sum>>15;
            if (tmp_sum>32767) {
                if (i!=3) tmp_sum=32767;
            }
            if (tmp_sum<-32768) {
                tmp_sum=-32768;
            }
            //                  printf("Au - saturated\n");

            *respom++=tmp_sum;

            tmp_sum=0;
        }
        m1pom+=(columns);
    }
};

bool DBG=true;

void show(const char name[10], int *I, int n) {
    if (!DBG) return;

    printf("%s: ",name);
    for (int i=0;i<n;i++) {
        printf("%d ",*(I+i));
    }
    printf("\n");
}

// Thorton procedure - Kalman predictive variance in UD
void thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows) {
    unsigned int i,j,k;
    // copy D to Dold
    int *Dold_pom=Dold;
    for (i=0;i<rows;i++) {
        *Dold_pom++=*D++;
    }
    D-=rows; // back to first D

    // initialize G = eye()
    int *G_pom = G;
    *G_pom++=1<<14;
    for (i=0;i<rows-1;i++) {
        // clean elem before diag
        for (j=0; j<rows; j++) {
            *G_pom++=0.0;
        }
        *G_pom++=1<<14;
    }
    // eye created

    long sigma; // in q30!!!!!!
    for (i=rows-1; true;i--) { // check i==0 at the END!
        sigma = 0;

        for (j=0;j<rows; j++) {
            //long s1=(((long)PSIU[i+j*rows]*PSIU[i+j*rows])>>15)*(Dold[i]);
            long s2=((((long)PSIU[i*rows+j]*Dold[j]))>>(2*qAU-15))*PSIU[i*rows+j];
//                      printf("%d - %d\n",s1,s2);
            sigma += s2;
        }
        sigma += Q[i*rows+i]<<15; // Q is in q15
        for (j=i+1;j<rows; j++) {
            sigma += (((long)G[i*rows+j]*G[i*rows+j])>>13)*Q[j*rows+j];
//                      sigma += (((long)G[i+j*rows]*G[i+j*rows])>>13)*Q[j+j*rows];
        }

        if (sigma>16384<<15) sigma = 16384<<15;
        *(D+i)=sigma>>15;
        if (D[i]==0) D[i]=1;
//show("D",D,5);

        for (j=0;j<i;j++) {
//                      printf("\n%d,%d\n",i,j);
            sigma =0;
            for (k=0;k<rows;k++) {
                                int tmp= (((long(PSIU[i*rows+k])*Dold[k]))>>(2*qAU-15));
                                if (tmp>32767) printf("!");
                                sigma += (((long(PSIU[i*rows+k])*Dold[k]))>>(2*qAU-15))*PSIU[j*rows+k];
            }
            for (k=0;k<rows;k++) {
                sigma += ((((long)G[i*rows+k])*G[j*rows+k])>>13)*Q[k*rows+k];
            }
            long z=sigma/D[i]; // shift by 15
            if (z>32767) z=32767;
            if (z<-32768) z=-32768;

            U[j*rows+i] = (int)z;


            for (k=0;k<rows;k++) {
                PSIU[j*rows+k] -= ((long)U[j*rows+i]*PSIU[i*rows+k])>>15; //qAU*q15/q15=qAU
            }

            for (k=0;k<rows;k++) {
                G[j*rows+k] -=  ((long)U[j*rows+i]*G[i*rows+k])>>15;
            }

        }
        //show("U",U,25);
        //show("G",G,25);
        if (i==0) return;
    }
}

void bierman_fast(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) {
    int alpha;
    int beta,lambda;
    int b[5]; // ok even for 4-dim state
    int *a; // in [0,1] -> q15
    unsigned int iy,j,i;

    int *b_j,*b_i;
    int *a_j;
    int *D_j;
    int *U_ij;
    int *x_i;
    a = U; // iyth row of U
    for (iy=0; iy<dimy; iy++, a+=dimx) {
        // a is a row
        for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,b_j++,D_j++,a_j++)
            *b_j=((long)(*D_j)*(*a_j))>>15;

        alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b
        // R in q15, a in q15, b=q15
//              gamma = (1<<15)/alpha; //(in q15)
        //min alpha = R[iy] = 164
        //max gamma = 0.0061 => gamma_ref = q7
        for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,a_j++,b_j++,D_j++) {
            beta   = alpha;
            lambda = -((long)(*a_j)<<15)/beta;
            alpha  += ((long)(*a_j)*(*b_j))>>15;
            D[j] = ((((long)beta<<15)/alpha)*(*D_j))>>15; //gamma is long
            if (*D_j==0) *D_j=1;

            for (i=0,b_i=b,U_ij=U+j; i<j; i++, b_i++,U_ij+=dimx) {
                beta   = *U_ij;
                *U_ij +=  ((long)lambda*(*b_i))>>15;
                *b_i  +=  ((long)beta*(*b_j))>>15;
            }
        }
        int dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations
        // no shift due to gamma
        for (i=0,x_i=xp,b_i=b; i<dimx; i++,x_i++,b_i++) {
            *x_i  += ((long)dzs*(*b_i))>>15; // multiply by unscaled Kalman gain
        }

        //cout << "Ub: " << U << endl;
        //cout << "Db: " << D << endl <<endl;

    }

}

// Thorton procedure - Kalman predictive variance in UD
void thorton_fast(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows) {
    unsigned int i,j,k;
    // copy D to Dold
    int *Dold_i,*Dold_k;
    int *D_i;
    int *PSIU_ij,*PSIU_ik,*PSIU_jk;
    int *Q_jj,*Q_ii,*Q_kk;
    int *U_ji;
    int *G_ik,*G_jk;
    int irows,jrows;
    long sigma; // in q30!!

    for (i=0,Dold_i=Dold,D_i=D;i<rows;i++,Dold_i++,D_i++) {
        *Dold_i=*D_i;
    }

    // initialize G = eye()
    G_ik= G;
    *G_ik++=1<<14;
    for (i=0;i<rows-1;i++) {
        // clean elem before diag
        for (k=0; k<rows; k++) {
            *G_ik++=0;
        }
        *G_ik++=1<<14;
    }
    // eye created

    for (i=rows-1, Dold_i=Dold+i, D_i=D+i;
            true; i--, Dold_i--,D_i--) { // stop if i==0 at the END!
        irows=i*rows;
        sigma = 0;
        for (k=0, PSIU_ik=PSIU+irows,Dold_k=Dold;
                k<rows; k++, PSIU_ik++,Dold_k++) {
                sigma += (((long)(*PSIU_ik)**Dold_k)>>(qAU))*(*PSIU_ik)<<(15-qAU);
        }
        sigma += *(Q+i+irows)<<15;
        for (j=i+1, G_ik=G+irows+i+1; j<rows; j++,G_ik++) {
            sigma += (((long)(*G_ik)**G_ik)>>13)**(Q+j+j*rows);

        }
        
        *D_i=sigma>>15;
        if (*D_i==0) *D_i=1;
                if (*D_i>16384) *D_i = 16384;
                

        for (j=0;j<i;j++) {
            jrows = j*rows;

            sigma =0;
            for (k=0, PSIU_ik=PSIU+irows, PSIU_jk=PSIU+jrows, Dold_k=Dold;
                    k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) {

                                sigma += (((long(*PSIU_ik)**Dold_k)>>qAU)**PSIU_jk)<<(15-qAU);
            }

            for (k=i+1,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k;
                    k<rows;k++,G_ik++,G_jk++,Q_kk+=rows+1) {
                sigma += ((((long)*G_ik)**G_jk)>>13)**Q_kk;
            }

            long z=sigma/(*D_i); // shift by 15
            if (z>32767) z=32767;
            if (z<-32768) z=-32768;

            U_ji=U+jrows+i;
            *U_ji = (int)z;


            for (k=0,PSIU_ik=PSIU+irows,PSIU_jk=PSIU+jrows;
                    k<rows;k++,PSIU_ik++,PSIU_jk++) {
                *PSIU_jk -= ((long)*U_ji**PSIU_ik)>>15;
            }

            for (k=0,G_jk=G+jrows,G_ik=G+irows;
                    k<rows;k++, G_jk++, G_ik++) {
                *G_jk -=  ((long)*U_ji**G_ik)>>15;
            }

        }
        if (i==0) return;
    }
}

void bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) {
    long alpha;
    long gamma,beta,lambda;
    int b[5]; // ok even for 4-dim state
    int *a; // in [0,1] -> q15
    unsigned int iy,j,i;

    for (iy=0; iy<dimy; iy++) {
        // a is a row
        a = U+iy*dimx; // iyth row of U
        for (j=0;j<dimx;j++) {
            (j<iy)? b[j]=0: (j==iy)? b[j]=D[j] : b[j]=((long)D[j]*a[j])>>15;
        }

        alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b
        // R in q15, a in q15, b=q15
//              gamma = (1<<15)/alpha; //(in q15)
        //min alpha = R[iy] = 164
        //max gamma = 0.0061 => gamma_ref = q7
        for (j=0;j<dimx;j++) {
            beta   = alpha;
            lambda = -(long(a[j])<<15)/beta;
            alpha  += ((long(a[j])*b[j])>>15);
            D[j] = (((long(beta)<<15)/alpha)*D[j])>>15; //gamma is long
            if (D[j]==0) D[j]=1;

            //                  cout << "a: " << alpha << "g: " << gamma << endl;
            for (i=0;i<j;i++) {
                beta   = U[i*dimx+j];
                U[i*dimx+j] +=  (lambda*b[i])>>15;
                b[i]   +=  (beta*b[j])>>15;
            }
        }
        int dzs = (long(difz[iy])<<15)/alpha;  // apply scaling to innovations
        // no shift due to gamma
        for (i=0; i<dimx; i++) {
            xp[i]  += (long(dzs*b[i]))>>15; // multiply by unscaled Kalman gain
        }

        //cout << "Ub: " << U << endl;
        //cout << "Db: " << D << endl <<endl;

    }

}

/* square root of 0<a<1 using taylor at 0.5 in q15*/
int int_sqrt(int x) {
        double xd(double(x)/32768.);
        return round(sqrt(xd)*32768);
        
    //sqrt(x) == 1/2*2^(1/2)+1/2*2^(1/2)*(x-1/2)-1/4*2^(1/2)*(x-1/2)^2
    //         = k1 + k1*(x-0.5) - k2*(x-0.5)(x-0.5);
#define k1 23170 //0.5*sqrt(2)*32768
#define k2 11585 //0.25*sqrt(2)*32768

    int tmp;
    if (x>6554) {
        int xm05=x-16384;
        tmp = ((long)k1*xm05)>>15;
        tmp-=(((long(k2)*xm05)>>15)*xm05)>>15;
        tmp +=k1;
    } else {
        tmp = 4*x;
        tmp-=long(8*x)*x>>15;
    }
    return tmp;
}

void householder(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx) {
    int k,j,i;
    int sigma,alpha,beta;
    int B[25];//beware
    int w[5];
    int v[5];

    // copy Q to B
    for (i=0;i<dimx*dimx;i++) {
        B[i]=Q[i];
    }

    for (k=dimx-1; k>=0; k--) {
        sigma=0;
        for (j=0;j<dimx;j++) {
            sigma+=(long(B[k*dimx+j])*B[k*dimx+j])>>15;
        }
        for (j=0;j<=k;j++) {
            sigma+=(long(Ch[k*dimx+j])*Ch[k*dimx+j])>>15;
        }
        /*               double sigf=double(sigma)/(1<<15);
                         double alpf = sqrt(sigf);*/
                if (sigma>16384) sigma=16384;
        alpha=int_sqrt(sigma);
        // alpha = alpf*(1<<15);
        //
        sigma=0;
        for (j=0;j<dimx;j++) {
            w[j]=B[k*dimx+j];
            sigma+=(long(w[j])*w[j])>>15;
        }
        for (j=0; j<=k;j++) {
            if (j==k) {
                v[j]=Ch[k*dimx+j]-alpha;
            } else {
                v[j]=Ch[k*dimx+j];
            }
            sigma+=(long(v[j])*v[j])>>15;
        }
        alpha=sigma>>1;
                if (alpha==0) alpha =1;
        for (i=0;i<=k;i++) {
            sigma=0;
            for (j=0;j<dimx;j++) {
                sigma+=(long(B[i*dimx+j])*w[j])>>15;
            }
            for (j=0;j<=k;j++) {
                sigma+=(long(Ch[i*dimx+j])*v[j])>>15;
            }
            for (j=0;j<dimx;j++) {
                B[i*dimx+j]-=(long(sigma)*w[j]/alpha);
            };
            for (j=0;j<=k;j++) {
                Ch[i*dimx+j]-=(long(sigma)*v[j]/alpha);
            }
        }
    }

}

void carlson(int *difz, int *xp, int *Ch, int *R, unsigned int dimy, unsigned int dimx ) {
    int alpha,beta,gamma;
    int delta, eta,epsilon,zeta,sigma,tau;
    int i,j,iy;
    int w[5];

    for (iy=0; iy<dimy; iy++) {
        alpha=R[iy];
        delta = difz[iy];

        for (j=0;j<dimx;j++) {
            sigma=Ch[iy*dimx+j];
            beta=alpha;
            alpha+=(long(sigma)*sigma)>>15;
//                      double ab=(double)alpha*beta/32768./32768.;
//                      double s_ab=sqrt(ab);
            gamma=int_sqrt(((long)alpha*beta)>>15);
            //gamma = round(s_ab*(1<<15));
            eta=(long (beta)<<15) / gamma;
            //zeta=(long(sigma)<<15)/ gamma;
            w[j]=0;
            for (i=0;i<=j;i++) {
                tau=Ch[i*dimx+j];
                Ch[i*dimx+j]=((long(eta)*Ch[i*dimx+j])>>15) -(long(sigma)*w[i])/gamma;
                w[i]+=(long(tau)*sigma)>>15;
            }
        }

        //epsilon=(long(difz)<<15) / (alpha); // q15*q13/q13 = q15
        for (i=0;i<dimx;i++) {
            xp[i]+=(long(w[i])*delta)/alpha;
        }
    }
}

/* perform Householder update of Ch matrix using PSI*Ch , Q, */
extern void givens(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx){
        int i,j,k;
        int rho,s,c,tau;
        
        int A[25];//beware
        // copy Q to A
        for (i=0;i<dimx*dimx;i++) {
                A[i]=Q[i];
        }
        
        
        for (i=dimx-1; i>=0; i--){
                for (j=0; j<dimx; j++) {
                        rho=int_sqrt(((long)Ch[i*dimx+i]*Ch[i*dimx+i]+long(A[i*dimx+j])*A[i*dimx+j])>>15);
                        if (rho==0) break;
                        s=(long(A[i*dimx+j])<<15)/rho;
                        c=(long(Ch[i*dimx+i])<<15)/rho;
                        for (k=0;k<=i; k++){
                                tau=(long(c)*A[k*dimx+j]-long(s)*Ch[k*dimx+i])>>15;
                                Ch[k*dimx +i]=(long(s)*A[k*dimx+j]+long(c)*Ch[k*dimx+i])>>15;
                                A[k*dimx +j]=tau;
                        }
                }
        }
        for (j=0; j<i; j++){
                rho=int_sqrt((long(Ch[i*dimx+i])*Ch[i*dimx+i]+long(Ch[i*dimx+j])*Ch[i*dimx+j])>>15);
                if (rho==0) break; 
                s=(long(Ch[i*dimx+j])<<15)/rho;
                c=(long(Ch[i*dimx+i])<<15)/rho;
                for (k=0; k<=i; k++){
                        tau=(long(c)*Ch[k*dimx+j]-long(s)*Ch[k*dimx+i])>>15;
                        Ch[k*dimx+i]=(long(s)*Ch[k*dimx+j]+long(c)*Ch[k*dimx+i])>>15;
                        Ch[k*dimx+j]=tau;
                }
        }
}