root/applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp @ 1229

Revision 1229, 16.0 kB (checked in by smidl, 15 years ago)

change in Q

  • Property svn:eol-style set to native
Line 
1
2#include <estim/kalman.h>
3
4#include "ekf_obj.h"
5#include "../simulator.h"
6
7double minQ(double Q){if (Q>1.0){ return 1.0;} else {return Q;};};
8
9void mat_to_int(const imat &M, int *I){
10        for (int i=0;i<M.rows(); i++){
11                for (int j=0;j<M.cols(); j++){
12                        *I++ = M(i,j);
13                }
14        }
15}
16void vec_to_int(const ivec &v, int *I){
17        for (int i=0;i<v.length(); i++){
18                        *I++ = v(i);
19        }
20}
21
22///////////////
23void EKFfixed::bayes(const vec &yt, const vec &ut){
24        ekf(yt(0),yt(1),ut(0),ut(1));
25       
26        vec xhat(4);   
27        //UGLY HACK!!! reliance on a predictor!!
28        xhat(0)=zprevod(x_est[0],Qm)*Iref;
29        xhat(1)=zprevod(x_est[1],Qm)*Iref;
30        xhat(2)=zprevod(x_est[2],Qm)*Wref;
31        xhat(3)=zprevod(x_est[3],15)*Thetaref;
32       
33        E.set_mu(xhat);
34       
35        if ( BM::evalll ) {
36/*              //from enorm
37                vec xdif(x,4);//first 4 of x
38                //UGLY HACK!!! reliance on a predictor!!
39/*              xdif(0)=x[0]-zprevod(x_pred[0],Qm)*Iref;
40                xdif(1)=x[1]-zprevod(x_pred[1],Qm)*Iref;
41                xdif(2)=x[2]-zprevod(x_pred[2],Qm)*Wref;
42                xdif(3)=x[3]-zprevod(x_pred[3],15);*/
43               
44//              xdif -=xhat; //(xdif=x-xhat)
45               
46                mat Pfull(4,4);
47                double* Pp=Pfull._data();
48                for(int i=0;i<16;i++){*(Pp++) = zprevod(P_est[i],15);}
49               
50                E._R()._M()=Pfull;
51               
52               
53//              BM::ll = -0.5* ( 4 * 1.83787706640935 +log ( det ( Pfull ) ) +xdif* ( inv(Pfull)*xdif ) );*/
54        }
55};
56
57
58void EKFfixed::update_psi(void)
59{
60  int t_sin,t_cos,tmp;
61
62  // implementace v PC
63  t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
64  t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
65
66  PSI[2]=((long)cB*t_sin)>>15;
67  tmp=((long)cH*x_est[2])>>15;
68  PSI[3]=((long)tmp*t_cos)>>15;
69  PSI[6]=-((long)cB*t_cos)>>15;
70  PSI[7]=((long)tmp*t_sin)>>15;
71}
72
73
74void EKFfixed::prediction(int *ux)
75{
76  int t_sin,t_cos, tmp;
77
78  // implementace v PC
79  //t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
80  //t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
81
82  t_sin=prevod(sin(Thetaref*x_est[3]/Qm),15);
83  t_cos=prevod(cos(Thetaref*x_est[3]/Qm),15);
84 
85  tmp=((long)cB*x_est[2])>>15;
86  x_pred[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*ux[0])>>15;
87  x_pred[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*ux[1])>>15;
88  x_pred[2]=x_est[2];
89  x_pred[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
90
91  update_psi();
92
93  mmult(PSI,P_est,temp15a,3,3,3);
94//  mtrans(PSI,temp15b,5,5);
95  mmultt(temp15a,PSI,P_pred,3,3,3);
96  maddD(P_pred,Q,3,3);
97}
98
99void EKFfixed::correction(void)
100{
101  int Y_error[2];
102  long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */
103
104  choice_P(P_pred,temp15a,3);
105  maddD(temp15a,R,1,1);
106  minv2(temp15a,temp30a);
107        Ry(0,0)=zprevod(temp15a[0],15);
108        Ry(0,1)=zprevod(temp15a[1],15);
109        Ry(1,0)=zprevod(temp15a[2],15);
110        Ry(1,1)=zprevod(temp15a[3],15);
111 
112  mmultDr(P_pred,temp15a,3,3,1,1);
113  mmult1530(temp15a,temp30a,Kalm,3,1,1);
114
115
116  /* estimate the state system */
117  choice_x(x_pred, temp15a);
118  msub(Y_mes,temp15a,Y_error,1,0);
119  mmult(Kalm,Y_error,temp15a,3,1,0);
120  madd(x_pred,temp15a,x_est,3,0);
121
122  /* matrix of covariances - version without MMULTDL() */
123
124/* Version with MMULTDL() */
125  mmultDl(P_pred,temp15a,1,3,3,1);
126
127  mmult(Kalm,temp15a,P_est,3,1,3);
128  msub(P_pred,P_est,P_est,3,3);
129/* END */
130}
131
132
133void EKFfixed::ekf(double ux, double uy, double isxd, double isyd)
134{
135  // vypocet napeti v systemu (x,y)
136  ukalm[0]=prevod(ux/Uref,Qm);
137  ukalm[1]=prevod(uy/Uref,Qm);
138
139  // zadani mereni
140  Y_mes[0]=prevod(isxd/Iref,Qm);
141  Y_mes[1]=prevod(isyd/Iref,Qm);
142
143  ////// vlastni rutina EKF /////////////////////////
144  prediction(ukalm);
145  correction();
146
147  // navrat estimovanych hodnot regulatoru
148  vec& mu = E._mu();
149  (mu)(0)=zprevod(x_est[0],Qm)*Iref;
150  (mu)(1)=zprevod(x_est[1],Qm)*Iref;
151  (mu)(2)=zprevod(x_est[2],Qm)*Wref;
152  (mu)(3)=zprevod(x_est[3],15)*Thetaref;
153}
154
155void EKFfixed::init_ekf(double Tv)
156{
157  // Tuning of matrix Q
158  Q[0]=prevod(.05,15);       // 0.05
159  Q[5]=Q[0];
160  Q[10]=prevod(0.0002,15);      // 1e-3
161  Q[15]=prevod(0.001,15);      // 1e-3
162
163  // Tuning of matrix R
164  R[0]=prevod(0.1,15);         // 0.05
165  R[3]=R[0];
166
167  // Motor model parameters
168  cA=prevod(1-Tv*Rs/Ls,15);
169  cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);
170  cC=prevod(Tv/Ls/Iref*Uref,15);
171//  cD=prevod(1-Tv*Bf/J,15);
172//  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15);
173//  cF=prevod(p*Tv*Mref/J/Wref,15);
174  cG=prevod(Tv*Wref*4/Thetaref,15);
175//  cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
176  cH=prevod(Tv*Wref*Fmag/Iref/Ls,17); //cB in q
177  //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
178
179  /* Init matrix PSI with permanently constant terms */
180  PSI[0]=cA;
181  PSI[5]=PSI[0];
182  PSI[10]=0x7FFF;
183  PSI[14]=cG;
184  PSI[15]=0x7FFF;
185 
186  P_est[0]=0x7FFF;
187  P_est[5]=0x7FFF;
188  P_est[10]=0x7FFF;
189  P_est[15]=0x7FFF;
190}
191
192
193void EKF_UDfix::set_parameters ( const shared_ptr<diffbifn> &pfxu0, const shared_ptr<diffbifn> &phxu0, const mat Q0, const vec R0 ) {
194        pfxu = pfxu0;
195        phxu = phxu0;
196       
197        set_dim ( pfxu0->_dimx() );
198        dimy = phxu0->dimension();
199        dimc = pfxu0->_dimu();
200       
201        vec &_mu = est._mu();
202        // if mu is not set, set it to zeros, just for constant terms of A and C
203        if ( _mu.length() != dimension() ) _mu = zeros ( dimension() );
204        A = zeros ( dimension(), dimension() );
205        C = zeros ( dimy, dimension() );
206       
207        //initialize matrices A C, later, these will be only updated!
208        pfxu->dfdx_cond ( _mu, zeros ( dimc ), A, true );
209        //      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );
210        phxu->dfdx_cond ( _mu, zeros ( dimc ), C, true );
211        //      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
212       
213        R = R0;
214        Q = Q0;
215       
216        //
217}
218// aux fnc
219void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref){
220        mat P= U*diag(D)*U.T();
221        mat T = diag(1.0/(xref));
222        mat Pf = T*P*T;
223       
224        ldmat Pld(Pf);
225       
226        mat Ut=Pld._L().T()*(1<<15); // U is in q15 -- diagonal is 0!!!
227        Uf=round_i(Ut);
228        Df=round_i(Pld._D()*(1<<15));
229        ivec zer=find(Df==0);
230        for(int i=0; i<zer.length(); i++) Df(zer(i))=1;
231}
232
233
234void EKF_UDfix::bayes ( const vec &yt, const vec &cond ) {
235        //preparatory
236        vec &_mu=est._mu();
237        const vec &u=cond;
238        int dim = dimension();
239        ///// !!!!!!!!!!!!!!!!
240        U = est._R()._L().T();
241        D =  est._R()._D();
242       
243        ////////////
244       
245        pfxu->dfdx_cond ( _mu, u, A, false ); //update A by a derivative of fx
246        phxu->dfdx_cond ( _mu, u, C, false ); //update A by a derivative of fx
247       
248        mat PhiU = A*U;
249
250        //////
251/*      vec xref(4);
252        xref(0)= 30.0*1.4142;
253        xref(1)= 30.0*1.4142;
254        xref(2)= 6.283185*200.;
255        xref(3) = 3.141593;*/
256        //xref(4) = 34.0;
257       
258       
259        vec Din = D;
260        int i,j,k;
261        double sigma;
262        mat G = eye(dim);
263        //////// thorton
264       
265        //move mean;
266        _mu = pfxu->eval(_mu,u);
267       
268        for (i=dim-1; i>=0;i--){
269                sigma = 0.0; 
270                for (j=0; j<dim; j++) {
271                        sigma += PhiU(i,j)*PhiU(i,j) *Din(j); 
272                        sigma += G(i,j)*G(i,j) * Q(j,j);
273                }
274               
275/*              double sigma2= 0.0;
276                for (j=0; j<dim; j++) {
277                        sigma2 += PhiU(i,j)*PhiU(i,j) *Din(j);
278                }
279                sigma2 +=Q(i,i);//*G(i,i)=1.0
280                for (j=i+1; j<dim; j++) {
281                        sigma2 += G(i,j)*G(i,j) * Q(j,j);
282                }*/
283                D(i) = sigma; 
284               
285        /*      UDtof(U,D,Utf,Dtf,xref);
286                cout << "d=sig"<<endl;
287                cout << Dtf << endl;
288        */     
289                for (j=0;j<i;j++){ 
290//                      cout << i << "," << j << endl;
291                        sigma = 0.0; 
292                        for (k=0;k<dim;k++){ 
293                                sigma += PhiU(i,k)*Din(k)*PhiU(j,k); 
294                        }
295                        for (k=0;k<dim;k++){ 
296                                sigma += G(i,k)*Q(k,k)*G(j,k); 
297                        }
298                        //
299                        U(j,i) = sigma/D(i); 
300                       
301/*                      cout << "U=sig/D"<<endl;
302                        UDtof(U,D,Utf,Dtf,xref);
303                        cout << Utf << endl << Dtf << endl;
304                        cout << G << endl << Din << endl<<endl;*/
305                       
306                        for (k=0;k<dim;k++){ 
307                                PhiU(j,k) = PhiU(j,k) - U(j,i)*PhiU(i,k); 
308                        }
309                        for (k=0;k<dim;k++){ 
310                                G(j,k) = G(j,k) - U(j,i)*G(i,k); 
311                        }
312               
313                }
314        }
315
316        // bierman
317       
318        double dz,alpha,gamma,beta,lambda;
319        vec a;
320        vec b;
321        vec yp = phxu->eval(_mu,u);
322        vec xp=_mu; // used in bierman
323       
324       
325        for (int iy=0; iy<dimy; iy++){
326                a     = U.T()*C.get_row(iy);    // a is not modified, but
327                b     = elem_mult(D,a);                          // b is modified to become unscaled Kalman gain.
328                dz    = yt(iy) - yp(iy); 
329               
330                alpha = R(iy); 
331                gamma = 1/alpha; 
332                for (j=0;j<dim;j++){
333                        beta   = alpha; 
334                        alpha  = alpha + a(j)*b(j); 
335                        lambda = -a(j)*gamma; 
336                        gamma  = 1.0/alpha; 
337                        D(j) = beta*gamma*D(j); 
338                       
339                        //                      cout << "a: " << alpha << "g: " << gamma << endl;
340                        for (i=0;i<j;i++){
341                                beta   = U(i,j); 
342                                U(i,j) = beta + b(i)*lambda; 
343                                b(i)   = b(i) + b(j)*beta; 
344                        }
345                }
346                double dzs = gamma*dz;  // apply scaling to innovations
347                _mu   = _mu + dzs*b; // multiply by unscaled Kalman gain
348                //cout << "Ub: " << U << endl;
349                //cout << "Db: " << D << endl <<endl;
350               
351        }
352
353       
354        /////
355        est._R().__L()=U.T();
356        est._R().__D()=D;
357       
358        if ( evalll == true ) { //likelihood of observation y
359        }
360}
361
362void EKF_UDfix::from_setting ( const Setting &set ) {
363        BM::from_setting ( set );
364        shared_ptr<diffbifn> IM = UI::build<diffbifn> ( set, "IM", UI::compulsory );
365        shared_ptr<diffbifn> OM = UI::build<diffbifn> ( set, "OM", UI::compulsory );
366       
367        //statistics
368        int dim = IM->dimension();
369        vec mu0;
370        if ( !UI::get ( mu0, set, "mu0" ) )
371                mu0 = zeros ( dim );
372       
373        mat P0;
374        vec dP0;
375        if ( UI::get ( dP0, set, "dP0" ) )
376                P0 = diag ( dP0 );
377        else if ( !UI::get ( P0, set, "P0" ) )
378                P0 = eye ( dim );
379       
380        est._mu()=mu0;
381        est._R()=ldmat(P0);
382       
383        //parameters
384        vec dQ, dR;
385        UI::get ( dQ, set, "dQ", UI::compulsory );
386        UI::get ( dR, set, "dR", UI::compulsory );
387        set_parameters ( IM, OM, diag ( dQ ), dR  );
388       
389        UI::get(log_level, set, "log_level", UI::optional);
390}
391
392
393void EKFfixedUD::bayes(const itpp::vec& yt, const itpp::vec& ut)
394{
395ekf(ut[0],ut[1],yt[0],yt[1]);
396}
397
398
399void EKFfixedUD::ekf(double ux, double uy, double isxd, double isyd)
400{
401        // vypocet napeti v systemu (x,y)
402        int uf[2];
403        uf[0]=prevod(ux/Uref,15);
404        uf[1]=prevod(uy/Uref,15);
405       
406        int Y_mes[2];
407        // zadani mereni
408        Y_mes[0]=prevod(isxd/Iref,15);
409        Y_mes[1]=prevod(isyd/Iref,15);
410               
411        ////// vlastni rutina EKF -- /////////////////////////
412        int t_sin,t_cos, tmp;
413       
414        // implementace v PC
415/*      t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
416        t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/
417        t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
418        t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
419       
420        tmp=((long)cB*x_est[2])>>15; // q15*q13 -> q13
421        //             q15*q13 +          q13*q15      + q15*q13??
422        x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*(uf[0]))>>15; // in q13
423        x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*(uf[1]))>>15; // q13
424        x_est[2]=x_est[2];
425        //               q15              + q15*q13
426        x_est[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
427       
428        if(x_est[3]>(1<<15)) x_est[3]-=2*(1<<15);
429        if(x_est[3]<-(1<<15)) x_est[3]+=2*(1<<15);
430       
431        //void EKFfixed::update_psi(void)
432        {       
433                PSI[2]=((long)cB*t_sin)>>15;
434                tmp=((long)cH*x_est[2])>>15; // ! cH =cB with different scale!!
435                PSI[3]=((long)tmp*t_cos)>>15;
436                PSI[6]=-((long)cB*t_cos)>>15;
437                PSI[7]=((long)tmp*t_sin)>>15;
438        }
439        {
440                ivec Ad(PSI,16);
441                log_level.store(logA,get_from_ivec(Ad));
442        }
443       
444        ///////// end of copy ///////////////
445        mmultAU(PSI,Uf,PSIU,4,4);
446        //thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
447        thorton(Uf,Df,PSIU,Q,G,Dfold,4);
448       
449         {
450                ivec Ud(Uf,16);
451                log_level.store(logU,get_from_ivec(Ud));
452        }
453         {
454                ivec Gd(G,16);
455                log_level.store(logG,get_from_ivec(Gd));
456        }
457       
458       
459        int difz[2];
460        difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!!
461        difz[1]=(Y_mes[1]-x_est[1]);//<<2;
462
463        {
464                ivec dd(Df,4);//;dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1];
465                log_level.store(logD,get_from_ivec(dd));
466        }
467       
468        //bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx );
469        int dR[2];dR[0]=R[0];dR[1]=R[3];
470        //int xb[4]; xb[0]=x_est[0]<<2; xb[1]=x_est[1]<<2;  xb[2]=x_est[2]<<2;  xb[3]=x_est[3]; 
471        bierman(difz,x_est,Uf,Df,dR,2,4);
472        //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3];
473       
474        // navrat estimovanych hodnot regulatoru
475        vec& mu = E._mu();
476        (mu)(0)=zprevod(x_est[0],15)*Iref;
477        (mu)(1)=zprevod(x_est[1],15)*Iref;
478        (mu)(2)=zprevod(x_est[2],15)*Wref;
479        (mu)(3)=zprevod(x_est[3],15)*Thetaref;
480       
481        //x_est[2]=x[2]*32768/Wref;
482        //x_est[3]=x[3]*32768/Thetaref;
483//      mat T=diag(concat(vec_2(Iref,Iref), vec_2(Wref,Thetaref)));
484}
485
486void EKFfixedUD::init_ekf(double Tv)
487{
488        // Tuning of matrix Q
489        Q[0]=prevod(.0005,15);       // 0.05
490        Q[5]=Q[0];
491        Q[10]=prevod(0.0001,15);      // 1e-3
492        Q[15]=prevod(0.0001,15);      // 1e-3
493
494        Uf[0]=0x7FFF;// !       // 0.05
495        Uf[1]=Uf[2]=Uf[3]=Uf[4]=0;
496        Uf[5]=0x7FFF;//!
497        Uf[6]=Uf[6]=Uf[8]=Uf[9]=0;
498        Uf[10]=0x7FFF;//!      // 1e-3
499        Uf[11]=Uf[12]=Uf[13]=Uf[4]=0;
500        Uf[15]=0x7FFF;      // 1e-3
501       
502        Df[0]=1<<14;
503        Df[1]=1<<14;
504        Df[2]=1<<14;
505        Df[3]=1<<14;
506       
507        // Tuning of matrix R
508        R[0]=prevod(0.05,15);         // 0.05
509        R[3]=R[0];
510       
511        // Motor model parameters
512        cA=prevod(1-Tv*Rs/Ls,15);
513        cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);
514        cC=prevod(Tv/Ls/Iref*Uref,15);
515        //  cD=prevod(1-Tv*Bf/J,15);
516        //  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15);
517        //  cF=prevod(p*Tv*Mref/J/Wref,15);
518        cG=prevod(Tv*Wref/Thetaref,15); //in q15!
519        //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
520        cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); //
521        //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
522       
523        /* Init matrix PSI with permanently constant terms */
524        PSI[0]=cA;
525        PSI[5]=PSI[0];
526        PSI[10]=0x7FFF;
527        PSI[14]=cG;
528        PSI[15]=0x7FFF;
529       
530}
531
532void EKFfixedCh::bayes(const itpp::vec& yt, const itpp::vec& ut)
533{
534        ekf(ut[0],ut[1],yt[0],yt[1]);
535}
536
537
538void EKFfixedCh::ekf(double ux, double uy, double isxd, double isyd)
539{
540        // vypocet napeti v systemu (x,y)
541        int uf[2];
542        uf[0]=prevod(ux/Uref,15);
543        uf[1]=prevod(uy/Uref,15);
544       
545        int Y_mes[2];
546        // zadani mereni
547        Y_mes[0]=prevod(isxd/Iref,15);
548        Y_mes[1]=prevod(isyd/Iref,15);
549       
550        ////// vlastni rutina EKF -- /////////////////////////
551        int t_sin,t_cos, tmp;
552       
553        // implementace v PC
554        /*      t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
555         *      t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/
556        t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
557        t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
558       
559        tmp=((long)cB*x_est[2])>>15; // q15*q13 -> q13
560        //             q15*q13 +          q13*q15      + q15*q13??
561        x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*(uf[0]))>>15; // in q13
562        x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*(uf[1]))>>15; // q13
563        x_est[2]=x_est[2];
564        //               q15              + q15*q13
565        x_est[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
566       
567        if(x_est[3]>(1<<15)) x_est[3]-=2*(1<<15);
568        if(x_est[3]<-(1<<15)) x_est[3]+=2*(1<<15);
569       
570        //void EKFfixed::update_psi(void)
571        {       
572                PSI[2]=((long)cB*t_sin)>>15;
573                tmp=((long)cH*x_est[2])>>15; // ! cH =cB with different scale!!
574                PSI[3]=((long)tmp*t_cos)>>15;
575                PSI[6]=-((long)cB*t_cos)>>15;
576                PSI[7]=((long)tmp*t_sin)>>15;
577        }
578        {
579                ivec Ad(PSI,16);
580                log_level.store(logA,get_from_ivec(Ad));
581        }
582       
583        ///////// end of copy ///////////////
584        mmultACh(PSI,Chf,PSICh,4,4);
585        //thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
586        householder(PSICh,Q,4);
587       
588        {
589                ivec Chd(Chf,16);
590                log_level.store(logCh,get_from_ivec(Chd));
591        }
592       
593       
594        int difz[2];
595        difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!!
596        difz[1]=(Y_mes[1]-x_est[1]);//<<2;
597       
598       
599        //
600        int dR[2];dR[0]=R[0];dR[1]=R[3];
601        //int xb[4]; xb[0]=x_est[0]<<2; xb[1]=x_est[1]<<2;  xb[2]=x_est[2]<<2;  xb[3]=x_est[3]; 
602        carlson(difz,x_est,Chf,dR,2,4);
603        //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3];
604       
605        // navrat estimovanych hodnot regulatoru
606        vec& mu = E._mu();
607        (mu)(0)=zprevod(x_est[0],15)*Iref;
608        (mu)(1)=zprevod(x_est[1],15)*Iref;
609        (mu)(2)=zprevod(x_est[2],15)*Wref;
610        (mu)(3)=zprevod(x_est[3],15)*Thetaref;
611       
612        //x_est[2]=x[2]*32768/Wref;
613        //x_est[3]=x[3]*32768/Thetaref;
614        //      mat T=diag(concat(vec_2(Iref,Iref), vec_2(Wref,Thetaref)));
615}
616
617void EKFfixedCh::init_ekf(double Tv)
618{
619        // Tuning of matrix Q
620        Q[0]=prevod(.01,15);       // 0.05
621        Q[5]=Q[0];
622        Q[10]=prevod(0.0001,15);      // 1e-3
623        Q[15]=prevod(0.0001,15);      // 1e-3
624       
625        Chf[0]=0x7FFF;// !       // 0.05
626        Chf[1]=Chf[2]=Chf[3]=Chf[4]=0;
627        Chf[5]=0x7FFF;//!
628        Chf[6]=Chf[6]=Chf[8]=Chf[9]=0;
629        Chf[10]=0x7FFF;//!      // 1e-3
630        Chf[11]=Chf[12]=Chf[13]=Chf[4]=0;
631        Chf[15]=0x7FFF;      // 1e-3
632               
633        // Tuning of matrix R
634        R[0]=prevod(0.05,15);         // 0.05
635        R[3]=R[0];
636       
637        // Motor model parameters
638        cA=prevod(1-Tv*Rs/Ls,15);
639        cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);
640        cC=prevod(Tv/Ls/Iref*Uref,15);
641        //  cD=prevod(1-Tv*Bf/J,15);
642        //  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15);
643        //  cF=prevod(p*Tv*Mref/J/Wref,15);
644        cG=prevod(Tv*Wref/Thetaref,15); //in q15!
645        //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
646        cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); //
647        //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
648       
649        /* Init matrix PSI with permanently constant terms */
650        PSI[0]=cA;
651        PSI[5]=PSI[0];
652        PSI[10]=0x7FFF;
653        PSI[14]=cG;
654        PSI[15]=0x7FFF;
655       
656}
Note: See TracBrowser for help on using the browser.