/************************************
        Extended Kalman Filter
        Kalman Observer

        Z. Peroutka

Rev. 25.10.2010

EKF pocitan dle Grewala - Biermanova varianta

30.9.2010       Dodana testovaci rutina test_debug() umoznujici reinicializaci x_est.
25.10.2010      Rev. 2.
28.10.2010		xx


*************************************/

#include "funkce.h"
#include "parametry_motoru.h"         // aktivovat v DSP
#include "reference_Q15.h"
//#include "reference.h"
#include "matrix_vs.h"
#include "ekf_bierman.h"
#include "parametry_motoru.h"
#include "qmath.h"

#define K_PREVOD_FORM   (1<<(15-Qm))               // 2^(15-Qm)
#define pi2_3		21845				// 2/3*PI

#define D_INIT	(1<<qD)

/* Declaration of global functions */
void init_ekf(double Tv);
void ekf(int *x_estimation, int Um, int beta, int Ucn, int Uc, int isx, int isy);

/* Declaration of local functions */
static void prediction(int *ux);
static void correction(void);
static void update_psi(void);

// Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?
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] */

static int PSI[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */

/* Initial conditions and variables */
static int x_est[4]={0,0,0,0};
static int x_pred[4]={0,0,0,0};

static int Y_mes[2]={0,0};
static int ukalm[2]={0,0};

// matice pro Biermana
static int U[16]={0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF}; /* matrix [4,4] */
static int PSIU[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */
static int D[4]={D_INIT,D_INIT,D_INIT,D_INIT};

// constants of PMSM mathematical model
static int cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane

//////////////////////////////////////////////////////////////////////////////////////////////////////
void init_ekf(double Tv)
{
  // Tuning of matrix Q
  // Tuning of matrix Q		... 0.05, 2e-4, 1e-3	// Vencovi dle MPF vyslo, q11=q22=0.023, q33=2e-4, q44=7e-4
/*  Q[0]=prevod(0.02,15);       // 0.05
  Q[5]=Q[0];
  Q[10]=prevod(0.0002,15);     // 0.0002
  Q[15]=prevod(0.001,15);      // 1e-3

  // Tuning of matrix R
  R[0]=prevod(0.1,15);         // 0.1
  R[3]=R[0];
/**/
// 28.10.2010: 0.001; 0.0005; 0.0001
  Q[0]=prevod(0.01,15);         // puvodne: Q11=0.02; Q33=0.0002; Q44=0.001
  Q[5]=Q[0];					 // VS: 0.001; 0.001; 0.0001
  Q[10]=prevod(0.001,15);		// 0.01 na vsech variancich
  Q[15]=prevod(0.0005,15);

  // Tuning of matrix R
  R[0]=prevod(0.05,15);			// puvodni: 0.1; test: 0.08
  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*K_PREVOD_FORM/Thetaref,15);
//  cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
  cH=prevod(Tv*Wref*Fmag/Iref/Ls,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 DSP
  t_sin=qsin(x_est[3]);
  t_cos=qcos(x_est[3]);
//  t_sin=prevod(qsin(Thetaref*x_est[3]/32768.),15);
//  t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);

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


static void prediction(int *ux)
{
  int t_sin,t_cos, tmp;

  int G[16];
  int Dold[4];

  // implementace v DSP
  t_sin=qsin(x_est[3]);
  t_cos=qcos(x_est[3]);
//  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_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();

  // Thorton
  mmultAU(PSI,U,PSIU,4,4);
  thorton_fast(U,D,PSIU,Q,G,Dold,4);
}

static void correction(void)
{
  int Y_error[2];
  unsigned int ii;
  int dR[2];

  // prediction error
  Y_error[0]=Y_mes[0]-x_pred[0];
  Y_error[1]=Y_mes[1]-x_pred[1];

  // copy prediction to state exstimation vector
  for (ii=0;ii<4;ii++) x_est[ii]=x_pred[ii];

  // copy diagonal from matrix R
  dR[0]=R[0];
  dR[1]=R[3];

  // Bierman_fast - correction
  bierman_fast(Y_error,x_est,U,D,dR,2,4);
}


void ekf(int *x_estimation, int Um, int beta, int Ucn, int Uc, int isx, int isy)
{
  int Umk, ua, ub;

  // prepocet napeti pro EKF dle skutecneho napeti v ss obvodu
  Umk=((long)Um*Uc)/Ucn;

  // vypocet napeti v systemu (x,y)
  ukalm[0]=((long)Umk*qcos(beta))>>15;		// usx
  ukalm[1]=((long)Umk*qsin(beta))>>15;		// usy

  // zadani mereni
  Y_mes[0]=isx;
  Y_mes[1]=isy;

  ////// vlastni rutina EKF /////////////////////////
  prediction(ukalm);
  correction();

  // navrat estimovanych hodnot regulatoru
  *x_estimation=x_est[2];
  *(x_estimation+1)=x_est[3];
}

void reset_ekf(void)
{
  int ii;

  for (ii=0;ii<4;ii++) x_est[ii]=0;
  for (ii=0;ii<4;ii++) x_pred[ii]=0;

  for (ii=0;ii<4;ii++) D[ii]=1<<qD;

  for (ii=0;ii<16;ii++) PSIU[ii]=0;
  for (ii=0;ii<16;ii++) U[ii]=0;
  U[0]=0x7FFF;
  U[5]=0x7FFF;
  U[10]=0x7FFF;
  U[15]=0x7FFF;
/**/
}