root/applications/pmsm/simulator_zdenek/ekf_example/ekf.cpp @ 1469

Revision 278, 7.2 kB (checked in by smidl, 16 years ago)

props

  • Property svn:eol-style set to native
Line 
1/************************************
2        Extended Kalman Filter
3        Kalman Observer
4
5        Z. Peroutka
6
7Rev. 15.3.2008
8
9        EKF pocitan s daty ve formatu Q15, zatimco regulace pracuje v Q13 ->
10        resenim je vynasobit referencni hodnoty v EKF 4-mi (tim je automaticky
11        zajisten prechod mezi formaty Q13 a Q15). Realizovano konstantou K_PREVOD_FORM.
12
1315.3.2008       Kontrola kodu + zamena datovych typu q15->int a q30->long.
14
15*************************************/
16
17#include <math.h>
18#include "fixed.h"
19//#include "parametry_motoru.h"         // aktivovat v DSP
20#include "reference.h"
21#include "matrix.h"
22#include "ekf.h"
23
24#define K_PREVOD_FORM   4               // 2^(15-Qm)
25
26/* Declaration of global functions */
27void init_ekf(double Tv, double *param);
28void ekf(double *x_estimation, double Umd, double beta, double Ucnd, double Ucd, double isxd, double isyd);
29
30/* Declaration of local functions */
31static void prediction(int *ux);
32static void correction(void);
33static void update_psi(void);
34
35// global variables
36double ladeni_EKF[10];
37
38/* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
39static int H[8]={0x7FFF,0,0,0,0,0x7FFF,0,0}; /* matrix [2,4] */
40static int Ht[8]={0x7FFF,0,0,0x7FFF,0,0,0,0};/* matrix [4,2] */
41//static int Q[4][4]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */
42//static int R[2][2]={0,0,0,0}; /* matrix [2,2] */
43static int Q[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */
44static int R[4]={0,0,0,0}; /* matrix [2,2] */
45
46/* Initial conditions and variables */
47static int x_est[4]={0,0,0,0};
48static int x_pred[4]={0,0,0,0};
49static int P_pred[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */
50//static int P_est[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */
51static int P_est[16]={0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF}; /* matrix [4,4] */
52static int Y_mes[2]={0,0};
53static int ukalm[2]={0,0};
54static int Kalm[8]; /* matrix [5,2] */
55
56static int PSI[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */
57
58/* Temporary variables */
59static int temp15a[16];
60
61// constants of PMSM mathematical model
62static int cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
63
64//////////////////////////////////////////////////////////////////////////////////////////////////////
65void init_ekf(double Tv, double *param)
66{
67  // parametry simulace - v DSP vyhodit (nahrazeno "parametry_motoru.h")
68  double Rs, Ls, Fmag, kp, p;
69
70 // param=[Rs, Ls, Fmag, Bf, p, kp, J = 0.04?];
71  Rs=*param;
72  Ls=*(param+1);
73  Fmag=*(param+2);
74  p=*(param+4);
75  kp=*(param+5);
76  /// KONEC inicializace pro PC - az sem vyhodit v DSP ///
77
78  // Tuning of matrix Q
79/*  Q[0][0]=prevod(.05,15);       // 1e-2
80  Q[1][1]=Q[0][0];
81  Q[2][2]=prevod(1e-3,15);      // 10e-5
82  Q[3][3]=prevod(1e-3,15);      // model(ureg) ... 4e-3
83
84  // Tuning of matrix R
85  R[0][0]=prevod(0.05,15);         // model(ureg) ... 0.1
86  R[1][1]=R[0][0];
87/**/
88
89  // Tuning of matrix Q
90  Q[0]=prevod(.01,15);       // 0.05
91  Q[5]=Q[0];
92  Q[10]=prevod(0.0001,15);      // 1e-3
93  Q[15]=prevod(0.0001,15);      // 1e-3
94
95  // Tuning of matrix R
96  R[0]=prevod(0.05,15);         // 0.05
97  R[3]=R[0];
98/**/
99
100  /* Tuning of matrix P_est - initial values */
101/*    P_est[0][0]=prevod(0.999,15);
102    P_est[1][1]=prevod(0.999,15);
103    P_est[2][2]=prevod(0.999,15);
104    P_est[3][3]=prevod(0.999,15);
105    P_est[4][4]=prevod(0.999,15);
106/**/
107
108  // Motor model parameters
109  cA=prevod(1-Tv*Rs/Ls,15);
110  cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);
111  cC=prevod(Tv/Ls/Iref*Uref,15);
112//  cD=prevod(1-Tv*Bf/J,15);
113//  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15);
114//  cF=prevod(p*Tv*Mref/J/Wref,15);
115  cG=prevod(Tv*Wref*K_PREVOD_FORM/Thetaref,15);
116  cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
117//  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
118
119  /* Init matrix PSI with permanently constant terms */
120  PSI[0]=cA;
121  PSI[5]=PSI[0];
122  PSI[10]=0x7FFF;
123  PSI[14]=cG;
124  PSI[15]=0x7FFF;
125}
126
127
128static void update_psi(void)
129{
130  int t_sin,t_cos,tmp;
131
132  // implementace v PC
133  t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
134  t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
135
136  // implementace v DSP
137//  t_sin=qsin(x_est[3]);
138//  t_cos=qcos(x_est[3]);
139
140  PSI[2]=((long)cB*t_sin)>>15;
141  tmp=((long)cH*x_est[2])>>15;
142  PSI[3]=((long)tmp*t_cos)>>15;
143  PSI[6]=-((long)cB*t_cos)>>15;
144  PSI[7]=((long)tmp*t_sin)>>15;
145}
146
147
148void prediction(int *ux)
149{
150  int t_sin,t_cos, tmp;
151
152  // implementace v PC
153  t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
154  t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
155
156  // implementace v DSP
157//  t_sin=qsin(x_est[3]);
158//  t_cos=qcos(x_est[3]);
159
160  tmp=((long)cB*x_est[2])>>15;
161  x_pred[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*ux[0])>>15;
162  x_pred[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*ux[1])>>15;
163  x_pred[2]=x_est[2];
164  x_pred[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
165
166  update_psi();
167
168  mmult(PSI,P_est,temp15a,3,3,3);
169//  mtrans(PSI,temp15b,5,5);
170  mmultt(temp15a,PSI,P_pred,3,3,3);
171  maddD(P_pred,Q,3,3);
172}
173
174void correction(void)
175{
176  int Y_error[2];
177  long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */
178
179  /* Kalman gain calculation */
180//  mmult(P_pred,Ht,temp15a,5,5,2);
181/*  mmultt(P_pred,H,temp15a,4,4,1);
182  mmult(H,temp15a,temp15b,1,4,1);       these lines are replaced by choice_P */
183
184  choice_P(P_pred,temp15a,3);
185  maddD(temp15a,R,1,1);
186  minv2(temp15a,temp30a);
187/*  mmultt(P_pred,H,temp15a,4,4,1);     /* remove this line if choice_P is not used */
188//  mmultDr15(P_pred,Ht,temp15a,4,4,1,1);
189  mmultDr(P_pred,temp15a,3,3,1,1);
190  mmult1530(temp15a,temp30a,Kalm,3,1,1);
191
192  /* estimate the state system */
193//  mmult(H,x_pred,temp15a,1,4,0);
194  choice_x(x_pred, temp15a);
195  msub(Y_mes,temp15a,Y_error,1,0);
196  mmult(Kalm,Y_error,temp15a,3,1,0);
197  madd(x_pred,temp15a,x_est,3,0);
198
199  /* matrix of covariances - version without MMULTDL() */
200/*  mmult(Kalm,H,temp15a,4,1,4);
201  mmult(temp15a,P_pred,P_est,4,4,4);
202  msub(P_pred,P_est,P_est,4,4);
203/* END */
204
205/* Version with MMULTDL() */
206  mmultDl(P_pred,temp15a,1,3,3,1);
207/* if result matrix has more terms than DIAG matrix,it is necessary to enable
208erase sequence in definition of function MMULTDL() that is currently disabled. */
209
210  mmult(Kalm,temp15a,P_est,3,1,3);
211  msub(P_pred,P_est,P_est,3,3);
212/* END */
213}
214
215
216void ekf(double *x_estimation, double Umd, double beta, double Ucnd, double Ucd, double isxd, double isyd)
217{
218  int Umk, ua, ub;
219
220  /// prechod ze simulace do fixed-pointu - vyradit v DSP ///////////
221  int Um, Ucn, Uc, isx, isy, t_cos;
222
223  Um=prevod(Umd/Uref,Qm);
224  Ucn=prevod(Ucnd/Uref,Qm);
225  Uc=prevod(Ucd/Uref,Qm);
226  isx=prevod(isxd/Iref,Qm);
227  isy=prevod(isyd/Iref,Qm);
228  /// KONEC emulace FIXED-POINTu - az sem vyradit v DSP ////////////
229
230  // prepocet napeti pro EKF dle skutecneho napeti v ss obvodu
231  Umk=((long)Um*Uc)/Ucn;
232
233  // vypocet fazovych napeti stridace
234  ua=((long)Umk*prevod(cos(beta),15))>>15;
235  ub=((long)Umk*prevod(cos(beta-2./3.*M_PI),15))>>15;
236//  uc=((long)Umk*prevod(cos(beta+2./3.*M_PI),15))>>15;
237
238  // vypocet napeti v systemu (x,y)
239  ukalm[0]=ua;
240  ukalm[1]=(((long)ua+ub<<1)*18917)>>15;                // usy=(ua+2*ub)/sqrt(3)
241
242  // zadani mereni
243  Y_mes[0]=isx;
244  Y_mes[1]=isy;
245
246  ////// vlastni rutina EKF /////////////////////////
247  prediction(ukalm);
248  correction();
249
250  // navrat estimovanych hodnot regulatoru
251  *x_estimation=zprevod(x_est[2],Qm)*Wref;
252//  x_est[3]=(short int)x_est[3];
253  *(x_estimation+1)=zprevod((short int)x_est[3],15)*Thetaref;
254}
Note: See TracBrowser for help on using the browser.