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

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

        V. Smidl

Rev. 30.8.2010

30.8.2010      Prvni verze

*************************************/
#include "fixed.h"
#include "stdio.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;
  int *m2pom;

  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)(*m1++)**m2pom;
        m2pom+=columns;
      }
      // add the missing A(i,j)
                tmp_sum +=(long)(*m1)<<15; // no need to shift
                m1-=(j); // shift back to first element

      // saturation effect
      tmp_sum=tmp_sum>>15;
      if (tmp_sum>32767) tmp_sum=32767;
      if (tmp_sum<-32768) tmp_sum=-32768;

      *result++=tmp_sum;

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

void UDprt(int *U, int *D){
        return;
        for (int i=0;i<5;i++){
                for (int j=0;j<5;j++){
                        printf("%d,",U[i*5+j]);
                }
                printf("\n");
        }
        for (int i=0;i<5;i++){printf("%d,",D[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 q15
        for (i=rows-1; i>=0;i--){ // check i==0 at the END!
                sigma = 0;
                
                for (j=0;j<rows; j++){
                        sigma += (((long)PSIU[i+j*rows]*PSIU[i+j*rows])>>15)*(Dold[i]);
                }
                sigma += Q[i+i*rows];
                for (j=i+1;j<rows; j++){
                        sigma += (((long)G[i+j*rows]*G[i+j*rows])>>13)*Q[j+j*rows];
                }
                
                *(D+i)=sigma>>15;
                if (D[i]==0) D[i]=1;

/*              printf("d=sig\n");
                UDprt(U,D);
                UDprt(G,Dold);*/
                
                for (j=0;j<i;j++){
//                      printf("\n%d,%d\n",i,j);
                        sigma =0;
                        for (k=0;k<rows;k++){
                                sigma += ((((long)PSIU[i*rows+k])*PSIU[j*rows+k])>>15)*Dold[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;

/*                      printf("U=sig/D\n");
                        UDprt(U,D);
                        UDprt(G,Dold);*/
                        
                        for (k=0;k<rows;k++){ 
                                PSIU[j*rows+k] -= ((long)U[j*rows+i]*PSIU[i*rows+k])>>15; 
                        }
                        for (k=0;k<rows;k++){ 
                                G[j*rows+k] -=  ((long)U[j*rows+i]*G[i*rows+k])>>15; 
                        }
                        
/*                      printf("end\n");
                        UDprt(U,D);
                        UDprt(G,Dold);
                        printf("\n");   */
                }
                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];
        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++)
                        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; 
                        alpha  += ((long)(a[j])*b[j]>>15); 
                        lambda = -(a[j])*gamma>>15; 
                        gamma  = (1<<15)/alpha; // in q15 now
                        int mpl=((long)beta*gamma); // no-shift, max_gamma=2^15
                        D[j] = (long)mpl*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 = (gamma*(difz[iy]))>>15;  // 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;
                
        }
        
}