/************************************
        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;
}