root/applications/pmsm/simulator_zdenek/ekf_example/Zdrojove kody_final/PC/MATRIX.CPP

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

        V. Smidl, Z. Peroutka

Rev. 28.10.2010   (ZP)

26.10.2010      Prvni verze (VS)

26.10.2010      Upravena chyba v Thorton_fast - spatne shiftovani o vypoctu SIGMA.
27.10.2010      Pokus o odstraneni problemu v Thorton_fast - potize dela omezovani (orezavani) varianci.
28.10.2010      Drobne upravy v kodu.

*************************************/

#include "matrix.h"
#include <math.h>

#define HIGH_PRECISION  0

/* 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

            *respom++=tmp_sum>>15;

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


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 is a row
        a_j=a;
        b_j=b;
        D_j=D;

        for (j=0; j<dimx; j++)
        {
            *b_j=((long)(*D_j)*(*a_j))>>15;
            b_j++;
            D_j++;
            a_j++;
        }

        alpha = R[iy]; //\alpha = R+vDv = R+a*b
        // R in q15, a in q15, b=q15

        a_j=a;
        b_j=b;
        D_j=D;

        for (j=0; j<dimx; j++)
        {
            lambda=alpha;
            if (HIGH_PRECISION == 0)
              alpha = alpha + (((long)(*a_j)*(*b_j))>>15);
            else
              alpha = (((long)alpha<<15) + (long)(*a_j)*(*b_j))>>15;    // xxxx lepsi presnost

            *D_j = ((long)lambda**D_j)/alpha;

            if (*D_j==0) *D_j=1;

            b_i=b;
            U_ij=U+j;

            for (i=0; i<j; i++)
            {
                beta   = *U_ij;
                    if (HIGH_PRECISION == 0)
                                {
                  *U_ij = *U_ij - ((long)(*a_j)*(*b_i))/lambda;
                  *b_i  = *b_i + (((long)beta*(*b_j))>>15);
                                }
                                else
                                {
                  *U_ij = ((long)lambda**U_ij - (long)(*a_j)*(*b_i))/lambda;
                  *b_i  = (((long)*b_i<<15) + (long)beta**b_j)>>15;                                     
                                }
                b_i++;
                U_ij+=dimx;
            }

            a_j++;
            b_j++;
            D_j++;
        }

        x_i=xp;
        b_i=b;

        for (i=0; i<dimx; i++)
        {
          if (HIGH_PRECISION == 0)
            *x_i += ((long)difz[iy]**b_i)/alpha; // multiply by unscaled Kalman gain
          else  
            *x_i = ((long)alpha**x_i + (long)difz[iy]**b_i)/alpha; 

            x_i++;
            b_i++;
        }

        a+=dimx;
    }
}

/**/

// 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 qAU+15!!
    long z;
    long z_tmp;

    Dold_i=Dold;
    D_i=D;

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

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

    Dold_i=Dold+rows-1;
    D_i=D+rows-1;

    for (i=rows-1; 1; i--)
    { // stop if i==0 at the END!
        irows=i*rows;
        sigma = 0;

        PSIU_ik=PSIU+irows;
        Dold_k=Dold;

        for (k=0;k<rows; k++)
        {
            sigma += (((long)(*PSIU_ik)**PSIU_ik)>>(qAU))*(*Dold_k);
            PSIU_ik++;
            Dold_k++;
        }

        sigma += (long)*(Q+i+irows)<<(qAU+qD-15);

        G_ik=G+irows+i+1;

        for (j=i+1; j<rows; j++)
        {
            sigma += ((((long)(*G_ik)**G_ik)>>15)**(Q+j+j*rows))>>(30-qAU-qD);
            G_ik++;
        }

        if (sigma>((long)1<<(15+qAU)))
        {
                   *D_i = 32767;
                } else
        {
           *D_i=sigma>>qAU;
                }

        if (*D_i==0) *D_i=1;

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

            sigma =0;
            PSIU_ik=PSIU+irows;
            PSIU_jk=PSIU+jrows;
            Dold_k=Dold;

            for (k=0;k<rows; k++)
            {

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

            G_ik=G+irows+i;
            G_jk=G+jrows+i;
            Q_kk=Q+i*rows+i;

            for (k=i;k<rows;k++)
            {
                sigma += ((((long)(*G_ik)**G_jk)>>15)**Q_kk)>>(30-qAU-qD);
                G_ik++;
                G_jk++;
                Q_kk+=rows+1;
            }

            z=(sigma/(*D_i))<<(15-qAU); // shift to q15
            if (z>32767) z=32767;
            if (z<-32768) z=-32768;

            U_ji=U+jrows+i;
            *U_ji = z;


            PSIU_ik=PSIU+irows;
            PSIU_jk=PSIU+jrows;

            for (k=0; k<rows;k++)
            {
              if (HIGH_PRECISION == 0)
                *PSIU_jk -= ((long)*U_ji**PSIU_ik)>>15;
                          else
                *PSIU_jk = (((long)(*PSIU_jk)<<15) - (long)*U_ji**PSIU_ik)>>15;

                PSIU_ik++;
                PSIU_jk++;
            }

            G_jk=G+jrows;
            G_ik=G+irows;

            for (k=0;k<rows;k++)
            {
              if (HIGH_PRECISION == 0)
                *G_jk -= ((long)*U_ji**G_ik)>>15;
                          else
                *G_jk = (((long)(*G_jk)<<15) - (long)*U_ji**G_ik)>>15;
                 G_jk++;
                 G_ik++;
            }
        }

        Dold_i--;
        D_i--;
        if (i==0) return;
    }
}

/* square root of 0<a<1 using taylor at 0.5 in q15*/
int16 int_sqrt(int16 x) {
        double xd(double(x)/32768.);
        return (int)((sqrt(xd)+0.5)*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

    int16 tmp;
    if (x>6554) {
        int16 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(int16 *Ch, int16 *Q, unsigned int16 dimx) 
{
    int16 k,j,i;
    int16 alpha,beta;
    int32  sigma; // 2*qCh
    int32  tmp_long;
    int16 B[25];//Q in qCh
    int16 w[5];
    int16 v[5];

    int16 *B_ij, *Q_i, *B_kj, *Ch_kj, *Ch_ij, *w_j, *v_j;

    B_ij=B;
    Q_i=Q;
    // copy Q to B
    for (i=0;i<dimx*dimx;i++)
    {
       *B_ij++=(*Q_i++)>>(15-qCh);
    }

    for (k=dimx-1; k>=0; k--)
    {
        sigma=0;
        B_kj=B+k*dimx+k;
        Ch_kj=Ch+k*dimx;

        for (j=k;j<dimx ;j++)
        {
            sigma+=((long)*B_kj**B_kj);
            B_kj++;
        }

        for (j=0;j<=k;j++)
        {
            sigma+=((long)*Ch_kj**Ch_kj);
            Ch_kj++;
        }

        //alpha in qCh
        alpha = (int)(sqrt((double)sigma)+0.5);   // verze pro PC
//        alpha = qsqrt(sigma);                     // verze pro DSP

        sigma=0;

        w_j=w;
        B_kj=B+k*dimx;

        for (j=0;j<dimx;j++)
        {
            *w_j=*B_kj;
            sigma+=(long)*w_j**w_j;
            w_j++;
            B_kj++;
        }

        v_j=v;
        Ch_kj=Ch+k*dimx;

        for (j=0; j<=k;j++)
        {
            if (j==k) {
                *v_j=*Ch_kj-alpha;
            } else {
                *v_j=*Ch_kj;
            }
            sigma+=(long)*v_j**v_j;

            v_j++;
            Ch_kj++;
        }

        alpha=sigma>>(qCh+1); // alpha = sigma /2;
        if (alpha==0) alpha =1;

        for (i=0;i<=k;i++)
        {
            sigma=0;

            B_ij=B+i*dimx+i;
            w_j=w+i;

            for (j=i;j<dimx;j++)
            {
                sigma+=((long)*B_ij**w_j);

                B_ij++;
                w_j++;
            }

            Ch_ij = Ch + i*dimx;
            v_j=v;

            for (j=0;j<=k;j++)
            {
                sigma+=(long)*Ch_ij**v_j;

                Ch_ij++;
                v_j++;
            }

            sigma = sigma >> 15;               // navrat do Q15
            if (sigma>32767) sigma=32767;


            B_ij=B+i*dimx+i;
            w_j=w+i;

            for (j=i;j<dimx;j++)
            {
                tmp_long=((long)*B_ij*alpha-sigma**w_j)/alpha;
                if (tmp_long>32767)
                   tmp_long=32767;
                if (tmp_long<-32768)
                   tmp_long=-32768;
                *B_ij++=tmp_long;
                w_j++;
            };

            Ch_ij=Ch+i*dimx;
            v_j=v;

            for (j=0;j<=k;j++)
            {
                tmp_long=((long)*Ch_ij*alpha-sigma**v_j)/alpha;
                if (tmp_long>32767)
                   tmp_long=32767;
                if (tmp_long<-32768)
                   tmp_long=-32768;
                *Ch_ij++=tmp_long;
                v_j++;
            }
        }
    }
}


//  Nize uvedene fce jsou dle implementace v DSP
void carlson(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx ) {
    int16 alpha,beta,gamma;
    int16 delta, eta,epsilon,zeta,sigma,tau;
    int16 i,j,iy;
    int16 w[5];
        int32 tmp_long;

        int16 *Ch_ij, *w_i, *x_i;


    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;
            alpha=(((long)alpha<<15)+(long)sigma*sigma)>>15;                            // vyssi presnost
//            gamma= qsqrt(((long)alpha*beta));                          // verze pro DSP
            gamma= (int)(sqrt((double)((long)alpha*beta)));              // verze pro PC

            w[j]=0;

                        Ch_ij=Ch+j;
                        w_i=w;

            for (i=0;i<=j;i++)
            {
//                tau=Ch[i*dimx+j];
                                tau=*Ch_ij;
//                              tmp_long=((long)beta*Ch[i*dimx+j] -(long)sigma*w[i])/gamma;
                                tmp_long=((long)beta**Ch_ij -(long)sigma**w_i)/gamma;

                                if (tmp_long>32767)
                                        tmp_long=32767;
                                if (tmp_long<-32768)
                                        tmp_long=-32768;
                                *Ch_ij=tmp_long;
                        
//                w_i+=((long)tau*sigma)>>15;
                *w_i=(((long)*w_i<<15)+(long)tau*sigma)>>15;

                                w_i++;
                                Ch_ij+=dimx;
            }
        }

                x_i=xp;
                w_i=w;
        for (i=0;i<dimx;i++) {
//            xp[i]+=((long)w[i]*delta)/alpha;
//            *x_i+=((long)*w_i*delta)/alpha;
            *x_i=((long)*x_i*alpha+(long)*w_i*delta)/alpha;             // vyssi presnost
                        x_i++;
                        w_i++;
        }
    }
}

/* Matrix multiply Full matrix by upper diagonal matrix; */
void mmultACh(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns) {
        unsigned int16 i, j, k;
        int32 tmp_sum=0L;
        int16 *m2pom;
        int16 *m1pom=m1;
        int16 *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+=(int32)(*(m1pom++))**m2pom;
                                m2pom+=columns;
                        }
                        m1pom-=(j+1); // shift back to first element

                        *respom++=tmp_sum>>15;
                        
                        tmp_sum=0;
                }
                m1pom+=(columns);
        }
}

void givens(int16 *Ch, int16 *Q, unsigned int16 dimx)
{
        int16 i,j,k;
        int16 rho,s,c,tau;
        int32  tmp_long;

        int16 A[25];//beware

        int16 *A_ij, *Q_i, *Ch_ki, *Ch_kj, *Ch_ii, *Ch_ij, *A_kj;

        A_ij=A;
        Q_i=Q;
        // copy Q to A
        for (i=0;i<dimx*dimx;i++)
        {
//              A[i]=Q[i]>>(15-qCh);
          *A_ij++=(*Q_i++)>>(15-qCh);
        }

        for (i=dimx-1; i>=0; i--)
        {
                Ch_ii=Ch+i*dimx+i;
                A_ij=A+i*dimx;

                for (j=0; j<dimx; j++) 
                {
//                      tmp_long=(long)Ch[i*dimx+i]*Ch[i*dimx+i]+(long)A[i*dimx+j]*A[i*dimx+j];

                        tmp_long=(long)*Ch_ii**Ch_ii+(long)*A_ij**A_ij;

                        if (tmp_long>0)
                        {
//                              rho=qsqrt(tmp_long);                   // verze pro DSP
                                rho=(int)(sqrt((double)tmp_long));     // verze pro PC
                                s=((long)*A_ij<<15)/rho;
                                c=((long)*Ch_ii<<15)/rho;

                                Ch_ki=Ch+i;
                                A_kj=A+j;

                                for (k=0;k<=i; k++)
                                {
                                        tau=((long)c**A_kj-(long)s**Ch_ki)>>15;
                                        *Ch_ki=((long)s**A_kj+(long)c**Ch_ki)>>15;
                                        *A_kj=tau;

                                        Ch_ki+=dimx;
                                        A_kj+=dimx;
                                }
                        }
                        A_ij++;
                }
        
        Ch_ij = Ch+i*dimx;

        for (j=0; j<i; j++)
        {
                tmp_long=(long)*Ch_ii**Ch_ii+(long)*Ch_ij**Ch_ij;
                
                if (tmp_long>0)
                {
//                      rho=qsqrt(tmp_long);                     // verze pro DSP
                        rho=(int)(sqrt((double)tmp_long));       // verze pro PC
                        s=((long)*Ch_ij<<15)/rho;
                        c=((long)*Ch_ii<<15)/rho;
                        
                        Ch_kj = Ch + j;
                        Ch_ki = Ch + i;
                        
                        for (k=0; k<=i; k++)
                        {
                                tau=((long)c**Ch_kj-(long)s**Ch_ki)>>15;
                                *Ch_ki =((long)s**Ch_kj+(long)c**Ch_ki)>>15;
                                *Ch_kj=tau;

                                Ch_kj += dimx;
                                Ch_ki += dimx;
                        }
                }
                Ch_ij++;
        }
  }
}