// Bayesian Filtering for PMSM using Marginalized Particle Filter 

#include "parametry_motoru.h"
#define Fpm 0.1989
//#define pi 3.141596

#define N 4

#define Lsd Ls*0.9
#define Lsq Ls
#define _dt 0.000125
#define _ad (1.-Rs*_dt/(Lsd))
#define _aq (1.-Rs*_dt/(Lsq))
#define _b  Fpm/Lsq*_dt
#define _cd  _dt/(Lsd)
#define _cq  _dt/(Lsq)


# define floatx float

floatx qrandn();
floatx qrandu();
void rng_init();

void mpf_bayes ( const floatx isa, const floatx isb , const floatx usa, const floatx usb );
void mpf_init(floatx qom0, floatx qth0, floatx r0);
void mpf_mean(floatx *Ecosth, floatx *Esinth, floatx *Eome);
void mpf_th(floatx th1[N]);
void mpf_om(floatx om1[N]);

floatx kalman_om ( const floatx isa, const floatx isb , const floatx usa, const floatx usb, const floatx th );