root/applications/pmsm/simulator_zdenek/ekf_example/Zdrojove kody_final/DSP/Choleski/matrix_vs.c @ 1263

/************************************
        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_vs.h"
#include "qmath.h"

#define HIGH_PRECISION  1

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++;
            }
        }
    }
}

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, *Ch_iyj;


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

                Ch_iyj=Ch+iy*dimx;

        for (j=0;j<dimx;j++)
        {
            sigma=*Ch_iyj++;
            beta=alpha;
//            alpha+=((long)sigma*sigma)>>15;
            alpha=(((long)alpha<<15)+(long)sigma*sigma)>>15;                            // vyssi presnost
            gamma= qsqrt(((long)alpha*beta));            // predelat v DSP
            w[j]=0;

                        Ch_ij=Ch+j;
                        w_i=w;

            for (i=0;i<=j;i++) 
            {
                                tau=*Ch_ij;
                                tmp_long=((long)beta**Ch_ij -(long)sigma**w_i)/gamma;
                        
/*                              *Ch_ij=tmp_long;
                                if (tmp_long>32767) 
                                        *Ch_ij=32767;
                                if (tmp_long<-32768)
                                        *Ch_ij=-32768;  /**/
                                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_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++)
                {
                    if (*A_ij!=0)
                    {
                        tmp_long=(long)*Ch_ii**Ch_ii+(long)*A_ij**A_ij;

                        rho=qsqrt(tmp_long);
                        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++)
        {
                if (*Ch_ij!=0)
                {
                   tmp_long=(long)*Ch_ii**Ch_ii+(long)*Ch_ij**Ch_ij;
                   rho=qsqrt(tmp_long);
                   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++;
        }
  }
}