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