Changeset 1179 for applications/pmsm

Timestamp:

09/02/10 23:21:43 (14 years ago)

Author:

smidl

Message:

fast versions of thorton and bierman

Location:

applications/pmsm/simulator_zdenek/ekf_example

Files:

• ekf_obj.cpp (modified) (1 diff)
• ekf_obj.h (modified) (2 diffs)
• matrix_vs.cpp (modified) (1 diff)
• matrix_vs.h (modified) (1 diff)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -443 +443 @@
         UI::get(log_level, set, "log_level", UI::optional);
 }
+
+
+void EKFfixedUD::bayes(const itpp::vec& yt, const itpp::vec& ut)
+{
+ekf(ut[0],ut[1],yt[0],yt[1]);
+}
+
+
+void EKFfixedUD::ekf(double ux, double uy, double isxd, double isyd)
+{
+        // vypocet napeti v systemu (x,y)
+        int uf[2];
+        uf[0]=prevod(ux/Uref,Qm);
+        uf[1]=prevod(uy/Uref,Qm);
+        
+        int Y_mes[2];
+        // zadani mereni
+        Y_mes[0]=prevod(isxd/Iref,Qm);
+        Y_mes[1]=prevod(isyd/Iref,Qm);
+        
+        ////// vlastni rutina EKF -- /////////////////////////
+        int t_sin,t_cos, tmp;
+        
+        // implementace v PC
+        t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
+        t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
+        
+        tmp=((long)cB*x_est[2])>>15;
+        x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*uf[0])>>15;
+        x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*uf[1])>>15;
+        x_est[2]=x_est[2];
+        x_est[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
+        
+        //void EKFfixed::update_psi(void)
+        {               
+                PSI[2]=((long)cB*t_sin)>>15;
+                tmp=((long)cH*x_est[2])>>15;
+                PSI[3]=((long)tmp*t_cos)>>15;
+                PSI[6]=-((long)cB*t_cos)>>15;
+                PSI[7]=((long)tmp*t_sin)>>15;
+        }
+        
+        ///////// end of copy ///////////////
+        mmultAU(PSI,Uf,PSIU,4,4);
+        //thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
+        thorton(Uf,Df,PSIU,Q,G,Dfold,4);
+        
+        int difz[2];
+        difz[0]=Y_mes[0]-x_est[0];
+        difz[1]=Y_mes[1]-x_est[1];
+        //bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx );
+        int dR[2];dR[0]=R[0];dR[1]=R[3];
+        bierman_fast(difz,x_est,Uf,Df,dR,2,4);
+        
+        // navrat estimovanych hodnot regulatoru
+        vec& mu = E._mu();
+        (mu)(0)=zprevod(x_est[0],Qm)*Iref;
+        (mu)(1)=zprevod(x_est[1],Qm)*Iref;
+        (mu)(2)=zprevod(x_est[2],Qm)*Wref;
+        (mu)(3)=zprevod(x_est[3],15)*Thetaref;
+        
+//      mat T=diag(concat(vec_2(Iref,Iref), vec_2(Wref,Thetaref)));
+}
+
+void EKFfixedUD::init_ekf(double Tv)
+{
+        // Tuning of matrix Q
+        Q[0]=prevod(.01,15);       // 0.05
+        Q[5]=Q[0];
+        Q[10]=prevod(0.0001,15);      // 1e-3
+        Q[15]=prevod(0.0001,15);      // 1e-3
+
+        Uf[0]=0x7FFF;       // 0.05
+        Uf[5]=0x7FFF;
+        Uf[10]=0x7FFF;      // 1e-3
+        Uf[15]=0x7FFF;      // 1e-3
+        
+        Df[0]=0x7FFF;
+        Df[1]=0x7FFF;
+        Df[2]=0x7FFF;
+        Df[3]=0x7FFF;
+        
+        // Tuning of matrix R
+        R[0]=prevod(0.05,15);         // 0.05
+        R[3]=R[0];
+        
+        // Motor model parameters 
+        cA=prevod(1-Tv*Rs/Ls,15);
+        cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);
+        cC=prevod(Tv/Ls/Iref*Uref,15);
+        //  cD=prevod(1-Tv*Bf/J,15);
+        //  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15);
+        //  cF=prevod(p*Tv*Mref/J/Wref,15);
+        cG=prevod(Tv*Wref*4/Thetaref,15);
+        cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
+        //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
+        
+        /* Init matrix PSI with permanently constant terms */
+        PSI[0]=cA;
+        PSI[5]=PSI[0];
+        PSI[10]=0x7FFF;
+        PSI[14]=cG;
+        PSI[15]=0x7FFF;
+        
+        //////////////////////// =================
+        ///// TEST thorton vs. thorton_fast
+        
+        int Ut[16]={0x7FFF, 100, 200, 300, 0, 0x7FFF, 500, 600, 0,0,0x7FFF, 800, 0,0,0, 0x7FFF};
+        int Dt[4]={100,200,300,400};
+        int PSIu[16] = {100, 200,300, 400 , 500, 600,700,800, 900,1000,1100,1200, 1300,1400,1500,1600};
+        int Dold[4];
+        
+        thorton(Ut,Dt,PSIu,Q,G,Dold,4);
+        int Ut2[16]={0x7FFF, 100, 200, 300, 0, 0x7FFF, 500, 600, 0,0,0x7FFF, 800, 0,0,0, 0x7FFF};
+        int Dt2[4]={100,200,300,400};
+        int PSIu2[16] = {100, 200,300, 400 , 500, 600,700,800, 900,1000,1100,1200, 1300,1400,1500,1600};
+        thorton_fast(Ut2,Dt2,PSIu2,Q,G,Dold,4);
+        cout<< Q<<endl;
+}
+

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -100 +100 @@
 
 UIREGISTER(EKFfixed);
+
+/*!
+\brief Extended Kalman Filter with UD matrices in fixed point arithmetic
+
+An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
+*/
+class EKFfixedUD : public BM {
+        public:
+                void init_ekf(double Tv);
+                void ekf(double ux, double uy, double isxd, double isyd);
+                                
+                /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
+                int Q[16]; /* matrix [4,4] */
+                int R[4]; /* matrix [2,2] */
+                
+                int x_est[4]; /* estimate and prediction */
+                
+                int PSI[16]; /* matrix [4,4] */
+                int PSIU[16]; /* matrix PIS*U, [4,4] */
+                
+                int Uf[16]; // upper triangular of covariance (inplace)
+                int Df[4];  // diagonal covariance
+                int Dfold[4]; // temp of D
+                int G[16];  // temp for bierman
+                
+                int cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
+                
+                enorm<fsqmat> E;
+                mat Ry;
+                
+        public:
+                //! Default constructor
+                EKFfixedUD ():BM(),E(),Ry(2,2){
+                        int i;
+                        for(i=0;i<16;i++){Q[i]=0;}
+                        for(i=0;i<4;i++){R[i]=0;}
+                        
+                        for(i=0;i<4;i++){x_est[i]=0;}
+                        for(i=0;i<16;i++){Uf[i]=0;}
+                        for(i=0;i<4;i++){Df[i]=0;}
+                        for(i=0;i<16;i++){G[i]=0;}
+                        for(i=0;i<4;i++){Dfold[i]=0;}
+                        
+                        for(i=0;i<16;i++){PSI[i]=0;}
+                        
+                        set_dim(4);
+                        E._mu()=zeros(4);
+                        E._R()=zeros(4,4);
+                        init_ekf(0.000125);
+                };
+                //! Here dt = [yt;ut] of appropriate dimensions
+                void bayes ( const vec &yt, const vec &ut );
+                //!dummy!
+                const epdf& posterior() const {return E;};
+                
+};
+
+UIREGISTER(EKFfixedUD);
 
 //! EKF for comparison of EKF_UD with its fixed-point implementation
@@ -197 +255 @@
 
 
+
+
 #endif // KF_H
 

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -13 +13 @@
 #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
+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
     {
-      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);
-  }
+        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]);
+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*rows+j]*PSIU[i*rows+j])>>15)*(Dold[i]);
+                        printf("-- %d,%d --", PSIU[i*rows+j], Dold[i]);
                 }
-                printf("\n");
-        }
-        for (int i=0;i<5;i++){printf("%d,",D[i]);}
-        printf("\n");
-}
- 
+                sigma += Q[i*rows+i];
+        for (j=i+1;j<rows; j++) {
+            sigma += (((long)G[i*rows+j]*G[i*rows+j])>>13)*Q[j*rows+j];
+                        printf("-- %d,%d --", G[i*rows+j], Q[j*rows+j]);
+                }
+        printf("%d\n",sigma);
+                
+        *(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];
+                   }
+            printf("%d\n",sigma);
+                        for (k=0;k<rows;k++) {
+                sigma += ((((long)G[i*rows+k])*G[j*rows+k])>>13)*Q[k*rows+k];
+            }
+            printf("%d\n",sigma);
+                        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(int x=0;x<16;x++) printf("%d,", PSIU[x]);
+                        printf("\n");
+                        
+            for (k=0;k<rows;k++) {
+                G[j*rows+k] -=  ((long)U[j*rows+i]*G[i*rows+k])>>15;
+            }
+            for(int x=0;x<16;x++) printf("%d,", G[x]);
+                        printf("\n");
+                        
+            /*                  printf("end\n");
+                                UDprt(U,D);
+                                UDprt(G,Dold);
+                                printf("\n");   */
+        }
+        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 +=  (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(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; 
+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 q15
+
+    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;
+           k<rows; k++, PSIU_ik++) {//Dold_i=
+            sigma += (((long)(*PSIU_ik)**PSIU_ik)>>15)*(*Dold_i);
+                        printf("-- %d,%d --", *PSIU_ik, *Dold_i);
                 }
-                *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;
+                sigma += *(Q+i+irows);
+        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);
+                        printf("++ %d,%d ++", *G_ik,  *(Q+j+j*rows));
+                        
+        }
+        printf("%d\n",sigma);
                 
-                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; 
-                        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;
-                
-        }
-        
-}
+        *D_i=sigma>>15;
+        if (*D_i==0) *D_i=1;
+
+
+        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)**PSIU_jk)>>15)**Dold_k;
+            }
+            printf("%d\n",sigma);
+                        
+            for (k=i,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;
+            }
+            printf("%d\n",sigma);
+                        
+                        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(int x=0;x<16;x++) printf("%d,", PSIU[x]);
+                        printf("\n");
+                        
+            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;
+            }
+            for(int x=0;x<16;x++) printf("%d,", G[x]);
+                        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]; // 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++)
+            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;
+
+    }
+
+}

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h

@@ -18 +18 @@
 /* perform Bierman update of UD matrix using difz, R and xp, for size dimx*/
 extern void bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx );
+
+/* perform Thorton update of UD matrix using PSI*U, Q, and temporaries G, Dold, for size dimx*/
+extern void thorton_fast(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
+
+/* perform Bierman update of UD matrix using difz, R and xp, for size dimx*/
+extern void bierman_fast(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx );