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

Revision 1201, 12.8 kB (checked in by smidl, 14 years ago)

verbose ekf

  • 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,15);
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,Qm);
404        uf[1]=prevod(uy/Uref,Qm);
405       
406        int Y_mes[2];
407        // zadani mereni
408        Y_mes[0]=prevod(isxd/Iref,Qm);
409        Y_mes[1]=prevod(isyd/Iref,Qm);
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;
421        x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*(uf[0]<<2))>>15;
422        x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*(uf[1]<<2))>>15;
423        x_est[2]=x_est[2];
424        x_est[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
425       
426        if(x_est[3]>(1<<15)) x_est[3]-=2*(1<<15);
427        if(x_est[3]<-(1<<15)) x_est[3]+=2*(1<<15);
428       
429        //void EKFfixed::update_psi(void)
430        {               
431                PSI[2]=((long)cB*t_sin)>>15;
432                tmp=((long)cB*x_est[2])>>15;
433                PSI[3]=((long)tmp*t_cos)>>15;
434                PSI[6]=-((long)cB*t_cos)>>15;
435                PSI[7]=((long)tmp*t_sin)>>15;
436        }
437        {
438                ivec Ad(PSI,16);
439                log_level.store(logA,get_from_ivec(Ad));
440        }
441       
442        ///////// end of copy ///////////////
443        mmultAU(PSI,Uf,PSIU,4,4);
444        //thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
445        thorton(Uf,Df,PSIU,Q,G,Dfold,4);
446       
447         {
448                ivec Ud(Uf,16);
449                log_level.store(logU,get_from_ivec(Ud));
450        }
451         {
452                ivec Gd(G,16);
453                log_level.store(logG,get_from_ivec(Gd));
454        }
455       
456       
457        int difz[2];
458        difz[0]=(Y_mes[0]<<2)-x_est[0]; // Y_mes in q13!!
459        difz[1]=(Y_mes[1]<<2)-x_est[1];
460
461        {
462                vec dd(4);dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1];
463                log_level.store(logD,dd);
464        }
465       
466        //bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx );
467        int dR[2];dR[0]=R[0];dR[1]=R[3];
468        bierman(difz,x_est,Uf,Df,dR,2,4);
469       
470        // navrat estimovanych hodnot regulatoru
471        vec& mu = E._mu();
472        (mu)(0)=zprevod(x_est[0],15)*Iref;
473        (mu)(1)=zprevod(x_est[1],15)*Iref;
474        (mu)(2)=zprevod(x_est[2],15)*Wref;
475        (mu)(3)=zprevod(x_est[3],15)*Thetaref;
476       
477//      mat T=diag(concat(vec_2(Iref,Iref), vec_2(Wref,Thetaref)));
478}
479
480void EKFfixedUD::init_ekf(double Tv)
481{
482        // Tuning of matrix Q
483        Q[0]=prevod(.01,15);       // 0.05
484        Q[5]=Q[0];
485        Q[10]=prevod(0.0001,15);      // 1e-3
486        Q[15]=prevod(0.0001,15);      // 1e-3
487
488        Uf[0]=0x7FFF;       // 0.05
489        Uf[1]=Uf[2]=Uf[3]=Uf[4]=0;
490        Uf[5]=0x7FFF;
491        Uf[6]=Uf[6]=Uf[8]=Uf[9]=0;
492        Uf[10]=0x7FFF;      // 1e-3
493        Uf[11]=Uf[12]=Uf[13]=Uf[4]=0;
494        Uf[15]=0x7FFF;      // 1e-3
495       
496        Df[0]=0x7FFF;
497        Df[1]=0x7FFF;
498        Df[2]=0x7FFF;
499        Df[3]=0x7FFF;
500       
501        // Tuning of matrix R
502        R[0]=prevod(0.05,15);         // 0.05
503        R[3]=R[0];
504       
505        // Motor model parameters
506        cA=prevod(1-Tv*Rs/Ls,15);
507        cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);
508        cC=prevod(Tv/Ls/Iref*Uref,15);
509        //  cD=prevod(1-Tv*Bf/J,15);
510        //  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15);
511        //  cF=prevod(p*Tv*Mref/J/Wref,15);
512        cG=prevod(Tv*Wref/Thetaref,15); //no *4!!
513        //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
514        // cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); <<< use cB instead
515        //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
516       
517        /* Init matrix PSI with permanently constant terms */
518        PSI[0]=cA;
519        PSI[5]=PSI[0];
520        PSI[10]=0x7FFF;
521        PSI[14]=cG;
522        PSI[15]=0x7FFF;
523       
524        //////////////////////// =================
525        ///// TEST thorton vs. thorton_fast
526       
527/*      int Ut[16]={0x7FFF, 100, 200, 300, 0, 0x7FFF, 500, 600, 0,0,0x7FFF, 800, 0,0,0, 0x7FFF};
528        int Dt[4]={100,200,300,400};
529        int PSIu[16] = {100, 200,300, 400 , 500, 600,700,800, 900,1000,1100,1200, 1300,1400,1500,1600};
530        int Dold[4];
531       
532        thorton(Ut,Dt,PSIu,Q,G,Dold,4);
533        int Ut2[16]={0x7FFF, 100, 200, 300, 0, 0x7FFF, 500, 600, 0,0,0x7FFF, 800, 0,0,0, 0x7FFF};
534        int Dt2[4]={100,200,300,400};
535        int PSIu2[16] = {100, 200,300, 400 , 500, 600,700,800, 900,1000,1100,1200, 1300,1400,1500,1600};
536        thorton_fast(Ut2,Dt2,PSIu2,Q,G,Dold,4);
537        cout<< Q<<endl;*/
538}
Note: See TracBrowser for help on using the browser.