Index: /simulator_zdenek/ekf_example/FIXED.CPP
===================================================================
--- /simulator_zdenek/ekf_example/FIXED.CPP (revision 50)
+++ /simulator_zdenek/ekf_example/FIXED.CPP (revision 50)
@@ -0,0 +1,107 @@
+/* 
+   Operace v pevne radove carce
+   
+   9.10.2005
+*/
+
+
+#include "fixed.h"
+
+long prevod(double x, unsigned int posun);
+// prevod "x" do systemu "Qposun"
+double zprevod(long x, unsigned int posun);
+// prevod "x" ze systemu "Qposun" do DOUBLE
+long nasob(int x, int y,unsigned int posun);
+// (x*y)>>posun
+int deleni15(int x, int y);
+// (x*Q15/y) se saturaci
+int deleni(int x, int y, int posun1, int posun2, int scale, int posun_fin);
+// obecne deleni: x_"Qposun1"*"Qscale"/y_"Qposun2" -> "Q_posun_fin"
+
+  
+long prevod(double x, unsigned int posun)
+{
+  long y,nasobic;
+  int znamenko;
+  
+  nasobic=(long)1<<posun;
+  if (x>=0) znamenko=1;
+  else znamenko=-1;
+  y=(long)(x*nasobic+znamenko*0.5);
+  
+  return y;
+}
+
+double zprevod(long x, unsigned int posun)
+{
+  long nasobic;
+  double y;
+  
+  nasobic=(long)1<<posun;
+  y=(double)x/nasobic;
+  
+  return y;
+}
+
+long nasob(int x, int y,unsigned int posun)
+{
+  long z,zaokrouhl;			// zaokrouhl ... zaokrouhlovani vysledku pricteni 0.5
+  int znamenko;
+  
+  if (posun>0)
+    zaokrouhl=(long)1<<(posun-1);
+  else zaokrouhl=0;
+  z=(long)x*y;
+  if (z>=0) znamenko=1;
+  else znamenko=-1;
+  z=(z+znamenko*zaokrouhl)>>posun;
+  
+  return z;
+}
+
+int deleni15(int x, int y)   // deleni Q15/Q15 se saturaci
+{
+  long z;
+    
+  z=(long)x*32768/y;
+  if (z>32767) z=32767;
+  if (z<-32768) z=-32768;  
+  
+  return (int)z;
+}
+
+int deleni(int x, int y, int posun1, int posun2, int scale, int posun_fin)  
+/*
+x/y    ... podil
+posun1 ... posun x (scale faktor)
+posun2 ... posun y
+scale  ... posun pri deleni - pomocny
+posun_fin ... system, ve kterem ma byt vysledek (scale faktor vysledku)
+*/
+{
+  int z;
+  int bit_posun;
+  long zz, scale_fakt, scale_fin;
+  
+  scale_fakt=(long)1<<scale;
+  bit_posun=(posun1+scale-posun2)-posun_fin;
+  
+  zz=(long)x*scale_fakt/y;
+
+  if (bit_posun>=0)
+    zz=(int)(zz>>bit_posun);
+  else
+  { bit_posun*=-1;
+    zz=(int)(zz<<bit_posun);
+  }
+  
+  // Saturace vystupu
+  if (zz>32767)
+    z=32767;
+  else if (zz<-32768)
+    z=-32768;
+  else
+    z=zz;
+  
+  return z;
+}
Index: /simulator_zdenek/ekf_example/SIMULACE.CPP
===================================================================
--- /simulator_zdenek/ekf_example/SIMULACE.CPP (revision 50)
+++ /simulator_zdenek/ekf_example/SIMULACE.CPP (revision 50)
@@ -0,0 +1,218 @@
+/* Hlavni simulacni program
+
+   Simulace tramvaje s PMSM v plovouci radove carce
+
+   Presne parametry tramvaje Skoda 15T
+
+   31.5.2007
+
+REV.   
+31.5. 2007	Doplnena moznost provozu PWM se 3. harmonickou - konstanta MODULACE nastavuje typ modulace
+25.6. 2007      Vyrazen ss obvod a trolej (uvazovano konst. napeti na troleji)       
+*/
+
+#include <stdio.h>
+#include <math.h>
+#include "pmsm_mod.h"
+#include "pwm.h"
+#include "regulace.h"
+#include "ekf.h"
+
+#define Qm      13
+#define CHYBA_POLOHY	(0./180.*M_PI)	// chyba +/-8 stupnu elektrickych pro senzorovany loziskovy stit
+
+#define MODULACE	1       // 0...sinusova PWM, 1...PWM se 3.harmonickou
+#define REZIM_REGULACE  1       // 0...reg. momentu, 1...reg.rychlosti, 2... Isqw=sqrt(Imax^2-Id^2) - max. moment
+
+static double param[7]={0.28,0.003465,0.1989,0.0,4,1.5,0.04};
+// [Rs, Ls, Fmag, Bf, p, kp, J = 0.04?];
+static double REL[6], REL1[6]={1.,1.,1.,1.,1.,1.};
+// REL = [Ur, Ir, wr, thetar, Mr, Fr];
+static double DT=3e-6;			// mrtve casy
+static double va_char[16]={0,10,50,100,200,300,500,1000, 0,1,1.8,2.4,3.2,3.8,4.8,6.8};    // ubytky
+static unsigned int pocet=8;		// velikost VA-charky
+
+// trolejovy filtr
+//static double Rf=22e-3,Lf=1.8e-3,Cf=10e-3;	// filtr VEKTRA
+static double Rf=5e-3,Lf=1e-3,Cf=1e-3;		// filtr 15T
+// parametry ss obvodu
+static double Ut,Uc,Ucn,It,Iz;
+// modulace a skalarni rizeni
+double fc,fr,fs;
+
+double t, t_end, t_sense;
+double h,hx,h_disp=0;
+double h_reg,h_model,hpom,hpom_model;
+unsigned int h_model_counter,h_model_counter_mez;
+
+static double u[2]={0.,0.};		// format u={Um, beta}
+static double us[2]={0.,0.};	// format us={us_alfa, us_beta}
+double Isd, Isq, Fr, Fs;
+
+static int reg_counter=1, print_counter=0;
+static double Idwf,Iqwf,Urm_max,Ww;
+
+static double rychlost;
+
+static double Isx, Isy;
+static double theta;
+static double Uc_mer;
+
+static int k_rampa=1, k_rampa_tmp=0;
+
+static double tmp_uhel=0;
+
+// EKF - vysledek estimace
+static double ekf_estim[2]={0.,0.};     // w_est, theta_est
+
+FILE *fw;
+
+static void sim_init(void);
+
+static void sim_init(void)
+{
+  // parametry troleje a modulator
+  Ucn=600.;Ut=200.;Uc=Ut;It=0.;Iz=0.;
+  fc=4000.;
+
+  // vzorkovani
+  h_reg=1./2./fc;
+  h_model=h_reg;
+  hpom=h;hpom_model=h;
+
+  // vzorkovani pomoci citacu
+  h_model_counter_mez=(int)(h_model/h);
+  h_model_counter=h_model_counter_mez;
+
+  fs=0.;					// zadani primo v Hz
+//  fs=700.1*(param[4]/60.);	// zadani pres mechanicke otacky fs=n*pp/60
+
+  // pocatecni stav motoru
+  x[0]=0.;x[1]=0.;x[3]=0.;x[4]=0.;x[5]=0.;x[6]=0.;x[7]=0.;x[8]=0.;
+  x[2]=2.*M_PI*fs;  // konstantni rychlost
+  x[3]=M_PI/10.;
+
+//  REL[0]=600; REL[1]=12.0*sqrt(2); REL[2]=2*M_PI*200; REL[3]=3.14159;
+//  REL[4]=17.8;REL[5]=1.0;       // parametry pro v�MSM 6kW!!!
+
+  Isx=0.;Isy=0.;theta=x[3];rychlost=x[2];Uc_mer=Uc;
+}
+
+void main(void)
+{
+  h=1e-6;
+
+  t_end=5.;    // profil Isq 5s, rozbeh 12s, rozbeh +/- 20s, reverzace 12s
+  t_sense=0.0; // 1.
+  t=h;
+
+  sim_init();
+
+//  pwm_full_new_init(h,fc,DT,0,1,1);
+  pwm_full_new_init_3h(h,fc,DT,0,-1,-1);
+  init_pmsm(param, REL1);
+  init_regulace(param,h_reg);
+  init_ekf(h_reg,param);
+
+  fw=fopen("data\\graf1.txt","w");
+
+  Idwf=0.;
+  Ww=0.;
+  Iqwf=0.;
+
+  while (t<=t_end)
+  {
+    pwm_full_new_3h(us,u,&Iz,Ucn,Uc,x,va_char,pocet,MODULACE);		// mod=0 ... sinusova PWM, mod=1 ... 3. harmonicka
+    pmsm_double(*us,*(us+1),h,5);
+
+    if (h_model_counter>=h_model_counter_mez)           // pocatek ISR
+    {
+      if (reg_counter>-1)                                // reg. smycka jede v kazdem ISR => reg_counter>-1, jinak jako u ASM reg_counter>1
+      {
+        if (t>.05)	// 0.05
+        {
+          // skokova zmena Isqw
+//		  Iqwf=30.;
+		  // zmena Isqw po rampa - profil
+/*		  Iqwf+=k_rampa*0.0125;	// 0.055
+		  if (Iqwf>30.) {Iqwf=30.; if (k_rampa_tmp<8000) k_rampa_tmp++; else {k_rampa=-1;k_rampa_tmp=0;}}
+		  if (Iqwf<-30.) {Iqwf=-30.; if (k_rampa_tmp<8000) k_rampa_tmp++; else {k_rampa=1;k_rampa_tmp=0;}}
+		  if ((t>3.) && (Iqwf>0)) Iqwf=0; /* konec profilu Isqw */
+//		  if (Iqwf>0) Iqwf=0;
+
+//          if (Ww<2.*M_PI*275.) Ww+=2.*M_PI*0.0125;
+//		  if (Ww>-2.*M_PI*275.) Ww-=2.*M_PI*0.0125;
+//		  Ww=2.*M_PI*180;
+		// jednoducha reverzace - cely dej cca 11s
+ 	        Ww+=k_rampa*2.*M_PI*0.125/2.;    //1000Hz/s
+		if (Ww>2.*M_PI*250.) {Ww=2.*M_PI*250.; if (k_rampa_tmp<8000) k_rampa_tmp++; else {k_rampa=-1;k_rampa_tmp=0;}}
+		if (Ww<-2.*M_PI*250.) Ww=-2.*M_PI*250.;   /* */
+          // skokova zmena napeti troleje
+//          Ut=500;       // nutno zadat konstantni Isdw pri startu
+        }
+
+        // letmy start s nenulovym momentem
+//        Iqwf=149.*sqrt(2.);
+
+		// pomal�ezd 10s do max. rychlosti 700rpm
+/*     	        Iqwf=149.*sqrt(2.);     // konst. Isqw = Imax
+                x[2]+=2.*M_PI*0.004;
+		if (x[2]>1615) x[2]=1615;
+                if (x[2]<-1615) x[2]=-1615;     /* osetreni pro test rozjezdu do zapornych otacek */
+
+		// rampa rozjezd 5s do max. rychlosti 700rpm a 10s do -700rpm
+/*     	        Iqwf=149.*sqrt(2.);     // konst. Isqw = Imax
+                x[2]+=k_rampa*2.*M_PI*0.008;
+		if (x[2]>1615) {x[2]=1615; if (k_rampa_tmp<24000) k_rampa_tmp++; else {k_rampa=-1;k_rampa_tmp=0;}}
+		if (x[2]<-1615) x[2]=-1615;  /**/
+
+                // KALMAN
+                ekf(ekf_estim,*u,*(u+1),Ucn,Uc_mer,Isx,Isy);
+
+                // vystup z EKF zaveden do regulatoru
+                rychlost=ekf_estim[0]; theta=ekf_estim[1];
+
+		Urm_max=1.0;
+
+		// idealni vzorkovani - test vlivu
+//		Isx=x[0];Isy=x[1];rychlost=x[2];theta=x[3];Uc_mer=Uc;
+
+		if (MODULACE==0)	// sinusova PWM
+		  vektor_regulace(Idwf,Iqwf,Urm_max,Ww,u,Isx,Isy,theta,rychlost,Ucn/2.,Uc_mer,Ucn,REZIM_REGULACE);	// rezim=1 ... reg. rychlosti, rezim=0 ... reg. momentu
+													                // rezim=2 ... Iqw=sqrt(Imax^2-Idw^2)
+		else
+		  vektor_regulace(Idwf,Iqwf,Urm_max,Ww,u,Isx,Isy,theta,rychlost,Ucn/sqrt(3.),Uc_mer,Ucn,REZIM_REGULACE);// rezim=1 ... reg. rychlosti, rezim=0 ... reg. momentu
+														        // rezim=2 ... Iqw=sqrt(Imax^2-Idw^2)
+
+        reg_counter=0;
+        Isx=x[0];Isy=x[1];rychlost=x[2];theta=x[3]+CHYBA_POLOHY;Uc_mer=Uc;       // re��zorkov�
+      }
+      hpom_model=0;
+      h_model_counter=0;
+      reg_counter++;
+    }
+
+    if (t>=t_sense)
+      if (print_counter>199)
+      { fprintf(fw,"%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f \n",t,x[0],x[1],x[6],*ladeni_regulace,x[7],*(ladeni_regulace+2),x[4],x[5],x[3],x[2],Ww,*(ladeni_regulace+1),Urm_max,*(ladeni_regulace+4),*(ladeni_regulace+5),x[10],*(ladeni_regulace+6),Uc, *(ladeni_regulace+7),ladeni_pila,ladeni_ur,*(ladeni_regulace+8),*(ladeni_regulace+9),ekf_estim[0],ekf_estim[1]);
+      // t, Isx, Isy, Isd, Isdw, Isq, Isqw, M, Fs, poloha, rychlost, rychlost_w, Urm, Urm_max,
+      // Fs_model, M_model, zatezny uhel (beta), zatezny uhel vypocteny z napeti (zbeta), Uc
+        print_counter=0;
+      }
+
+    if (h_disp>0.2)
+    { printf("%f \n",t);
+      h_disp=0;
+    }
+
+    t+=h;
+    hpom+=h;
+    hpom_model+=h;
+    h_model_counter++;
+    h_disp+=h;
+    print_counter++;
+  }
+
+//  printf("*** \n\n");
+  fclose(fw);
+ }
Index: /simulator_zdenek/ekf_example/PWM.CPP
===================================================================
--- /simulator_zdenek/ekf_example/PWM.CPP (revision 50)
+++ /simulator_zdenek/ekf_example/PWM.CPP (revision 50)
@@ -0,0 +1,340 @@
+/* modul PWM modulace
+   ubytky a mrtve casy
+
+   shodna verze s PWM_FULL_LC
+
+   31.5. 2007
+
+   VER. zmenena metoda vypoctu pily (nove funkce)
+   31.5.2007	Doplnena PWM se treti harmonickou - fce pwm_full_new_3h(...) - prepinani mezi sinusovou PWM a 3. harmonickou pomoci priznaku mod!
+*/
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include "pwm.h"
+
+void pwm_full_init(double Ucn, double fc, double DT);
+void pwm_full(double *us, double *u, double *Iz,double Ucn, double Uc, double *i, double fc, double *va_char,unsigned int pocet, double h);
+
+void pwm_full_new_init(double h, double fc, double DT, int citac_init, int D_smer, int D_smer2);
+void pwm_full_new(double *us, double *u, double *Iz, double Ucn, double Uc, double *is, double *va_char, unsigned int pocet);
+
+// PWM se treti harmonickou
+void pwm_full_new_init_3h(double h, double fc, double DT, int citac_init, int D_smer, int D_smer2);
+void pwm_full_new_3h(double *us, double *u, double *Iz, double Ucn, double Uc, double *is, double *va_char, unsigned int pocet, unsigned int mod);
+
+static double ubytek(double *va_char, double I, unsigned int pocet);
+
+static double up, up2, dup;
+static int smer, smer2;
+
+static int citac, citac2, citac_PR;
+
+double ladeni_pila, ladeni_ur;
+
+void pwm_full_init(double Ucn, double fc, double DT)
+/*
+Ucn ... jmenovite napeti kondenzatoru v ss obvodu
+fc  ... spinaci frekvence
+h   ... krok simulace
+DT  ... velikost mrtveho casu
+*/
+{
+  smer=-1; smer2=1;
+  up=Ucn/2;
+//  up=0.;
+  up2=up-2*Ucn*fc*DT;
+}
+
+void pwm_full(double *us, double *u, double *Iz,double Ucn, double Uc, double *is, double fc, double *va_char,unsigned int pocet, double h)
+/*
+u    ... pozadovane napeti
+us   ... [usx, usy]...vystupni napeti stridace v (x,y)
+Iz   ... proud v ss obvodu
+Ucn  ... jmenovite nap. v ss obvodu
+Uc   ... skutecne napeti na C
+is   ... [isx, isy]...proud motorem
+va_char ... VA-char vykonovych spinacu
+pocet   ... pocet bodu VA-charky
+h    ... krok simulace
+*/
+{
+  unsigned int i;
+  double iabc[3], ur[3],ustr[3],ua,ub,uc;
+  double dtr[3],dd[3];	      // ubytky na polovodicich
+
+  ladeni_pila=up;
+
+  iabc[0]=*is;
+  iabc[1]=(-*is+sqrt(3.)**(is+1))/2.;
+  iabc[2]=(-*is-sqrt(3.)**(is+1))/2.;
+
+  ur[0]=*u;
+  ur[1]=(-*u+sqrt(3.)**(u+1))/2.;
+  ur[2]=(-*u-sqrt(3.)**(u+1))/2.;
+
+  for (i=0;i<3;i++)
+  { dtr[i]=ubytek(va_char,fabs(iabc[i]),pocet);
+    dd[i]=dtr[i]*.73;
+  }
+
+  // implementace ubytku a mrtvych casu
+  for (i=0;i<3;i++)
+    if (iabc[i]>=0)
+      if ((ur[i]>up) && (ur[i]>up2))
+	ustr[i]=Uc/2-dtr[i];
+      else
+	ustr[i]=-(Uc/2+dd[i]);
+    else
+      if ((ur[i]<up) && (ur[i]<up2))
+	ustr[i]=-(Uc/2-dtr[i]);
+      else
+	ustr[i]=Uc/2+dd[i];
+
+// fazova napeti zateze
+  ua=(2*ustr[0]-ustr[1]-ustr[2])/3;
+  ub=(2*ustr[1]-ustr[0]-ustr[2])/3;
+  uc=(2*ustr[2]-ustr[0]-ustr[1])/3;
+
+// vystupni napeti stridace v (x,y)
+  *us=(2.*ua-ub-uc)/3.;
+  *(us+1)=(ub-uc)/sqrt(3.);
+
+// vypocet pily pro PWM a pily pro mrtve casy
+  dup=2.*Ucn*fc*h;
+  up+=smer*dup;
+  if (up>Ucn/2)
+  { up-=2*dup;
+    smer=-1;
+  }
+  if (up<-Ucn/2)
+  { up+=2.*dup;
+    smer=1;
+  }
+
+  up2+=smer2*dup;
+  if (up2>Ucn/2)
+  { up2-=2.*dup;
+    smer2=-1;
+  }
+  if (up2<-Ucn/2)
+  { up2+=2.*dup;
+    smer2=1;
+  }
+
+  *Iz=(ustr[0]>0)*iabc[0]+(ustr[1]>0)*iabc[1]+(ustr[2]>0)*iabc[2];
+}
+
+static double ubytek(double *va_char, double I, unsigned int pocet)
+{
+  unsigned int ii;
+  double delta_u;
+
+  ii=0;
+  while ((*(va_char+ii)<I) && (ii<(pocet-1)))
+    ii++;
+
+  if (ii==(pocet-1))  // konec va_char - predpoklad nasyceni charky
+    delta_u=*(va_char+ii+pocet);
+  else
+    if (ii==0)
+      delta_u=0;
+    else
+      delta_u=*(va_char+ii-1+pocet)+(I-*(va_char+ii-1))/(*(va_char+ii)-*(va_char+ii-1))*(*(va_char+ii+pocet)-*(va_char+ii-1+pocet));
+
+  return delta_u;
+}
+
+
+void pwm_full_new(double *us, double *u, double *Iz, double Ucn, double Uc, double *is, double *va_char, unsigned int pocet)
+/*
+u    ... pozadovane napeti
+us   ... [usx, usy]...vystupni napeti stridace v (x,y)
+Iz   ... proud v ss obvodu
+Ucn  ... jmenovite nap. v ss obvodu
+Uc   ... skutecne napeti na C
+is   ... [isx, isy]...proud motorem
+va_char ... VA-char vykonovych spinacu
+pocet   ... pocet bodu VA-charky
+t    ... aktualni cas simulace
+*/
+{
+  unsigned int i;
+  double iabc[3], ur[3],ustr[3],ua,ub,uc;
+  double dtr[3],dd[3];	      // ubytky na polovodicich
+
+  // vypocet pily
+  up=(double)citac/citac_PR*Ucn/2.;
+  up2=(double)citac2/citac_PR*Ucn/2.;
+
+  ladeni_pila=up;
+
+  iabc[0]=*is;
+  iabc[1]=(-*is+sqrt(3.)**(is+1))/2.;
+  iabc[2]=(-*is-sqrt(3.)**(is+1))/2.;
+
+  ur[0]=*u;
+  ur[1]=(-*u+sqrt(3.)**(u+1))/2.;
+  ur[2]=(-*u-sqrt(3.)**(u+1))/2.;
+
+  for (i=0;i<3;i++)
+  { dtr[i]=ubytek(va_char,fabs(iabc[i]),pocet);
+    dd[i]=dtr[i]*.73;
+  }
+
+  // implementace ubytku a mrtvych casu
+  for (i=0;i<3;i++)
+    if (iabc[i]>=0)
+      if ((ur[i]>up) && (ur[i]>up2))
+	ustr[i]=Uc/2-dtr[i];
+      else
+	ustr[i]=-(Uc/2+dd[i]);
+    else
+      if ((ur[i]<up) && (ur[i]<up2))
+	ustr[i]=-(Uc/2-dtr[i]);
+      else
+	ustr[i]=Uc/2+dd[i];
+
+// fazova napeti zateze
+  ua=(2*ustr[0]-ustr[1]-ustr[2])/3;
+  ub=(2*ustr[1]-ustr[0]-ustr[2])/3;
+  uc=(2*ustr[2]-ustr[0]-ustr[1])/3;
+
+// vystupni napeti stridace v (x,y)
+  *us=(2.*ua-ub-uc)/3.;
+  *(us+1)=(ub-uc)/sqrt(3.);
+
+  *Iz=(ustr[0]>0)*iabc[0]+(ustr[1]>0)*iabc[1]+(ustr[2]>0)*iabc[2];
+
+  // posun pily ... inkrement citacu
+  if ((citac==citac_PR)||(citac==-citac_PR)) smer*=-1;
+  if ((citac2==citac_PR)||(citac2==-citac_PR)) smer2*=-1;
+  citac+=smer;
+  citac2+=smer2;
+}
+
+void pwm_full_new_init(double h, double fc, double DT, int citac_init, int D_smer, int D_smer2)
+/*
+fc  ... spinaci frekvence
+h   ... krok simulace
+DT  ... velikost mrtveho casu
+citac_init ... pocatecni nastaveni citace
+D_smer, D_smer1 ... pocatecni nastaveni smeru citani
+*/
+{
+  smer=D_smer; smer2=D_smer2;
+  citac=citac_init;
+  citac2=citac_init-(DT/h);
+  citac_PR=1./4./fc/h;
+}
+
+
+void pwm_full_new_init_3h(double h, double fc, double DT, int citac_init, int D_smer, int D_smer2)
+/*
+fc  ... spinaci frekvence
+h   ... krok simulace
+DT  ... velikost mrtveho casu
+citac_init ... pocatecni nastaveni citace
+D_smer, D_smer1 ... pocatecni nastaveni smeru citani
+*/
+{
+  smer=D_smer; smer2=D_smer2;
+  citac=citac_init;
+  citac2=abs(citac_init-(DT/h));
+  citac_PR=1./2./fc/h;
+}
+
+
+void pwm_full_new_3h(double *us, double *u, double *Iz, double Ucn, double Uc, double *is, double *va_char, unsigned int pocet, unsigned int mod)
+/*
+u    ... pozadovane napeti ve formatu u={Um, beta}
+us   ... [usx, usy]...vystupni napeti stridace v (x,y)
+Iz   ... proud v ss obvodu
+Ucn  ... jmenovite nap. v ss obvodu
+Uc   ... skutecne napeti na C
+is   ... [isx, isy]...proud motorem
+va_char ... VA-char vykonovych spinacu
+pocet   ... pocet bodu VA-charky
+t    ... aktualni cas simulace
+mod ... mod= - sinusova PWM; mod=1 - PWM se 3. harmonickou
+*/
+{
+  unsigned int i;
+  double iabc[3], ur[3],ustr[3],ua,ub,uc;
+  double dtr[3],dd[3];	      // ubytky na polovodicich
+  double Um, beta;
+  double U3;				  // korekce 3. harmonickou
+
+
+  Um=*u;
+  beta=*(u+1);
+
+  // vypocet pily
+//  up=(double)citac/citac_PR*Ucn/2.;
+//  up2=(double)citac2/citac_PR*Ucn/2.;
+  // vypocet pily presne jako v procesoru
+  up=((double)citac/citac_PR-0.5)*Ucn;
+  up2=((double)citac2/citac_PR-0.5)*Ucn;
+
+  ladeni_pila=up;
+
+  iabc[0]=*is;
+  iabc[1]=(-*is+sqrt(3.)**(is+1))/2.;
+  iabc[2]=(-*is-sqrt(3.)**(is+1))/2.;
+
+  if (mod==0)	// sinusova PWM
+  {
+    ur[0]=Um*cos(beta);
+    ur[1]=Um*cos(beta-2./3.*M_PI);
+    ur[2]=Um*cos(beta+2./3.*M_PI);
+  }
+  else			// PWM se 3. harmonickou
+  {
+    // dle Karla
+//    U3=0.13*cos(3.*beta);
+	// moje
+	U3=0.17*cos(3.*beta);
+    ur[0]=Um*(cos(beta)-U3);
+    ur[1]=Um*(cos(beta-2./3.*M_PI)-U3);
+    ur[2]=Um*(cos(beta+2./3.*M_PI)-U3);
+  }
+
+  ladeni_ur=ur[0];
+
+  for (i=0;i<3;i++)
+  { dtr[i]=ubytek(va_char,fabs(iabc[i]),pocet);
+    dd[i]=dtr[i]*.73;
+  }
+
+  // implementace ubytku a mrtvych casu
+  for (i=0;i<3;i++)
+    if (iabc[i]>=0)
+      if ((ur[i]>up) && (ur[i]>up2))
+	ustr[i]=Uc/2-dtr[i];
+      else
+	ustr[i]=-(Uc/2+dd[i]);
+    else
+      if ((ur[i]<up) && (ur[i]<up2))
+	ustr[i]=-(Uc/2-dtr[i]);
+      else
+	ustr[i]=Uc/2+dd[i];
+
+// fazova napeti zateze
+  ua=(2.*ustr[0]-ustr[1]-ustr[2])/3.;
+  ub=(2.*ustr[1]-ustr[0]-ustr[2])/3.;
+  uc=(2.*ustr[2]-ustr[0]-ustr[1])/3.;
+
+// vystupni napeti stridace v (x,y)
+  *us=(2.*ua-ub-uc)/3.;
+  *(us+1)=(ub-uc)/sqrt(3.);
+
+  *Iz=(ustr[0]>0)*iabc[0]+(ustr[1]>0)*iabc[1]+(ustr[2]>0)*iabc[2];
+
+  // posun pily ... inkrement citacu
+//  if ((citac==citac_PR)||(citac==-citac_PR)) smer*=-1;
+//  if ((citac2==citac_PR)||(citac2==-citac_PR)) smer2*=-1;
+  if ((citac==citac_PR)||(citac==0)) smer*=-1;
+  if ((citac2==citac_PR)||(citac2==0)) smer2*=-1;
+  citac+=smer;
+  citac2+=smer2;
+}
Index: /simulator_zdenek/ekf_example/reference.h
===================================================================
--- /simulator_zdenek/ekf_example/reference.h (revision 50)
+++ /simulator_zdenek/ekf_example/reference.h (revision 50)
@@ -0,0 +1,42 @@
+/****************************************
+
+	Parametry synchronniho motoru s permanentnimi magnety
+
+	Definice zakl. parametru pro Pn=5.9kW
+
+	Z. Peroutka
+
+Rev.	10.9.2007
+
+Pohon NFR
+
+- cidlo proudu Imax=55A -> 50A
+10.9.2007	zmena reference proudu z Iref=12*sqrt(2) na Iref=30*sqrt(2)
+			zmena reference momentu z Mref=17.8 na Mref=34
+			zmena reference toku z Fref=1 na Fref=Fmag=0.1989
+******************************************/
+
+// pri pouziti Q13 se za vztazne hodnoty voli jmenovite hodnoty
+
+#define Uref		600.0
+#define Iref		(30.0*1.4142)
+#define Wref		(6.283185*200.)
+#define Thetaref	3.141593
+#define	Mref		34.0
+#define Fref		0.1989
+
+#define Qm		13		// veliciny motoru jsou v presnosti Qm
+
+// parametry cidel - maximalni hodnoty (Xmax) ... vyuzivano v modulu ADC
+#define Imax	50.0
+#define Umax	662.5		// 662.5 / 442.5 / 222.5
+
+#define K_I_p	(Imax/Iref)	// zesileni cidel proudu
+#define K_U_p	(Umax/Uref)	// zesileni cidel napeti
+
+#define Q_K_I	14			// system pro zesileni cidel proudu (konst. K_I)
+#define Q_K_U	14			// system pro zesileni cidel napeti (konst. K_U)
+
+// maximalni proud motoru, pri kterem se menic vypina - porucha
+#define Isfault		(48.0/Iref)
+#define Ucmax_fault	(650.0/Uref)
Index: /simulator_zdenek/ekf_example/fixed.h
===================================================================
--- /simulator_zdenek/ekf_example/fixed.h (revision 50)
+++ /simulator_zdenek/ekf_example/fixed.h (revision 50)
@@ -0,0 +1,17 @@
+/* 
+   Operace v pevne radove carce
+   
+   9.10.2005
+*/
+
+
+extern long prevod(double x, unsigned int posun);
+// prevod "x" do systemu "Qposun"
+extern double zprevod(long x, unsigned int posun);
+// prevod "x" ze systemu "Qposun" do DOUBLE
+extern long nasob(int x, int y,unsigned int posun);
+// (x*y)>>posun
+extern int deleni15(int x, int y);
+// (x*Q15/y)
+extern int deleni(int x, int y, int posun1, int posun2, int scale, int posun_fin);
+// obecne deleni: x_"Qposun1"*"Qscale"/y_"Qposun2" -> "Q_posun_fin"
Index: /simulator_zdenek/ekf_example/matrix.cpp
===================================================================
--- /simulator_zdenek/ekf_example/matrix.cpp (revision 50)
+++ /simulator_zdenek/ekf_example/matrix.cpp (revision 50)
@@ -0,0 +1,419 @@
+/************************************
+        Extended Kalman Filter
+        Matrix operations
+
+        Z. Peroutka
+
+Rev. 15.3.2008
+
+15.3. 2008      Kompletni kontrola vypoctu + zamena q15->int a q30->long
+
+*************************************/
+
+#include "matrix.h"
+
+/* Vsechny meze se udavaji ve tvaru (rozmer_matice - 1), abych mel ve FOR konstantni horni mez) */
+
+/* Matrix addition in q15: m1 + m2 = result[rows, columns] */
+void madd(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns);
+
+/* add diagonal matrix m2 to matrix m1 - both in format q15, minrowcol = min(rows, columns) */
+void maddD(int *m1, int *m2, unsigned int minrowcol, unsigned int columns);
+
+/* Matrix substraction in q15: m1 - m2 = result[rows, columns] */
+void msub(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns);
+
+/* Matrix multiply in q15: m1[rows,columnsx]*m2[columnsx,columns] = result[rows,columns] */
+void mmult(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns);
+
+/* Matrix multiplication in q15: m1[rows,columnsx]*(m2[columnsx,columns]transpose) = result[rows,columns] */
+void mmultt(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns);
+
+/* matrix multiplication in q15: sum is in q15 */
+void mmult15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns);
+
+
+/* Matrix multiplication in q15 (sum is in q15): m1[rows,columnsx]*(m2[columns,columnsx]transpose) = result[rows,columns] */
+void mmultt15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns);
+
+
+/* Matrix multiplication - Q15 * Q30 format -> RESULT in Q15 */
+void mmult1530(int *m1, long *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns);
+
+/* Left matrix multiplication with DIAG ones matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns]
+minrowcol=minimum(rows, columnsx) */
+void mmultDl(int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns, unsigned int minrowcol);
+
+/* Left matrix multiplication with DIAG matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns]
+minrowcol=minimum(rows, columnsx) */
+void mmultDl15(int *DIAG, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns, unsigned int minrowcol);
+
+/* Right matrix multiplication with DIAG ones matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns]
+minrowcol=minimum(columnsx,columns) */
+void mmultDr(int *m1, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns, unsigned int minrowcol);
+
+/* Right matrix multiplication with DIAG matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns]
+minrowcol=minimum(columnsx,columns) */
+void mmultDr15(int *m1, int *DIAG, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns, unsigned int minrowcol);
+
+/* Matrix transposition in q15: m1.' = result[rows, columns] */
+void mtrans(int *m1, int *result, unsigned int rows, unsigned int columns);
+
+/* Matrix [2,2] inversion in q15: inv(m1) = result[rows, columns] */
+void minv2(int *matrix, long *result);
+
+void choice_P(int *m, int *result, unsigned int columns);
+void choice_x(int *m, int *result);
+
+
+/* matrix addition in q15 */
+void madd(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns)
+{
+  unsigned int i,j;
+
+  for (i=0; i<=rows; i++)
+    for (j=0; j<=columns; j++)
+      *result++ = *m1++ + *m2++;
+}
+
+
+/* add diagonal matrix m2 to matrix m1 - both in format q15, minrowcol = min(rows, columns) */
+void maddD(int *m1, int *m2, unsigned int minrowcol, unsigned int columns)
+{
+  unsigned int i;
+
+  for (i=0; i<=minrowcol; i++)
+/*    *(m1+i*(columns+1)+i) += *(m2+i*(columns+1)+i); */
+  {
+    *m1 += *m2;
+    m1+=(columns+2);
+    m2+=(columns+2);
+  }
+
+}
+
+/* Matrix substraction in q15 */
+void msub(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns)
+{
+  unsigned int i,j;
+
+  for (i=0; i<=rows; i++)
+    for (j=0; j<=columns; j++)
+      *result++ = *m1++ - *m2++;
+}
+
+
+/* matrix multiplication in q15 */
+void mmult(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns)
+{
+  unsigned int i, j, k;
+  long tmp_sum=0;
+  int *m2pom;
+
+  for (i=0; i<=rows; i++)
+  {
+    for (j=0; j<=columns; j++)
+    {
+      m2pom=m2+j;
+
+      for (k=0; k<=columnsx; k++)
+      {
+        tmp_sum+=(long)(*m1++)**m2pom;
+        m2pom+=columns+1;
+      }
+
+      // 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-=(columnsx+1);
+    }
+    m1+=(columnsx+1);
+  }
+}
+
+
+/* Matrix multiplication in q15: m1[rows,columnsx]*(m2[columns,columnsx]transpose) = result[rows,columns] */
+void mmultt(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns)
+{
+  unsigned int i, j, k;
+  long tmp_sum=0;
+  int *m2pom=m2;
+
+  for (i=0; i<=rows; i++)
+  {
+    for (j=0; j<=columns; j++)
+    {
+      for (k=0; k<=columnsx; k++)
+        tmp_sum+=(long)(*m1++)*(*m2pom++);
+
+      // 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-=(columnsx+1);
+    }
+    m2pom=m2;
+    m1+=(columnsx+1);
+  }
+}
+
+
+/* matrix multiplication in q15: sum is in q15 */
+void mmult15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns)
+{
+  unsigned int i, j, k;
+  long tmp_sum=0;
+  int *m2pom;
+
+  for (i=0; i<=rows; i++)
+  {
+    for (j=0; j<=columns; j++)
+    {
+      m2pom=m2+j;
+
+      for (k=0; k<=columnsx; k++)
+      {
+        tmp_sum+=((long)(*m1++)**m2pom)>>15;
+        m2pom+=columns+1;
+      }
+
+      // saturation effect
+      if (tmp_sum>32767) tmp_sum=32767;
+      if (tmp_sum<-32768) tmp_sum=-32768;
+
+      *result++=tmp_sum;
+
+      tmp_sum=0;
+      m1-=(columnsx+1);
+    }
+    m1+=(columnsx+1);
+  }
+}
+
+
+/* Matrix multiplication in q15 (sum is in q15): m1[rows,columnsx]*(m2[columns,columnsx]transpose) = result[rows,columns] */
+void mmultt15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns)
+{
+  unsigned int i, j, k;
+  long tmp_sum=0;
+  int *m2pom=m2;
+
+  for (i=0; i<=rows; i++)
+  {
+    for (j=0; j<=columns; j++)
+    {
+      for (k=0; k<=columnsx; k++)
+        tmp_sum+=((long)(*m1++)*(*m2pom++))>>15;
+
+      // saturation effect
+      if (tmp_sum>32767) tmp_sum=32767;
+      if (tmp_sum<-32768) tmp_sum=-32768;
+
+      *result++=tmp_sum;
+
+      tmp_sum=0;
+      m1-=(columnsx+1);
+    }
+    m2pom=m2;
+    m1+=(columnsx+1);
+  }
+}
+
+
+/* Matrix multiplication - Q15 * Q30 format -> RESULT in Q15 */
+void mmult1530(int *m1, long *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns)
+{
+  unsigned int i, j, k;
+  long tmp_sum=0;
+  long *m2pom;
+
+  for (i=0; i<=rows; i++)
+  {
+    for (j=0; j<=columns; j++)
+    {
+      m2pom=m2+j;
+
+      for (k=0; k<=columnsx; k++)
+      {
+        tmp_sum+=(long)(*m1++)**m2pom;
+        m2pom+=columns+1;
+      }
+
+      // 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-=(columnsx+1);
+    }
+    m1+=(columnsx+1);
+  }
+}
+ 
+
+/* Left matrix multiplication with DIAG ones matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns] 
+minrowcol=minimum(rows, columnsx) */
+void mmultDl(int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns, unsigned int minrowcol)
+{
+  unsigned int i,j;
+/*  int *res_tmp=result; */
+  
+/* This erase sequence have to be enabled in case that result matrix has more terms than DIAG matrix */
+/*  for (i=0;i<=rows;i++)	
+    for (j=0;j<=columns;j++)
+      *res_tmp++=0;
+/* END OF ERASE SEQUENCE */
+
+  for (i=0;i<=minrowcol;i++)
+    for (j=0;j<=columns;j++)
+      *result++=*m2++;
+}
+
+
+/* Left matrix multiplication with DIAG matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns] 
+minrowcol=minimum(rows, columnsx) */
+void mmultDl15(int *DIAG, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns, unsigned int minrowcol)
+{
+  unsigned int i,j;
+/*  int *res_tmp=result; */
+
+/* This erase sequence have to be enabled in case that result matrix has more terms than DIAG matrix */  
+/*  for (i=0;i<=rows;i++)		
+    for (j=0;j<=columns;j++)
+      *res_tmp++=0;
+/* END OF ERASE SEQUENCE */
+
+  for (i=0;i<=minrowcol;i++)
+  {
+    for (j=0;j<=columns;j++)
+      *result++=((long)(*DIAG)*(*m2++))>>15;
+    
+    DIAG+=(columnsx+1);
+  }
+}
+
+
+/* Right matrix multiplication with DIAG ones matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns] 
+minrowcol=minimum(columnsx,columns) */
+void mmultDr(int *m1, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns, unsigned int minrowcol)
+{
+  unsigned int i,j;
+/*  int *res_tmp=result; */
+
+/* This erase sequence have to be enabled in case that result matrix has more terms than DIAG matrix */
+/*  for (i=0;i<=rows;i++)
+    for (j=0;j<=columns;j++)
+      *res_tmp++=0;
+/* END OF ERASE SEQUENCE */
+
+  for (i=0;i<=rows;i++)
+  {
+    for (j=0;j<=minrowcol;j++)
+      *result++=*m1++;
+    
+    m1+=(columnsx-minrowcol);
+    result+=(columns-minrowcol);
+  }
+}  
+
+
+
+/* Right matrix multiplication with DIAG matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns] 
+minrowcol=minimum(columnsx,columns) */
+void mmultDr15(int *m1, int *DIAG, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns, unsigned int minrowcol)
+{
+  unsigned int i,j;
+  int *DIAG_tmp=DIAG;
+/*  int *res_tmp=result; */
+
+/* This erase sequence have to be enabled in case that result matrix has more terms than DIAG matrix */  
+/*  for (i=0;i<=rows;i++)		
+    for (j=0;j<=columns;j++)
+      *res_tmp++=0;
+/* END OF ERASE SEQUENCE */
+
+  for (i=0;i<=rows;i++)
+  {
+    for (j=0;j<=minrowcol;j++)
+    {
+      *result++=((long)(*m1++)**DIAG_tmp)>>15;
+      DIAG_tmp+=columns+2;
+    }
+     
+    DIAG_tmp=DIAG;
+    m1+=(columnsx-minrowcol);
+    result+=(columns-minrowcol);
+  }
+}  
+
+
+/* matrix transposition - neni optimalizovana, protoze se nepouziva */
+void mtrans(int *m1, int *result, unsigned int rows, unsigned int columns)
+{
+  unsigned int i, j;
+  
+  for (i=0; i<=rows; i++)
+    for (j=0; j<=columns; j++)
+      *result++=*(m1+(columns+1)*j+i);
+}
+
+/* matrix [2,2] inversion */
+void minv2(int *matrix, long *result)
+{
+  int det;
+  unsigned int i;
+
+  det=((long)*matrix**(matrix+3)-(long)*(matrix+1)**(matrix+2))>>15;
+
+  *result++=(((long)*(matrix+3))<<15)/det;
+  *result++=(((long)*(matrix+1))<<15)/det;
+  *result++=(((long)*(matrix+2))<<15)/det;
+  *result++=(((long)*matrix)<<15)/det;
+}
+
+
+void choice_P(int *m, int *result, unsigned int columns)
+{
+  *result++=*m;
+  *result++=*(m+1);
+  *result++=*(m+columns+1);
+  *result++=*(m+columns+2);
+}
+
+
+void choice_x(int *m, int *result)
+{
+  *result++=*m++;
+  *result=*m;
+}
+
Index: /simulator_zdenek/ekf_example/ekf.cpp
===================================================================
--- /simulator_zdenek/ekf_example/ekf.cpp (revision 50)
+++ /simulator_zdenek/ekf_example/ekf.cpp (revision 50)
@@ -0,0 +1,254 @@
+/************************************
+        Extended Kalman Filter
+        Kalman Observer
+
+        Z. Peroutka
+
+Rev. 15.3.2008
+
+        EKF pocitan s daty ve formatu Q15, zatimco regulace pracuje v Q13 ->
+        resenim je vynasobit referencni hodnoty v EKF 4-mi (tim je automaticky
+        zajisten prechod mezi formaty Q13 a Q15). Realizovano konstantou K_PREVOD_FORM.
+
+15.3.2008       Kontrola kodu + zamena datovych typu q15->int a q30->long.
+
+*************************************/
+
+#include <math.h>
+#include "fixed.h"
+//#include "parametry_motoru.h"         // aktivovat v DSP
+#include "reference.h"
+#include "matrix.h"
+#include "ekf.h"
+
+#define K_PREVOD_FORM   4               // 2^(15-Qm)
+
+/* Declaration of global functions */
+void init_ekf(double Tv, double *param);
+void ekf(double *x_estimation, double Umd, double beta, double Ucnd, double Ucd, double isxd, double isyd);
+
+/* Declaration of local functions */
+static void prediction(int *ux);
+static void correction(void);
+static void update_psi(void);
+
+// global variables
+double ladeni_EKF[10];
+
+/* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
+static int H[8]={0x7FFF,0,0,0,0,0x7FFF,0,0}; /* matrix [2,4] */
+static int Ht[8]={0x7FFF,0,0,0x7FFF,0,0,0,0};/* matrix [4,2] */
+//static int Q[4][4]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */
+//static int R[2][2]={0,0,0,0}; /* matrix [2,2] */
+static int Q[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */
+static int R[4]={0,0,0,0}; /* matrix [2,2] */
+
+/* Initial conditions and variables */
+static int x_est[4]={0,0,0,0};
+static int x_pred[4]={0,0,0,0};
+static int P_pred[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */
+//static int P_est[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */
+static int P_est[16]={0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF}; /* matrix [4,4] */
+static int Y_mes[2]={0,0};
+static int ukalm[2]={0,0};
+static int Kalm[8]; /* matrix [5,2] */
+
+static int PSI[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */
+
+/* Temporary variables */
+static int temp15a[16];
+
+// constants of PMSM mathematical model
+static int cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+void init_ekf(double Tv, double *param)
+{
+  // parametry simulace - v DSP vyhodit (nahrazeno "parametry_motoru.h")
+  double Rs, Ls, Fmag, kp, p;
+
+ // param=[Rs, Ls, Fmag, Bf, p, kp, J = 0.04?];
+  Rs=*param;
+  Ls=*(param+1);
+  Fmag=*(param+2);
+  p=*(param+4);
+  kp=*(param+5);
+  /// KONEC inicializace pro PC - az sem vyhodit v DSP ///
+
+  // Tuning of matrix Q
+/*  Q[0][0]=prevod(.05,15);       // 1e-2
+  Q[1][1]=Q[0][0];
+  Q[2][2]=prevod(1e-3,15);      // 10e-5
+  Q[3][3]=prevod(1e-3,15);      // model(ureg) ... 4e-3
+
+  // Tuning of matrix R
+  R[0][0]=prevod(0.05,15);         // model(ureg) ... 0.1
+  R[1][1]=R[0][0];
+/**/
+
+  // 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
+
+  // Tuning of matrix R
+  R[0]=prevod(0.05,15);         // 0.05
+  R[3]=R[0];
+/**/
+
+  /* Tuning of matrix P_est - initial values */
+/*    P_est[0][0]=prevod(0.999,15);
+    P_est[1][1]=prevod(0.999,15);
+    P_est[2][2]=prevod(0.999,15);
+    P_est[3][3]=prevod(0.999,15);
+    P_est[4][4]=prevod(0.999,15);
+/**/
+
+  // 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*K_PREVOD_FORM/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;
+}
+
+
+static void update_psi(void)
+{
+  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);
+
+  // implementace v DSP
+//  t_sin=qsin(x_est[3]);
+//  t_cos=qcos(x_est[3]);
+
+  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;
+}
+
+
+void prediction(int *ux)
+{
+  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);
+
+  // implementace v DSP
+//  t_sin=qsin(x_est[3]);
+//  t_cos=qcos(x_est[3]);
+
+  tmp=((long)cB*x_est[2])>>15;
+  x_pred[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*ux[0])>>15;
+  x_pred[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*ux[1])>>15;
+  x_pred[2]=x_est[2];
+  x_pred[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
+
+  update_psi();
+
+  mmult(PSI,P_est,temp15a,3,3,3);
+//  mtrans(PSI,temp15b,5,5);
+  mmultt(temp15a,PSI,P_pred,3,3,3);
+  maddD(P_pred,Q,3,3);
+}
+
+void correction(void)
+{
+  int Y_error[2];
+  long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */
+
+  /* Kalman gain calculation */
+//  mmult(P_pred,Ht,temp15a,5,5,2);
+/*  mmultt(P_pred,H,temp15a,4,4,1);
+  mmult(H,temp15a,temp15b,1,4,1); 	these lines are replaced by choice_P */
+
+  choice_P(P_pred,temp15a,3);
+  maddD(temp15a,R,1,1);
+  minv2(temp15a,temp30a);
+/*  mmultt(P_pred,H,temp15a,4,4,1); 	/* remove this line if choice_P is not used */
+//  mmultDr15(P_pred,Ht,temp15a,4,4,1,1);
+  mmultDr(P_pred,temp15a,3,3,1,1);
+  mmult1530(temp15a,temp30a,Kalm,3,1,1);
+
+  /* estimate the state system */
+//  mmult(H,x_pred,temp15a,1,4,0);
+  choice_x(x_pred, temp15a);
+  msub(Y_mes,temp15a,Y_error,1,0);
+  mmult(Kalm,Y_error,temp15a,3,1,0);
+  madd(x_pred,temp15a,x_est,3,0);
+
+  /* matrix of covariances - version without MMULTDL() */
+/*  mmult(Kalm,H,temp15a,4,1,4);
+  mmult(temp15a,P_pred,P_est,4,4,4);
+  msub(P_pred,P_est,P_est,4,4);
+/* END */
+
+/* Version with MMULTDL() */
+  mmultDl(P_pred,temp15a,1,3,3,1);
+/* if result matrix has more terms than DIAG matrix,it is necessary to enable
+erase sequence in definition of function MMULTDL() that is currently disabled. */
+
+  mmult(Kalm,temp15a,P_est,3,1,3);
+  msub(P_pred,P_est,P_est,3,3);
+/* END */
+}
+
+
+void ekf(double *x_estimation, double Umd, double beta, double Ucnd, double Ucd, double isxd, double isyd)
+{
+  int Umk, ua, ub;
+
+  /// prechod ze simulace do fixed-pointu - vyradit v DSP ///////////
+  int Um, Ucn, Uc, isx, isy, t_cos;
+
+  Um=prevod(Umd/Uref,Qm);
+  Ucn=prevod(Ucnd/Uref,Qm);
+  Uc=prevod(Ucd/Uref,Qm);
+  isx=prevod(isxd/Iref,Qm);
+  isy=prevod(isyd/Iref,Qm);
+  /// KONEC emulace FIXED-POINTu - az sem vyradit v DSP ////////////
+
+  // prepocet napeti pro EKF dle skutecneho napeti v ss obvodu
+  Umk=((long)Um*Uc)/Ucn;
+
+  // vypocet fazovych napeti stridace
+  ua=((long)Umk*prevod(cos(beta),15))>>15;
+  ub=((long)Umk*prevod(cos(beta-2./3.*M_PI),15))>>15;
+//  uc=((long)Umk*prevod(cos(beta+2./3.*M_PI),15))>>15;
+
+  // vypocet napeti v systemu (x,y)
+  ukalm[0]=ua;
+  ukalm[1]=(((long)ua+ub<<1)*18917)>>15;		// usy=(ua+2*ub)/sqrt(3)
+
+  // zadani mereni
+  Y_mes[0]=isx;
+  Y_mes[1]=isy;
+
+  ////// vlastni rutina EKF /////////////////////////
+  prediction(ukalm);
+  correction();
+
+  // navrat estimovanych hodnot regulatoru
+  *x_estimation=zprevod(x_est[2],Qm)*Wref;
+//  x_est[3]=(short int)x_est[3];
+  *(x_estimation+1)=zprevod((short int)x_est[3],15)*Thetaref;
+}
Index: /simulator_zdenek/ekf_example/PWM.H
===================================================================
--- /simulator_zdenek/ekf_example/PWM.H (revision 50)
+++ /simulator_zdenek/ekf_example/PWM.H (revision 50)
@@ -0,0 +1,20 @@
+/* modul PWM modulace
+   ubytky a mrtve casy
+   
+   shodna verze s PWM_FULL_LC
+   
+   31.5. 2007
+*/
+
+extern void pwm_full_init(double Ucn, double fc, double DT);
+extern void pwm_full(double *us, double *u, double *Iz,double Ucn, double Uc, double *i, double fc, double *va_char,unsigned int pocet, double h);
+
+extern double ladeni_pila;
+extern double ladeni_ur;
+
+extern void pwm_full_new_init(double h, double fc, double DT, int citac_init, int D_smer, int D_smer2);
+extern void pwm_full_new(double *us, double *u, double *Iz, double Ucn, double Uc, double *is, double *va_char, unsigned int pocet);
+
+// vylepsena PWM ... mod=0 - sinusova PWM; mod=1 - PWM se 3. harmonickou
+extern void pwm_full_new_init_3h(double h, double fc, double DT, int citac_init, int D_smer, int D_smer2);
+extern void pwm_full_new_3h(double *us, double *u, double *Iz, double Ucn, double Uc, double *is, double *va_char, unsigned int pocet, unsigned int mod);
Index: /simulator_zdenek/ekf_example/matrix.h
===================================================================
--- /simulator_zdenek/ekf_example/matrix.h (revision 50)
+++ /simulator_zdenek/ekf_example/matrix.h (revision 50)
@@ -0,0 +1,72 @@
+/************************************
+        Extended Kalman Filter
+        Matrix operations
+
+        Z. Peroutka
+
+Rev. 15.3.2008
+
+15.3. 2008      Kompletni kontrola vypoctu + zamena q15->int a q30->long
+
+*************************************/
+
+/* Vsechny meze se udavaji ve tvaru (rozmer_matice - 1), abych mel ve FOR konstantni horni mez) */
+
+/* Matrix addition in q15: m1 + m2 = result[rows, columns] */
+extern void madd(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns);
+
+/* add diagonal matrix m2 to matrix m1 - both in format q15, minrowcol = min(rows, columns) */
+extern void maddD(int *m1, int *m2, unsigned int minrowcol, unsigned int columns);
+
+/* Matrix substraction in q15: m1 - m2 = result[rows, columns] */
+extern void msub(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns);
+
+/* Matrix multiply in q15: m1[rows,columnsx]*m2[columnsx,columns] = result[rows,columns] */
+extern void mmult(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns);
+
+/* Matrix multiplication in q15: m1[rows,columnsx]*(m2[columnsx,columns]transpose) = result[rows,columns] */
+extern void mmultt(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns);
+
+/* matrix multiplication in q15: sum is in q15 */
+extern void mmult15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns);
+
+/* Matrix multiplication in q15 (sum is in q15): m1[rows,columnsx]*(m2[columns,columnsx]transpose) = result[rows,columns] */
+extern void mmultt15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns);
+
+/* Matrix multiplication - Q15 * Q30 format -> RESULT in Q15 */
+extern void mmult1530(int *m1, long *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns);
+
+/* Left matrix multiplication with DIAG ones matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns]
+minrowcol=minimum(rows, columnsx) */
+extern void mmultDl(int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns, unsigned int minrowcol);
+
+/* Left matrix multiplication with DIAG matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns]
+minrowcol=minimum(rows, columnsx) */
+extern void mmultDl15(int *DIAG, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns, unsigned int minrowcol);
+
+/* Right matrix multiplication with DIAG ones matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns]
+minrowcol=minimum(columnsx,columns) */
+extern void mmultDr(int *m1, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns, unsigned int minrowcol);
+
+/* Right matrix multiplication with DIAG matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns]
+minrowcol=minimum(columnsx,columns) */
+extern void mmultDr15(int *m1, int *DIAG, int *result, unsigned int rows, unsigned int columnsx, unsigned int
+columns, unsigned int minrowcol);
+
+/* Matrix transposition in q15: m1.' = result[rows, columns] */
+extern void mtrans(int *m1, int *result, unsigned int rows, unsigned int columns);
+
+/* Matrix [2,2] inversion in q15: inv(m1) = result[rows, columns] */
+extern void minv2(int *matrix, long *result);
+
+extern void choice_P(int *m, int *result, unsigned int columns);
+extern void choice_x(int *m, int *result);
+
Index: /simulator_zdenek/ekf_example/ekf.h
===================================================================
--- /simulator_zdenek/ekf_example/ekf.h (revision 50)
+++ /simulator_zdenek/ekf_example/ekf.h (revision 50)
@@ -0,0 +1,23 @@
+/************************************
+        Extended Kalman Filter
+        Kalman Observer
+
+        Z. Peroutka
+
+Rev. 15.3.2008
+
+        EKF pocitan s daty ve formatu Q15, zatimco regulace pracuje v Q13 ->
+        resenim je vynasobit referencni hodnoty v EKF 4-mi (tim je automaticky
+        zajisten prechod mezi formaty Q13 a Q15). Realizovano konstantou K_PREVOD_FORM.
+
+15.3.2008       Kontrola kodu + zamena datovych typu q15->int a q30->long.
+
+*************************************/
+
+/* Declaration of global functions */
+void init_ekf(double Tv, double *param);
+void ekf(double *x_estimation, double Umd, double beta, double Ucnd, double Ucd, double isxd, double isyd);
+
+// pouze pro ucely ladeni
+extern double ladeni_EKF[10];
+
Index: /simulator_zdenek/ekf_example/PMSM_MOD.cpp
===================================================================
--- /simulator_zdenek/ekf_example/PMSM_MOD.cpp (revision 50)
+++ /simulator_zdenek/ekf_example/PMSM_MOD.cpp (revision 50)
@@ -0,0 +1,144 @@
+/* model synchronniho motoru s permanentnimi magnety
+
+   PMSM v plovouci radove carce - fyzikalni model!
+   
+   31.5.2007
+
+   Upraven vypocet momentu M = Fmag*Isq!! (nikoliv M = Fs*Isq!!)
+   31.5.2007	doplnen vypocet zatezneho uhlu (beta)
+*/
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include "pmsm_mod.h"
+
+double x[10]; // (isx,isy,wme,theta_e,M,Fsd,Isd,Isq,Mz,beta)		... beta = zatezny uhel
+
+void init_pmsm(double *param, double *REL);
+void pmsm_double(double usx, double usy, double h, unsigned int vypocet);
+
+// pomocne funkce
+static double uhel(double x, double y);
+
+static double Isx,Isy,wme,Mz,M,Fs,Isd,Isq;
+static double Rs,Ls,Fmag,Bfric,p,kp,J;
+// Rs=odpor stator; Ls=statorova indukcnost (synchronni reaktance)
+// p=pocet polparu, kp=konst. Parkovy transf.,
+// J=moment setrvacnosti (J<0 ... nekonecny)
+
+static double Uref, Iref, Wref, Mref, Fref, Thetaref; // referencni hodnoty
+
+static double dIsx,dIsx1,dIsx2,dIsx3,dIsy,dIsy1,dIsy2,dIsy3;
+static double dTheta,dTheta1,dTheta2,dTheta3;
+static double dw,dw1,dw2,dw3;
+
+static double A,B,C,D,E,F,G;
+
+static double Fsq;
+
+
+/////////////////// POMOCNE FUNKCE //////////////////////////////////
+double uhel(double x, double y)
+{
+  double th;
+
+  if (x==0)
+    if (y==0) th=0.;
+	else if (y>0) th=M_PI/2.;
+	     else th=-M_PI/2.;
+  else
+    th=atan(y/x);
+
+  if (x<0) th+=M_PI;
+
+  return th;
+}
+
+
+void init_pmsm(double *param, double *REL)
+{
+  unsigned int i;
+
+  Rs=*(param);
+  Ls=*(param+1);
+  Fmag=*(param+2);
+  Bfric=*(param+3);
+
+  p=*(param+4);
+  kp=*(param+5);
+  J=*(param+6);
+
+  Uref=*REL;
+  Iref=*(REL+1);
+  Wref=*(REL+2);
+  Thetaref=*(REL+3);
+  Mref=*(REL+4);
+  Fref=*(REL+5);
+
+// definice konstant modelu
+  A=Rs/Ls;
+  B=Fmag/Ls*Wref/Iref;
+  C=Uref/Iref/Ls;
+  D=Bfric/J;
+  E=kp*p*p*Fmag/J*Iref/Wref;
+  F=p/J*Mref/Wref;
+  G=Wref/Thetaref;
+
+// definice pocatecnich hodnot
+  dIsx=0;dIsx1=0;dIsx2=0;dIsx3=0;dIsy=0;dIsy1=0;dIsy2=0;dIsy3=0;
+  dTheta=0;dTheta1=0;dTheta2=0;dTheta3=0;
+  dw=0;dw1=0;dw2=0;dw3=0;
+}
+
+void pmsm_double(double usx, double usy, double h, unsigned int vypocet)
+// vypocet<5...Euler, vypocet>4 ... Adams 4.radu
+{
+// (isx,isy,wme,theta_e,M,Fs,Mz)
+  // vypocty diferenci
+  dIsx=-A*x[0]+B*x[2]*sin(x[3]*Thetaref)+C*usx;
+  dIsy=-A*x[1]-B*x[2]*cos(x[3]*Thetaref)+C*usy;
+  dTheta=G*x[2];
+
+  if (J>0)
+    dw=E*(x[1]*cos(x[3]*Thetaref)-x[0]*sin(x[3]*Thetaref))-D*x[2]-F*x[8];
+  else
+    dw=0;
+
+// integrace
+  if (vypocet<5)  // Euler
+  { x[0]+=dIsx*h;
+    x[1]+=dIsy*h;
+    x[2]+=dw*h;
+    x[3]+=dTheta*h;
+  }
+  else			// Adams 4.radu
+  { x[0]+=h/24*(55*dIsx-59*dIsx1+37*dIsx2-9*dIsx3);
+    x[1]+=h/24*(55*dIsy-59*dIsy1+37*dIsy2-9*dIsy3);
+    x[2]+=h/24*(55*dw-59*dw1+37*dw2-9*dw3);
+    x[3]+=h/24*(55*dTheta-59*dTheta1+37*dTheta2-9*dTheta3);
+  }
+
+  // uprava theta tak, aby bylo stale v rozmezi (-pi,pi)
+  if (x[3]>M_PI/Thetaref) x[3]-=(2*M_PI/Thetaref);
+  if (x[3]<-M_PI/Thetaref) x[3]+=(2*M_PI/Thetaref);
+
+  // zamena diferenci
+  dIsx3=dIsx2;dIsx2=dIsx1;dIsx1=dIsx;
+  dIsy3=dIsy2;dIsy2=dIsy1;dIsy1=dIsy;
+  dTheta3=dTheta2;dTheta2=dTheta1;dTheta1=dTheta;
+  dw3=dw2;dw2=dw1;dw1=dw;
+
+  // vypocet Isd, Isq
+  x[6]=x[0]*cos(x[3]*Thetaref)+x[1]*sin(x[3]*Thetaref);         // Isd
+  x[7]=x[1]*cos(x[3]*Thetaref)-x[0]*sin(x[3]*Thetaref);         // Isq
+
+  // vypocet toku statoru Fsd - neni normovany!!
+  x[5]=Ls*x[6]*Iref+Fmag;
+
+  // Vypocet momentu
+  x[4]=kp*p*Fmag*Iref/Mref*(x[1]*cos(x[3]*Thetaref)-x[0]*sin(x[3]*Thetaref));
+
+  // vypocet zatezneho uhlu
+  Fsq=Ls*x[7]*Iref;
+  x[10]=uhel(x[5],Fsq);
+}
Index: /simulator_zdenek/ekf_example/parametry_motoru.h
===================================================================
--- /simulator_zdenek/ekf_example/parametry_motoru.h (revision 50)
+++ /simulator_zdenek/ekf_example/parametry_motoru.h (revision 50)
@@ -0,0 +1,20 @@
+/****************************************
+
+	Parametry synchronniho motoru s permanentnimi magnety
+
+	Definice zakl. parametru
+
+	Z. Peroutka
+
+Rev.	4.6.2006
+
+Pohon NFR
+******************************************/
+
+#define Rs		0.28
+#define Ls		0.003465
+#define Fmag	0.1989
+#define J		0.04		// odhad - zatim nemereno
+
+#define p		4.0
+#define kp		1.5
Index: /simulator_zdenek/ekf_example/pmsm_mod.h
===================================================================
--- /simulator_zdenek/ekf_example/pmsm_mod.h (revision 50)
+++ /simulator_zdenek/ekf_example/pmsm_mod.h (revision 50)
@@ -0,0 +1,12 @@
+/* model synchronniho motoru s permanentnimi magnety
+   
+   PMSM v plovouci radove carce - fyzikalni model!
+   
+   3.6.2006
+*/
+
+extern double x[10]; // (isx,isy,wme,theta_e,M,Fs,Isd,Isq,Mz,beta)	... beta=zatezny uhel
+
+extern void init_pmsm(double *param, double *REL);
+extern void pmsm_double(double usx, double usy, double h, unsigned int vypocet);
+
Index: /simulator_zdenek/ekf_example/regulace.CPP
===================================================================
--- /simulator_zdenek/ekf_example/regulace.CPP (revision 50)
+++ /simulator_zdenek/ekf_example/regulace.CPP (revision 50)
@@ -0,0 +1,376 @@
+/*********************************
+
+        Vektorove rizeni
+
+        regulacni struktura - plovouci radova carka
+
+        Z. Peroutka
+
+Rev. 15.3.2008
+
+5.6.2007        Doplnen vypocet uhlu beta, ktery doplnuje regulator Isq
+
+**********************************/
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include "nastaveni_regulatoru_float.h"
+#include "regulace.h"
+
+
+void init_regulace(double *param, double TV);
+//void vektor_regulace(double Idw, double Iqw, double Urm_max, double Ww, double *u, double Isx, double Isy, double theta, double rychlost, double Ucn_2, unsigned int rezim);
+void vektor_regulace(double Idw, double Iqw, double Urm_max, double Ww, double *u, double Isx, double Isy, double theta, double rychlost, double Ucn_2, double Uc, double Ucn, unsigned int rezim);
+
+static void reset_regulatoru(void);
+
+static double pi_reg(double epsilon, double Kpf, double Kif, double MAX, double MIN, double *S);
+static double uhel(double x, double y);
+static void filtr(double U, double *Uf, double kt);
+
+// regulatory proudu
+static double ud, uq, alfa, beta;
+static double Sid, Siq, Surm;
+
+// omezeni momentu
+static double Kiqmax, Iqwmax, Iqw_reg;
+static double Idw_urm, Idw_reg;
+
+// regulator rychlosti
+static double Sw, MAXw_lim;
+
+static double U;
+static double Um, Urmf;          // velikost napeti
+
+// odvazbovaci obvod - blok vypocet napeti
+static double Kodv_ud, Kodv_uind;
+
+static double Isd, Isq, Fs, Fmag, moment, K_Fs, K_moment;
+static double Ismaxf2, tmp_omezeni;
+
+double ladeni_regulace[10];
+
+double Treg;
+
+static unsigned int start_reg;
+static double Ucf, Isdf, Ukomp;
+static double zbeta, zbeta_vyp, zbetaw;		// zatezny uhel vyhodnoceny z polohy pozadovaneho napeti
+
+static unsigned int ALGORITMUS;
+
+// regulator Isq pres zatezny uhel
+static double Sbeta;
+
+// regulator Isd - kompenzace vypocteneho napeti
+static double SUkomp;
+
+// pomocne promenne
+static unsigned int blokace_beta=0;
+
+/////////////////// POMOCNE FUNKCE //////////////////////////////////
+double uhel(double x, double y)
+{
+  double th;
+
+  if (x==0)
+    if (y==0) th=0.;
+	else if (y>0) th=M_PI/2.;
+	     else th=-M_PI/2.;
+  else
+    th=atan(y/x);
+
+  if (x<0) th+=M_PI;
+
+  return th;
+}
+
+
+double pi_reg(double epsilon, double Kpf, double Kif, double MAX, double MIN, double *S)
+{
+  double out;
+
+  out=Kpf*epsilon+*S;
+  if (out>MAX) out=MAX;
+  else if (out<MIN) out=MIN;
+  else
+    *S+=Kif*epsilon;
+
+  return out;
+}
+
+void filtr(double U, double *Uf, double kt)
+{
+  double Ufpom;
+
+  Ufpom=*Uf;
+  *Uf=Ufpom+kt*(U-*Uf);
+}
+
+//////////////////////////////////////////////////////////////////////////
+
+
+void init_regulace(double *param, double TV)
+{
+  double Kpd;   // pomocna velicina
+  double Rs, Ls, kp, p;
+
+  // parametry pro odvazbeni
+  Rs=*(param+0);
+  Ls=*(param+1);
+  Fmag=*(param+2);
+  kp=*(param+5);
+  p=*(param+4);
+  // vyhodit az sem v DSP ////////////////////
+
+  Sid=0;        // nulovani integracni slozky
+  Siq=0;
+
+  // regulator odbuzovani
+//  Kpd=Kpurm*2.0/Iref;      // 2.0, protoze Urm je ladeno v pomernych hodnotach Um*2/Ucn a tady je to vztazeno na Ucn
+//  Kiurmf=prevod((Kpd*TV/Tiurm),Q_Kiurm);
+
+//  Kt_urm=prevod(TV/Tfurm,Q_Kturm);   // casova konstanta filtru Urm
+
+  Surm=0;        // nulovani integracni slozky
+  Urmf=0;        // filtrovane napeti Urm
+
+  // omezeni momentu kvuli momentu zvratu - omezeni pomoci frmax
+  Ismaxf2=Ismax*Ismax;
+
+  // blok VYPOCET NAPETI
+  Kodv_ud=Ls;
+  Kodv_uind=Fmag;
+
+  // regulator rychlosti
+  Sw=0;
+
+  // vypocet modelu
+  K_Fs=Ls;
+  K_moment=kp*p*Fmag;
+
+  Treg=TV;
+
+  // pocatecni ALGORITMUS
+  ALGORITMUS=0;		// implicitne se pouzije klasicke vektorove rizeni
+
+  // regulator Isq prostrednictvim beta
+  zbeta=0.;
+  Sbeta=0.;
+  
+  // regulator Isd - kompenzace vypocteneho napeti
+  SUkomp=0.;
+
+  // start regulace
+  start_reg=1;			// indikace 1. pruchodu regulacni smyckou kvuli nastaveni spravnych hodnot filtru Uc a Isd
+}
+
+void vektor_regulace(double Idw, double Iqw, double Urm_max, double Ww, double *u, double Isx, double Isy, double theta, double rychlost, double Ucn_2, double Uc, double Ucn, unsigned int rezim)
+{
+  // vypocet Isd, Isq
+  Isd=Isx*cos(theta)+Isy*sin(theta);
+  Isq=Isy*cos(theta)-Isx*sin(theta);
+
+  // filtrace napeti ss obvodu
+  if (start_reg==1)
+  { 
+    Ucf=Uc;
+    Isdf=Isd;
+    start_reg=0;
+  }
+  else
+  {
+    filtr(Uc,&Ucf,Treg/Tfuc);
+    filtr(Isd,&Isdf,Treg/Tfid);
+  } 
+
+  // vyber varianty regulace
+  if (fabs(rychlost)>prechod_1_2) ALGORITMUS=1;	// prechod z algoritmu 1 na 2
+  if (fabs(rychlost)<prechod_2_1) ALGORITMUS=0;	// prechod z algortimu 2 na 1
+//  ALGORITMUS=0;         // provozovana pouze definovana varianta algoritmu rizeni  
+
+  Iqw_reg=pi_reg(Ww-rychlost,Kpw,Kpw*Treg/Tiw,MAXw,-MAXw,&Sw);
+  if (rezim==0)
+    Iqw_reg=Iqw;          // vyrazeni reg. rychlosti
+  if (rezim==2)           // zkouska max. momentu
+    Iqw_reg=Iqwmax;       // Iqw = sqrt(Imax^2-Idw^2)
+
+////////////// REGULACE - VARIANTA 1 - klasicke vektorove rizeni ///////////////////
+if (ALGORITMUS==0) 
+{
+  // Regulator odbuzovani
+  Idw_urm=pi_reg(Urm_max-Urmf,Kpurm,Kpurm*Treg/Tiurm,0.,MINurm,&Surm);
+
+  // regulace proudu Id, Iq
+//  Idw_urm = Idw;      // vyrazena regulace Urm
+//  Idw_urm=0;
+  ud=pi_reg(Idw_urm-Isd,Kpi,Kpi*Treg/Tii,MAXi,-MAXi,&Sid);
+
+  // omezeni max. momentu (resp. max. Iqw) s ohledem na max. proud
+  tmp_omezeni=Ismaxf2-Idw_urm*Idw_urm;
+  if (tmp_omezeni<0) Iqwmax=0;
+  else Iqwmax=sqrt(tmp_omezeni);
+  if (MAXw>Iqwmax) MAXw_lim=Iqwmax;else MAXw_lim=MAXw;
+
+  // regulace rychlosti
+//  Iqw_reg=pi_reg(Ww-rychlost,Kpw,Kpw*Treg/Tiw,MAXw_lim,-MAXw_lim,&Sw);
+/*  if (rezim==0)
+    Iqw_reg=Iqw;          // vyrazeni reg. rychlosti
+  if (rezim==2)           // zkouska max. momentu
+    Iqw_reg=Iqwmax;       // Iqw = sqrt(Imax^2-Idw^2)
+*/
+  if (Iqw_reg>Iqwmax) Iqw_reg=Iqwmax;
+  if (Iqw_reg<-Iqwmax) Iqw_reg=-Iqwmax;
+  uq=pi_reg(Iqw_reg-Isq,Kpi,Kpi*Treg/Tii,MAXi,-MAXi,&Siq);
+
+///////// Vypocet napeti - "ODVAZBENI"
+  ud-=Kodv_ud*rychlost*Iqw_reg;
+  uq+=Kodv_ud*rychlost*Idw_urm+Kodv_uind*rychlost;
+
+  // omezeni - saturace napeti - stejne omezeni jako u reg. proudu
+  if (ud>MAXi) ud=MAXi;
+  if (ud<-MAXi) ud=-MAXi;
+  if (uq>MAXi) uq=MAXi;
+  if (uq<-MAXi) uq=-MAXi;
+
+  ////////////// KONEC ODVAZBENI /////////////////////////////////////////
+
+  // velikost a poloha vektoru napeti
+  Um=sqrt(ud*ud+uq*uq);
+  alfa=uhel(ud,uq);
+
+  // vypocet zatezneho uhlu z pozadovaneho napeti
+  if (rychlost>=0)
+    zbetaw=alfa-M_PI/2.;
+  else
+  { zbetaw=alfa+M_PI/2.;
+    if (ud<0) zbetaw-=2.*M_PI;
+  }
+
+  Sbeta=zbetaw;		// inicializace regulatoru Isq prostednictvim beta - sumace pri prepnuti nastavena na aktualni hodnotu zatezneho uhlu!
+}
+///////////////////// konec kodu VARIANTY 1 //////////////////////////
+else 	// (ALGORITMUS == 1)	... regulace zatezneho uhlu
+{
+///////////////// KOD VARIANTA 2 - regulace zatezneho uhlu ///////////
+  // omezeni max. momentu (resp. max. Iqw) s ohledem na max. proud
+  tmp_omezeni=Ismaxf2-Isdf*Isdf;
+  if (tmp_omezeni<0) Iqwmax=0;
+  else Iqwmax=sqrt(tmp_omezeni);
+  if (MAXw>Iqwmax) MAXw_lim=Iqwmax;else MAXw_lim=MAXw;
+  
+  // regulace rychlosti
+/*  Iqw_reg=pi_reg(Ww-rychlost,Kpw,Kpw*Treg/Tiw,MAXw_lim,-MAXw_lim,&Sw);
+  if (rezim==0)
+    Iqw_reg=Iqw;          // vyrazeni reg. rychlosti
+  if (rezim==2)           // zkouska max. momentu
+    Iqw_reg=Iqwmax;       // Iqw = sqrt(Imax^2-Idw^2)
+*/
+  if (Iqw_reg>Iqwmax) Iqw_reg=Iqwmax;
+  if (Iqw_reg<-Iqwmax) Iqw_reg=-Iqwmax;
+  
+//  Iqw_reg=Iqw;		// bez omezovani Isqw
+  // regulace Isq prostrednictvim zatezneho uhlu
+  zbeta=pi_reg(Iqw_reg-Isq,Kpib,Kpib*Treg/Tiib,MAXbeta,-MAXbeta,&Sbeta);
+
+  // test upravy signalu beta
+//if ((zbeta>73./180.*M_PI)||(blokace_beta==1)) {zbeta=80./180.*M_PI; blokace_beta=1;}
+
+   // vypocet napeti - jednoducha varianta bez uvazovani odbuzeni
+  Um=sqrt(Kodv_uind*Kodv_uind*rychlost*rychlost+Kodv_ud*Kodv_ud*rychlost*rychlost*Iqw_reg*Iqw_reg);     // rychlost musi byt v kvadratu, aby se eliminoval vliv znamenka rychlosti - Um musi byt >=0!
+//  Um=rychlost*sqrt(Kodv_uind*Kodv_uind+Kodv_ud*Kodv_ud*Iqw*Iqw);	// neni uvazovano omezeni Isqw
+
+  // vylepseny vypocet napeti - odvazbovaci obvod
+/*  ud=Kodv_ud*rychlost*Iqw_reg;
+  uq=Kodv_ud*rychlost*Isdf+Kodv_uind*rychlost;
+  Um=sqrt(ud*ud+uq*uq);
+*/
+  
+  // korece napeti pomoci regulatoru Isd
+  Ukomp=pi_reg(0.-Isd,0,1.*Treg/Tiidb,MAXud,MINud,&SUkomp);		// do zaporu neomezovat!!!  
+  Um+=Ukomp;
+
+  // zahrnuti vlivu skutecneho napeti Uc
+  Um=Um*Ucn/Ucf;		// v pevne radove carce pak: Um=(long)Um*Qm/Ucf;
+
+  // omezeni na Urm=1
+  if (Um>Ucn_2) Um=Ucn_2;
+
+  reset_regulatoru();	// zajistuje pripravenost reg. struktury VAR1
+
+  // doplneni vypoctu zatezneho uhlu ... zbeta_vyp
+  if (fabs(rychlost*Kodv_ud*Iqw_reg)>=Um)
+    if (Iqw_reg>0)
+      if (rychlost>0) zbeta_vyp=M_PI/2.;
+      else zbeta_vyp=-M_PI/2.;
+    else
+      if (rychlost>0) zbeta_vyp=-M_PI/2.;
+      else zbeta_vyp=M_PI/2.;
+  else
+    zbeta_vyp=asin(rychlost*Kodv_ud*Iqw_reg/Um);        // kontrola, zda Kodv_ud=Ls!
+
+//  zbeta=0;      // vyrazeni reg. Isq
+  zbeta_vyp=0;  // vyrazeni bloku "vypocet zatezneho uhlu"
+  // urceni pozadovaneho zatezneho uhlu a jeho omezeni na <-90st; 90st.>
+  if (rychlost<0)
+    zbetaw=-zbeta_vyp+zbeta;
+  else
+    zbetaw=zbeta_vyp+zbeta;
+
+  if (zbetaw>MAXbeta)
+    zbetaw=MAXbeta;
+  if (zbetaw<-MAXbeta)
+    zbetaw=-MAXbeta; /**/
+
+  // urceni polohy vektoru napeti v souradnem systemu (d,q)
+  if (rychlost<0)
+    alfa=zbetaw-M_PI/2.;
+  else
+    alfa=zbetaw+M_PI/2.;
+}
+///////////////////// konec kodu VARIANTY 2 //////////////////////////
+
+// KOD SPOLECNY PRO OBE REGULACNI STRUKTURY
+  // vypocet polohy vektoru napeti ve stojicim souradnem systemu
+  beta=alfa+theta;
+
+  // vypocet Urmf pro dalsi periodu vzorkovani
+  filtr(Um/Ucn_2,&Urmf,Treg/Tfurm);
+  
+  // vypocet velikosti toku a momentu
+  Fs=Fmag+K_Fs*Isd;
+  moment=K_moment*Isq;
+
+  // test filtru	- umisteni filtru nema vubec vliv na kvalitu regulace algoritmy var. 2
+/*  if (start_reg==0)
+  {
+	filtr(Uc,&Ucf,Treg/Tfuc);
+	filtr(Isd,&Isdf,Treg/Tfid);
+  } */
+
+/////// PRECHOD ZPET DO SIMULACE //////////////
+
+// Odblokovat pri pouziti PWM_FULL_NEW()
+//  *u=ud*cos(theta)-uq*sin(theta);
+//  *(u+1)=ud*sin(theta)+uq*cos(theta);
+// Odblokovat pri pouziti PWM_FULL_NEW_3h()
+  *u=Um;
+  *(u+1)=beta;
+
+  ladeni_regulace[0]=Idw_urm;
+  ladeni_regulace[1]=Urmf;
+  ladeni_regulace[2]=Iqw_reg;
+  ladeni_regulace[3]=Um;
+  ladeni_regulace[4]=Fs;
+  ladeni_regulace[5]=moment;
+  ladeni_regulace[6]=zbetaw;
+  ladeni_regulace[7]=Um/Ucn_2;
+  ladeni_regulace[8]=Ukomp;
+  ladeni_regulace[9]=ALGORITMUS;
+}
+
+void reset_regulatoru(void)
+{
+  Surm=0;
+  Sid=0;
+  Siq=0;
+}
Index: /simulator_zdenek/ekf_example/data/kresli_ladeni.m
===================================================================
--- /simulator_zdenek/ekf_example/data/kresli_ladeni.m (revision 50)
+++ /simulator_zdenek/ekf_example/data/kresli_ladeni.m (revision 50)
@@ -0,0 +1,93 @@
+load graf1.txt -ascii
+
+% t, Isx, Isy, Isd, Isdw, Isq, Isqw, M, Fs, poloha, rychlost, rychlost_w,
+% Urm, Urm_max, beta, zbeta, Uc
+
+figure
+plot(graf1(:,1),graf1(:,2))     % Isx
+% hold on
+% plot(graf1(:,1),graf1(:,3),'r')
+legend('Isx');
+
+figure
+plot(graf1(:,1),graf1(:,4))     % Isd
+hold on
+plot(graf1(:,1),graf1(:,5),'r')    
+legend('Isd','Isdw');
+
+figure
+plot(graf1(:,1),graf1(:,6))     % Isq
+hold on
+plot(graf1(:,1),graf1(:,7),'r')   
+legend('Isq','Isqw');
+
+figure
+plot(graf1(:,1),graf1(:,8))     % M
+hold on
+plot(graf1(:,1),graf1(:,16),'r')     % M_model
+legend('Moment','Moment model');
+
+figure
+plot(graf1(:,1),graf1(:,9))     % Fs
+hold on
+plot(graf1(:,1),graf1(:,15),'r')     % Fs_model
+legend('Fs','Fs model');
+
+figure
+plot(graf1(:,1),graf1(:,10))     % Poloha
+title('Poloha theta elektricka');
+
+figure
+plot(graf1(:,1),graf1(:,11)/(2*pi))     % fme
+hold on
+plot(graf1(:,1),graf1(:,12)/(2*pi),'r')     
+legend('fme','fmew');
+
+figure
+plot(graf1(:,1),graf1(:,13))     % Urm
+hold on
+plot(graf1(:,1),graf1(:,14),'r')     
+legend('Urm','Urm max');
+
+figure
+plot(graf1(:,1),graf1(:,17)/pi*180)     % zatezny uhel (beta)
+legend('zatezny uhel (beta)');
+
+figure
+plot(graf1(:,1),graf1(:,18)/pi*180)     % zatezny uhel (zbeta) - vypocet z napeti
+legend('zatezny uhel (zbeta)');
+
+figure
+plot(graf1(:,1),graf1(:,19))     % Uc
+legend('Uc');
+
+figure
+plot(graf1(:,1),graf1(:,20))     % Um*2/Ucn
+legend('Um*2/Ucn');
+
+figure
+plot(graf1(:,1),graf1(:,21))     % Upila
+hold on
+plot(graf1(:,1),graf1(:,22),'r')     % Ua
+legend('Upila','Ua');
+
+figure
+plot(graf1(:,1),graf1(:,23))     % Ukomp
+legend('Ukomp');
+
+figure
+plot(graf1(:,1),graf1(:,24))     % ALGORITMUS
+legend('ALGORITMUS');
+
+figure
+plot(graf1(:,1),graf1(:,25)/2/pi)     % fme_ekf
+hold on
+plot(graf1(:,1),graf1(:,11)/(2*pi),'r')     % fme skutecna
+legend('fme ekf','fme');
+
+figure
+plot(graf1(:,1),graf1(:,26))     % theta_elektricka_ekf
+hold on
+plot(graf1(:,1),graf1(:,10),'r')     % Poloha elektricka skutecna
+legend('Theta ekf','Theta real');
+
Index: /simulator_zdenek/ekf_example/nastaveni_regulatoru_float.h
===================================================================
--- /simulator_zdenek/ekf_example/nastaveni_regulatoru_float.h (revision 50)
+++ /simulator_zdenek/ekf_example/nastaveni_regulatoru_float.h (revision 50)
@@ -0,0 +1,65 @@
+/***********************************************
+
+     Nastaveni regulatoru vektoroveho rizeni
+
+     Konstanty a omezovace
+
+     Z. Peroutka
+
+Rev. 31.5.2007
+***********************************************/
+
+// maximalni proud
+#define Ismax   40.0    // max. proud motoru
+
+// regulatory proudu
+#define Kpi     20.            // Kp*Iref/Uref
+#define Tii     0.005           // casova konstanta reg. ... Kp*Iref/Uref*TV/Ti
+#define MAXi    1199.0
+#define Q_Kpi   15
+#define Q_Kii   15
+
+// regulator odbuzovani - RUrm  Kp=15, Ti=0.5
+#define Kpurm   50.0           // 2.0/Iref ev. sqrt(3)/Iref pro PWM se 3. harmonickou
+#define Tiurm   0.05				// 31.5.2007: puvodne 0.01
+#define MINurm  (-0.95*Ismax)   //  0.95 ... aby i p�akci reg. Urm byl motor schopen produkovat moment
+#define Tfurm   0.01            // casova konstanta filtru Urm
+#define Q_Kpurm 12
+#define Q_Kiurm 15
+#define Q_Kturm 15              // TV/Tf  ... nechat Q15!!!!
+
+// blok "Vypocet napeti" - "odvazbovaci blok" ... definice systemu
+#define Q_Kodv_ud       15              // Wref*Iref/Uref*Ls
+
+// regulator rychlosti
+#define Kpw     3.0                     // Kpw*Wref/Iref
+#define Tiw     0.1
+#define MAXw    Ismax
+#define Q_Kpw   7
+#define Q_Kiw   15
+
+// model motoru pro vypocet toku a momentu
+#define Q_K_Fs          15      // max. Q15 ... Ls*Iref/Fref
+#define Q_K_moment      12      // max. Q15 ... kp*p*Fref*Iref/Mref
+
+
+// filtrace napeti v ss obvodu
+#define Tfuc	0.1
+
+// filtrace Isd
+#define Tfid	0.1
+
+// prechod mezi algoritmy regulace VARIANTA 1 a VARIANTA 2
+#define	prechod_1_2		(2.*M_PI*75.)		// 55Hz ... wme [rad/s]
+#define prechod_2_1		(2.*M_PI*70.)		// 50Hz ... wme [rad/s]
+
+// regulator Isq prostednictvim beta - algoritmus VARIANTA 2: Karel (Kp=0.0002, Ti=0.001s)
+#define Kpib	0.005
+#define Tiib	0.01			
+#define MAXbeta	(40./180.*M_PI)		// zatezny uhel omezen na +/- 40st.
+
+// regulator Isd - VAR2 - kompenzace vypocteneho napeti
+#define	Kpidb	0
+#define	Tiidb	0.1
+#define MAXud	50.
+#define MINud	(-600.)		// principialne neni nutne omezovat, avsak s ohledem na format Q13 s dostatecnou bezpecnosti uvazovano 600V
Index: /simulator_zdenek/ekf_example/regulace.h
===================================================================
--- /simulator_zdenek/ekf_example/regulace.h (revision 50)
+++ /simulator_zdenek/ekf_example/regulace.h (revision 50)
@@ -0,0 +1,17 @@
+/*********************************
+
+        Vektorove rizeni
+
+        regulacni struktura
+
+        Z. Peroutka
+
+Rev. 13.10.2006
+**********************************/
+
+extern void init_regulace(double *param, double TV);
+//extern void vektor_regulace(double Idw, double Iqw, double Urm_max, double Ww, double *u, double Isx, double Isy, double theta, double rychlost, double Ucn_2, unsigned int rezim);
+extern void vektor_regulace(double Idw, double Iqw, double Urm_max, double Ww, double *u, double Isx, double Isy, double theta, double rychlost, double Ucn_2, double Uc, double Ucn, unsigned int rezim);
+
+extern double ladeni_regulace[10];
+// Idw_urm, Urmf, Iqw_reg 
