root/applications/pmsm/simulator_zdenek/ekf_example/Zdrojove kody_final/DSP/Bierman/matrix_vs.c @ 1264

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

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