root/applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp @ 1439

Revision 1439, 7.7 kB (checked in by smidl, 12 years ago)

Funkcni 1D Kalman

Line 
1#include "mpf_double.h"
2#include "math.h"
3#include <stdio.h>
4//#include <libconfig/lib/grammar.y>
5
6static floatx om[N];
7static floatx Pt[N];
8static floatx sth[N];
9static floatx cth[N];
10static floatx isdm[N];
11static floatx isqm[N];
12static floatx th[N];
13static floatx w[N];
14static floatx lwi[N];
15
16static floatx qth;
17static floatx qom;
18static floatx r;
19
20
21#define MAXrand 100
22#define MAXrandn 100
23static const floatx RArandu[MAXrand]={0.8147, 0.9058, 0.1270, 0.9134, 0.6324, 0.0975, 0.2785, 0.5469, 0.9575, 0.9649,
24                                      0.1576, 0.9706, 0.9572, 0.4854, 0.8003, 0.1419, 0.4218, 0.9157, 0.7922, 0.9595, 0.6557, 0.0357,
25                                      0.8491, 0.9340, 0.6787, 0.7577, 0.7431, 0.3922, 0.6555, 0.1712, 0.7060, 0.0318, 0.2769, 0.0462,
26                                      0.0971, 0.8235, 0.6948, 0.3171, 0.9502, 0.0344, 0.4387, 0.3816, 0.7655, 0.7952, 0.1869, 0.4898,
27                                      0.4456, 0.6463, 0.7094, 0.7547, 0.2760, 0.6797, 0.6551, 0.1626, 0.1190, 0.4984, 0.9597, 0.3404,
28                                      0.5853, 0.2238, 0.7513, 0.2551, 0.5060, 0.6991, 0.8909, 0.9593, 0.5472, 0.1386, 0.1493, 0.2575,
29                                      0.8407, 0.2543, 0.8143, 0.2435, 0.9293, 0.3500, 0.1966, 0.2511, 0.6160, 0.4733, 0.3517, 0.8308,
30                                      0.5853, 0.5497, 0.9172, 0.2858, 0.7572, 0.7537, 0.3804, 0.5678, 0.0759, 0.0540, 0.5308, 0.7792,
31                                      0.9340, 0.1299, 0.5688, 0.4694, 0.0119, 0.3371
32                                     };
33static const floatx *randu_i=&RArandu[0];
34const static floatx *randu_last=&RArandu[MAXrand-1];
35
36static const floatx RArandn[MAXrandn]={
37        0.6353, -0.6313, -1.0181,  0.4801,  0.4855,  0.9901, -0.5412, -1.0290,  1.0641, -0.0132,
38        -0.6014, -2.3252, -0.1821,  0.6682, -0.0050,  0.2189, -1.3335,  0.2431, -0.2454, -0.5803,
39        0.5512, -1.2316,  1.5210, -0.0783, -0.2762,  0.2617,  1.0727, -1.2566, -1.5175,  2.1363,
40        -1.0998,  1.0556, -0.0384,  0.8892,  1.2765,  1.2134, -0.7121, -0.3472,  0.0097, -0.2576,
41        0.0860, -0.1132,  1.2274,  2.3093,  1.8634, -0.2747, -0.0113, -0.9414,  0.0714, -1.4095,
42        -2.0046,  0.3792, -0.6962,  0.5246, -0.5226, -0.1331, -0.0008, -1.1746,  0.3165,  1.7701,
43        -0.4931,  0.9442,  0.0075, -0.0118,  0.1034, -1.2705, -0.2494, -1.0211,  0.4998,  0.3255,
44        0.4620, -2.1204, -0.7829,  0.9131, -0.8076, -1.6636,  0.3966, -0.4017,  1.2781, -1.1190,
45        -0.3210, -0.6447,  0.5869,  0.0559,  0.6804, -0.7036, -0.2640,  0.1737, -0.5478,  0.6204,
46        1.2366, -0.7043, -0.2512, -1.1071, -2.3646,  0.2809, -1.6640, -0.1161,  0.2608,  1.2698
47};
48static const floatx *randn_i=&RArandn[0];
49const static floatx *randn_last=&RArandn[MAXrandn-1];
50
51floatx qrandu() {
52        if ( randu_i==randu_last )
53                randu_i =&RArandu[0];
54        else
55                randu_i++;
56
57        return *randu_i;
58}
59
60floatx qrandn() {
61        if ( randn_i==randn_last )
62                randn_i =&RArandn[0];
63        else
64                randn_i++;
65
66        return *randn_i;
67}
68
69void resample() {
70        int N_babies[N];
71        floatx cumdist;
72        int i;
73        floatx ui;
74        int i_from=0;
75        int i_to=0;
76
77        floatx u0 = qrandu();
78
79        int j=0;
80        N_babies[0]=0;
81        cumdist = w[0];
82        for ( i = 0; i < N; i++ ) {
83                ui  = ( i + u0 ) / N;
84                while ( ui > cumdist ) {
85                        j++;
86                        N_babies[j]=0;
87                        cumdist+=w[j];
88                }
89                N_babies [j] ++;
90        }
91        while (j++<=(N-1)) { // delete all N_babies after j
92                N_babies[j]=0; 
93        }
94
95        // COPY
96
97        while ( i_from<N ) {
98                while ( N_babies[i_from]>1 ) { // 1 baby stays where it is
99                       
100                        while ( N_babies[i_to]>=1 ) i_to++; // find first empty slot
101
102                        //copy it
103                        Pt[i_to]=Pt[i_from];
104                        om[i_to]=om[i_from];
105                        th[i_to]=th[i_from];
106                        isdm[i_to]=isdm[i_from];
107                        isqm[i_to]=isqm[i_from];
108
109                        N_babies[i_to]++;
110                        N_babies[i_from]--;
111                }
112                i_from++;
113        }
114}
115
116void mpf_bayes ( const floatx isa, const floatx isb , const floatx usa, const floatx usb ) {
117
118        floatx isd;
119        floatx isq;
120        floatx usd;
121        floatx usq;
122
123        floatx Cd;
124        floatx Cq;
125        floatx CC;
126        floatx oCC;
127
128        floatx difid;
129        floatx difiq;
130
131        floatx zeta;
132        floatx Kd;
133        floatx Kq;
134        floatx ro;
135        floatx ypd;
136        floatx ypq;
137        floatx detRy;
138        floatx ydiffd;
139        floatx ydiffq;
140        floatx ydC;
141
142        floatx maxlwi, sumlwi;
143        int i;
144       
145        floatx *th_i, *Pt_i, *om_i, *isdm_i, *isqm_i;
146        floatx *w_i, *lw_i;
147        floatx *sth_i, *cth_i;
148// implementation starts here
149        th_i = &th[0];
150        Pt_i = &Pt[0];
151        om_i = &om[0];
152        isdm_i = &isdm[0];
153        isqm_i = &isqm[0];
154        sth_i = &sth[0];
155        cth_i = &cth[0];
156        for ( i=0; i<N; i++ ) {
157
158                // time update
159                *Pt_i = 1.0*1.0*(*Pt_i)+qom; // Pt is now predictive variance
160                *th_i += *om_i*_dt+qth *  qrandn() ;
161                while ( *th_i>M_PI ) *th_i -=2*M_PI;
162                while ( *th_i<-M_PI ) *th_i+=2*M_PI;
163
164                *sth_i=sin ( *th_i );
165                *cth_i=cos ( *th_i );
166               
167                isd = *cth_i*isa+*sth_i*isb;
168                isq = -*sth_i*isa+*cth_i*isb;
169                // process old voltage
170                usd = (*cth_i)*usa+(*sth_i)*usb;
171                usq = -(*sth_i)*usa+(*cth_i)*usb;
172
173                Cd = isq*_dt;
174                Cq = -_b - isd* _dt;
175
176                difid=isd- _ad  * (*isdm_i) - _cd *(usd);
177                difiq=isq- _aq  * (*isqm_i) - _cq *(usq);
178
179                CC=Cd*Cd+Cq*Cq;
180                zeta = *Pt_i/ ( r+*Pt_i*CC );
181                oCC = ( 1-zeta*CC );
182                ro = oCC/r;
183
184                Kd = *Pt_i*Cd*ro;
185                Kq = *Pt_i*Cq*ro;
186
187                (*Pt_i)=(*Pt_i)*( 1.0- ( Kd*Cd+Kq*Cq ) );
188
189                ypd = Cd**om_i;
190                ypq = Cq**om_i;
191
192                //detRy = ro/r;
193                *om_i +=  Kd* ( difid - ypd ) +Kq* ( difiq-ypq );
194                ydiffd = ( ypd-difid );
195                ydiffq = ( ypq-difiq );
196                ydC = ydiffd*Cd + ydiffq*Cq;
197
198                //lwi[i] = 0.5* ( log ( detRy ) + ( ydC*ydC*zeta -(ydiffd*ydiffd+ydiffq*ydiffq) ) /r ) ;
199                lwi[i] = 0.5*( ydC*ydC*zeta -(ydiffd*ydiffd+ydiffq*ydiffq) ) /r ;
200               
201                *isdm_i=isd;
202                *isqm_i=isq;
203               
204                // shift all counters
205                th_i++;
206                Pt_i++;
207                om_i++;
208                isdm_i++;
209                isqm_i++;
210                sth_i++;
211                cth_i++;
212
213        }
214        maxlwi=-1e10;
215        lw_i = &lwi[0];
216        for ( i=0;i<N;i++,lw_i++ ) {
217                if ( *lw_i>maxlwi ) maxlwi=*lw_i;
218        }
219        lw_i = &lwi[0];
220        w_i = &w[0];
221        for ( i=0;i<N;i++,lw_i++,w_i++ ) {
222                *lw_i-=maxlwi;
223                (*w_i)=exp ( *lw_i ); // always resample -> wmin=1/n;
224        }
225
226        sumlwi=0.0;
227        w_i = &w[0];
228        for ( i=0;i<N;i++,w_i++ ) sumlwi+=*w_i;
229        w_i = &w[0];
230        for ( i=0;i<N;i++,w_i++ ) *w_i/=sumlwi;
231       
232        // NEFF
233        floatx sw2=0.0;
234        for ( i=0;i<N;i++ ) sw2+=w[i]*w[i];
235        printf("%f\n",1./sw2*N);
236       
237        resample();
238}
239
240
241void mpf_init(floatx qom0, floatx qth0, floatx r0) {
242        int i;
243        r=r0;
244        qth=qth0;
245        qom=qom0;
246
247        for ( i=0; i<N; i++ ) {
248                th[i]=qrandu() *2*M_PI - M_PI;
249                sth[i]=sin ( th[i] );
250                cth[i]=cos ( th[i] );
251               
252                om[i]=0;
253                Pt[i]=10.;
254                isdm[i]=0;
255                isqm[i]=0;
256                w[i]=1.0/N;
257        }
258
259}
260void mpf_mean ( floatx *Ecosth, floatx *Esinth, floatx *Eome ) {
261        int i;
262        *Ecosth=0.0;
263        *Esinth=0.0;
264        *Eome=0.0;
265        for ( i=0;i<N;i++ ) {
266                *Ecosth+=(w[i]*(cth[i]));
267                *Esinth+=(w[i]*(sth[i]));
268                *Eome+=(w[i]*(om[i]));
269        }
270
271}
272void mpf_th(floatx th1[N]){
273        int i;
274        for (i=0;i<N;i++) th1[i]=th[i];
275}
276
277floatx kalman_om( const floatx isa, const floatx isb , const floatx usa, const floatx usb, const floatx th ) {
278
279        floatx isd;
280        floatx isq;
281        floatx usd;
282        floatx usq;
283
284        floatx Cd;
285        floatx Cq;
286        floatx CC;
287        floatx oCC;
288
289        floatx difid;
290        floatx difiq;
291
292        floatx zeta;
293        floatx Kd;
294        floatx Kq;
295        floatx ro;
296        floatx ypd;
297        floatx ypq;
298        floatx detRy;
299        floatx ydiffd;
300        floatx ydiffq;
301        floatx ydC;
302
303        static floatx Pt;
304        static floatx om;
305        floatx cth, sth;
306        static floatx isdm, isqm;
307
308       
309                Pt = 1.0*1.0*Pt+qom; // Pt is now predictive variance
310                //while ( th>M_PI ) th=th-2*M_PI;
311                //while ( th<-M_PI ) th=th+2*M_PI;
312
313                sth=sin ( th );
314                cth=cos ( th );
315
316                isd = cth*isa+sth*isb;
317                isq = -sth*isa+cth*isb;
318                usd = cth*usa+sth*usb;
319                usq = -sth*usa+cth*usb;
320
321                Cd = isq*_dt;
322                Cq = -_b - isd* _dt;
323
324                difid=isd- _ad  *isdm - _cd *usd;
325                difiq=isq- _aq  *isqm - _cq *usq;
326
327                CC=Cd*Cd+Cq*Cq;
328                zeta = Pt/ ( r+Pt*CC );
329                oCC = ( 1-zeta*CC );
330                ro = oCC/r;
331
332                Kd = Pt*Cd*ro;
333                Kq = Pt*Cq*ro;
334
335                Pt=Pt* ( 1- ( Kd*Cd+Kq*Cq ) );
336
337                ypd = Cd*om;
338                ypq = Cq*om;
339
340                detRy = ro/r;
341                om = om + Kd* ( difid - ypd ) +Kq* ( difiq-ypq );
342                ydiffd = ( ypd-difid );
343                ydiffq = ( ypq-difiq );
344                ydC = ydiffd*Cd + ydiffq*Cq;
345
346                isdm=isd;
347                isqm=isq;
348               
349                return om;
350}
Note: See TracBrowser for help on using the browser.