Changeset 1469 for applications

Show
Ignore:
Timestamp:
12/02/13 13:54:59 (10 years ago)
Author:
smidl
Message:

upravy rand

Location:
applications/pmsm/simulator_zdenek/ekf_example
Files:
4 modified

Legend:

Unmodified
Added
Removed

  • applications/pmsm/simulator_zdenek/ekf_example/CMakeLists.txt

    r1468 r1469  
    88 
    99include_directories(../../bdm) 
    10 add_library (ekf_obj ekf_obj.cpp fixed.cpp matrix.cpp matrix_vs.cpp mpf_double.cpp ekf_mm.cpp qmath.cpp) 
     10add_library (ekf_obj ekf_obj.cpp fixed.cpp matrix.cpp matrix_vs.cpp mpf_double.cpp ekf_mm.cpp qmath.cpp RArandn.cpp) 

  • applications/pmsm/simulator_zdenek/ekf_example/RArandn.h

    r1468 r1469  
    1 #define MAXrandn 997 
    2 static const floatx RArandn[MAXrandn]={ 
    3 -4.3256481e-01, -1.6655844e+00, 1.2533231e-01, 2.8767642e-01, -1.1464714e+00, 1.1909155e+00, 1.1891642e+00, -3.7633277e-02, 3.2729236e-01, 1.7463914e-01, -1.8670858e-01, 7.2579055e-01, -5.8831654e-01, 2.1831858e+00, -1.3639588e-01, 1.1393131e-01, 1.0667682e+00, 5.9281461e-02, -9.5648405e-02, -8.3234946e-01, 2.9441082e-01, -1.3361819e+00, 7.1432455e-01, 1.6235621e+00, -6.9177570e-01, 8.5799667e-01, 1.2540014e+00, -1.5937296e+00, -1.4409644e+00, 5.7114762e-01, -3.9988558e-01, 6.8999738e-01, 8.1562229e-01, 7.1190832e-01, 1.2902498e+00, 6.6860051e-01, 1.1908381e+00, -1.2024571e+00, -1.9789558e-02, -1.5671730e-01, -1.6040856e+00, 2.5730423e-01, -1.0564729e+00, 1.4151415e+00, -8.0509040e-01, 5.2874301e-01, 2.1932067e-01, -9.2190162e-01, -2.1706745e+00, -5.9187825e-02, -1.0106337e+00, 6.1446305e-01, 5.0774079e-01, 1.6924299e+00, 5.9128259e-01, -6.4359520e-01, 3.8033725e-01, -1.0091155e+00, -1.9510670e-02, -4.8220789e-02, 4.3191842e-05, -3.1785945e-01, 1.0950037e+00, -1.8739903e+00, 4.2818327e-01, 8.9563847e-01, 7.3095734e-01, 5.7785735e-01, 4.0314032e-02, 6.7708919e-01, 5.6890021e-01, -2.5564542e-01, -3.7746896e-01, -2.9588711e-01, -1.4751345e+00, -2.3400405e-01, 1.1844484e-01, 3.1480904e-01, 1.4435082e+00, -3.5097474e-01, 6.2323385e-01, 7.9904862e-01, 9.4088994e-01, -9.9209174e-01, 2.1203515e-01, 2.3788207e-01, -1.0077634e+00, -7.4204475e-01, 1.0822950e+00, -1.3149970e-01, 3.8988049e-01, 8.7987107e-02, -6.3546523e-01, -5.5957330e-01, 4.4365349e-01, -9.4990380e-01, 7.8118162e-01, 5.6896065e-01, -8.2171429e-01, -2.6560685e-01, -1.1877770e+00, -2.2023207e+00, 9.8633739e-01, -5.1863507e-01, 3.2736756e-01, 2.3405701e-01, 2.1466139e-02, -1.0039445e+00, -9.4714606e-01, -3.7442920e-01, -1.1858862e+00, -1.0559029e+00, 1.4724799e+00, 5.5743832e-02, -1.2173175e+00, -4.1227134e-02, -1.1283439e+00, -1.3492775e+00, -2.6110162e-01, 9.5346545e-01, 1.2864443e-01, 6.5646751e-01, -1.1678194e+00, -4.6060518e-01, -2.6243995e-01, -1.2131521e+00, -1.3194370e+00, 9.3121751e-01, 1.1244896e-02, -6.4514582e-01, 8.0572879e-01, 2.3162601e-01, -9.8975967e-01, 1.3395857e+00, 2.8950203e-01, 1.4789171e+00, 1.1380280e+00, -6.8413859e-01, -1.2919360e+00, -7.2926276e-02, -3.3059888e-01, -8.4362764e-01, 4.9776966e-01, 1.4884905e+00, -5.4647589e-01, -8.4675816e-01, -2.4633653e-01, 6.6302415e-01, -8.5419737e-01, -1.2013148e+00, -1.1986943e-01, -6.5294015e-02, 4.8529556e-01, -5.9549090e-01, -1.4966774e-01, -4.3475193e-01, -7.9330223e-02, 1.5351523e+00, -6.0648286e-01, -1.3473627e+00, 4.6938312e-01, -9.0356694e-01, 3.5879639e-02, -6.2753122e-01, 5.3539795e-01, 5.5288352e-01, -2.0369048e-01, -2.0543247e+00, 1.3256073e-01, 1.5929407e+00, 1.0184118e+00, -1.5804025e+00, -7.8661919e-02, -6.8165686e-01, -1.0245531e+00, -1.2343535e+00, 2.8880702e-01, -4.2930300e-01, 5.5801190e-02, -3.6787357e-01, -4.6497337e-01, 3.7096058e-01, 7.2828293e-01, 2.1121602e+00, -1.3572977e+00, -1.0226101e+00, 1.0378342e+00, -3.8979955e-01, -1.3812656e+00, 3.1554263e-01, 1.5532426e+00, 7.0789388e-01, 1.9573848e+00, 5.0454235e-01, 1.8645290e+00, -3.3981178e-01, -1.1397794e+00, -2.1112348e-01, 1.1902449e+00, -1.1162088e+00, 6.3527413e-01, -6.0141213e-01, 5.5118471e-01, -1.0998405e+00, 8.5990593e-02, -2.0045633e+00, -4.9308792e-01, 4.6204801e-01, -3.2100469e-01, 1.2365557e+00, -6.3127966e-01, -2.3252111e+00, -1.2316365e+00, 1.0556484e+00, -1.1322399e-01, 3.7922362e-01, 9.4419973e-01, -2.1204267e+00, -6.4467892e-01, -7.0430173e-01, -1.0181372e+00, -1.8208187e-01, 1.5210132e+00, -3.8438764e-02, 1.2274480e+00, -6.9620480e-01, 7.5244865e-03, -7.8289304e-01, 5.8693856e-01, -2.5120737e-01, 4.8013582e-01, 6.6815503e-01, -7.8321196e-02, 8.8917262e-01, 2.3092875e+00, 5.2463868e-01, -1.1787324e-02, 9.1314082e-01, 5.5940679e-02, -1.1070699e+00, 4.8549771e-01, -5.0050738e-03, -2.7621786e-01, 1.2764525e+00, 1.8634006e+00, -5.2255930e-01, 1.0342445e-01, -8.0764913e-01, 6.8043858e-01, -2.3645898e+00, 9.9011487e-01, 2.1889912e-01, 2.6166246e-01, 1.2134445e+00, -2.7466699e-01, -1.3313445e-01, -1.2705002e+00, -1.6636065e+00, -7.0355426e-01, 2.8088049e-01, -5.4120933e-01, -1.3335307e+00, 1.0726863e+00, -7.1208545e-01, -1.1285561e-02, -8.1702920e-04, -2.4943628e-01, 3.9657532e-01, -2.6401335e-01, -1.6640109e+00, -1.0289751e+00, 2.4309470e-01, -1.2565901e+00, -3.4718319e-01, -9.4137219e-01, -1.1745603e+00, -1.0211417e+00, -4.0166673e-01, 1.7366567e-01, -1.1611849e-01, 1.0641191e+00, -2.4538630e-01, -1.5175391e+00, 9.7341591e-03, 7.1372865e-02, 3.1653581e-01, 4.9982567e-01, 1.2780841e+00, -5.4781615e-01, 2.6080840e-01, -1.3176672e-02, -5.8026400e-01, 2.1363084e+00, -2.5761712e-01, -1.4095285e+00, 1.7701009e+00, 3.2554598e-01, -1.1190396e+00, 6.2035014e-01, 1.2697818e+00, -8.9604251e-01, 1.3517544e-01, -1.3904001e-01, -1.1633953e+00, 1.1837195e+00, -1.5429662e-02, 5.3621869e-01, -7.1642862e-01, -6.5555939e-01, 3.1436276e-01, 1.0681408e-01, 1.8482162e+00, -2.7510568e-01, 2.2125541e+00, 1.5085258e+00, -1.9450786e+00, -1.6805428e+00, -5.7353413e-01, -1.8581653e-01, 8.9341157e-03, 8.3694989e-01, -7.2227067e-01, -7.2149048e-01, -2.0118100e-01, -2.0464161e-02, 2.7889000e-01, 1.0582948e+00, 6.2167328e-01, -1.7506153e+00, 6.9734755e-01, 8.1148586e-01, 6.3634495e-01, 1.3100803e+00, 3.2709752e-01, -6.7299316e-01, -1.4932750e-01, -2.4490178e+00, 4.7328561e-01, 1.1694566e-01, -5.9110384e-01, -6.5470768e-01, -1.0806619e+00, -4.7730865e-02, 3.7934454e-01, -3.3036105e-01, -4.9989825e-01, -3.5978608e-02, -1.7476033e-01, -9.5726508e-01, 1.2925479e+00, 4.4090964e-01, 1.2809409e+00, -4.9772981e-01, -1.1187166e+00, 8.0764962e-01, 4.1199579e-02, -7.5620861e-01, -8.9129148e-02, -2.0088503e+00, 1.0839180e+00, -9.8119056e-01, -6.8848864e-01, 1.3394795e+00, -9.0924316e-01, -4.1285773e-01, -5.0616319e-01, 1.6197478e+00, 8.0900711e-02, -1.0810565e+00, -1.1245178e+00, 1.7356763e+00, 1.9374586e+00, 1.6350682e+00, -1.2559402e+00, -2.1353751e-01, -1.9893205e-01, 3.0749918e-01, -5.7232546e-01, -9.7764837e-01, -4.4680941e-01, 1.0820919e+00, 2.3726479e+00, 2.2928834e-01, -2.6662314e-01, 7.0167218e-01, -4.8759049e-01, 1.8624797e+00, 1.1068511e+00, -1.2275657e+00, -6.6988511e-01, 1.3409295e+00, 3.8808332e-01, 3.9305893e-01, -1.7073336e+00, 2.2785864e-01, 6.8563286e-01, -6.3679011e-01, -1.0026056e+00, -1.8562067e-01, -1.0540327e+00, -7.1539489e-02, 2.7919842e-01, 1.3732753e+00, 1.7984103e-01, -5.4201656e-01, 1.6341906e+00, 8.2521515e-01, 2.3076114e-01, 6.7163395e-01, -5.0807788e-01, 8.5635160e-01, 2.6850345e-01, 6.2497518e-01, -1.0473379e+00, 1.5356704e+00, 4.3442574e-01, -1.9171359e+00, 4.6993952e-01, 1.2743511e+00, 6.3854245e-01, 1.3807825e+00, 1.3198429e+00, -9.0942924e-01, -2.3056051e+00, 1.7887302e+00, 3.9079781e-01, 2.0323676e-02, -4.0597700e-01, -1.5348945e+00, 2.2137310e-01, -1.3744787e+00, -8.3928553e-01, -2.0864256e-01, 7.5591300e-01, 3.7573445e-01, -1.3454130e+00, 1.4818762e+00, 3.2736138e-02, 1.8704528e+00, -1.2089905e+00, -7.8263187e-01, -7.6729852e-01, -1.0720010e-01, -9.7705667e-01, -9.6398812e-01, -2.3791724e+00, -8.3818791e-01, 2.5734563e-01, -1.8383400e-01, -1.6761530e-01, -1.1698891e-01, 1.6848817e-01, -5.0120630e-01, -7.0507642e-01, 5.0816468e-01, -4.2092154e-01, 2.2913337e-01, -9.5949703e-01, -1.4604259e-01, 7.4453803e-01, -8.9049603e-01, 1.3906186e-01, -2.3614430e-01, -7.5459129e-02, -3.5857191e-01, -2.0776349e+00, -1.4354571e-01, 1.3933415e+00, 6.5180409e-01, -3.7713356e-01, -6.6144306e-01, 2.4895798e-01, -3.8351616e-01, -5.2847980e-01, 5.5388364e-02, 1.2537686e+00, -2.5200036e+00, 5.8485612e-01, -1.0080644e+00, 9.4428482e-01, -2.4239571e+00, -2.2383143e-01, 5.8069883e-02, -4.2461402e-01, -2.0291795e-01, -1.5130770e+00, -1.1263519e+00, -8.1500216e-01, 3.6661427e-01, -5.8610676e-01, 1.5374090e+00, 1.4007153e-01, -1.8627667e+00, -4.5419310e-01, -6.5207411e-01, 1.0331788e-01, -2.2063162e-01, -2.7904337e-01, -7.3366201e-01, -6.4533580e-02, -1.4440041e+00, 6.1234029e-01, -1.3235030e+00, -6.6157668e-01, -1.4611451e-01, 2.4808496e-01, -7.6632557e-02, 1.7381700e+00, 1.6219719e+00, 6.2643590e-01, 9.1813590e-02, -8.0760705e-01, -4.6133709e-01, -1.4059692e+00, -3.7452963e-01, -4.7091124e-01, 1.7512956e+00, 7.5322462e-01, 6.4989341e-02, -2.9276426e-01, 8.2822847e-02, 7.6619148e-01, 2.2368499e+00, 3.2688673e-01, 8.6330394e-01, 6.7938660e-01, 5.5475763e-01, 1.0016304e+00, 1.2593651e+00, 4.4151065e-02, -3.1413767e-01, 2.2670764e-01, 9.9669193e-01, 1.2159117e+00, -5.4270242e-01, 9.1222834e-01, -1.7214113e-01, -3.3595464e-01, 5.4148678e-01, 9.3211121e-01, -5.7025265e-01, -1.4986054e+00, -5.0345644e-02, 5.5302477e-01, 8.3497887e-02, 1.5775235e+00, -3.3077420e-01, 7.9515483e-01, -7.8480015e-01, -1.2631215e+00, 6.6665499e-01, -1.3926322e+00, -1.3005625e+00, -6.0502216e-01, -1.4885645e+00, 5.5854296e-01, -2.7735429e-01, -1.2936848e+00, -8.8843518e-01, -9.8652008e-01, -7.1617644e-02, -2.4145909e+00, -6.9434901e-01, -1.3913890e+00, 3.2964780e-01, 5.9854448e-01, 1.4717540e-01, -1.0143899e-01, -2.6349808e+00, 2.8053414e-02, -8.7631013e-01, -2.6547745e-01, -3.2757791e-01, -1.1582475e+00, 5.8005321e-01, 2.3975573e-01, -3.5088492e-01, 8.9209848e-01, 1.5782988e+00, -1.1081739e+00, -2.5931047e-02, -1.1106277e+00, 7.5083418e-01, 5.0016703e-01, -5.1726088e-01, -5.5920949e-01, -7.5337062e-01, 9.2581269e-01, -2.4852032e-01, -1.4983452e-01, -1.2584154e+00, 3.1261988e-01, 2.6902769e+00, 2.8969629e-01, -1.4228030e+00, 2.4678572e-01, -1.4357729e+00, 1.4857313e-01, -1.6930726e+00, 7.1918848e-01, 1.1417728e+00, 1.5519363e+00, 1.3836301e+00, -7.5809225e-01, 4.4266274e-01, 9.1109790e-01, -1.0740856e+00, 2.0176191e-01, 7.6286322e-01, -1.2881872e+00, -9.5296182e-01, 7.7817450e-01, -6.3310696e-03, 5.2448681e-01, 1.3642718e+00, 4.8203933e-01, -7.8706585e-01, 7.5199927e-01, -1.6688791e-01, -8.1622811e-01, 2.0940652e+00, 8.0152532e-02, -9.3729507e-01, 6.3573878e-01, 1.6820280e+00, 5.9363417e-01, 7.9015288e-01, 1.0525385e-01, -1.5857856e-01, 8.7090745e-01, -1.9475893e-01, 7.5474485e-02, -5.2663484e-01, -6.8548438e-01, -2.6838765e-01, -1.1883460e+00, 2.4857898e-01, 1.0245162e-01, -4.1006877e-02, -2.2475823e+00, -5.1077648e-01, 2.4924264e-01, 3.6919671e-01, 1.7919662e-01, -3.7283412e-02, -1.6033099e+00, 3.3937204e-01, -1.3113481e-01, 4.8518979e-01, 5.9875070e-01, -8.6030887e-02, 3.2529205e-01, -3.3514323e-01, -3.2244920e-01, -3.8237413e-01, -9.5337093e-01, 2.3357560e-01, 1.2352447e+00, -5.7853160e-01, -5.0153737e-01, 7.2286352e-01, 3.9498485e-02, 1.5412788e+00, -1.7010528e+00, -1.0337413e+00, -7.6370764e-01, 2.1764260e+00, 4.3161210e-01, -4.4376518e-01, 2.9995868e-02, -3.1567087e-01, 9.7784570e-01, 1.8294874e-02, 8.1796283e-01, 7.0234110e-01, -2.3127056e-01, -1.1368958e-01, 1.2794052e-01, -7.9940970e-01, -2.3861196e-01, -8.9463238e-02, -1.0232642e+00, 9.3753792e-01, -1.1317193e+00, -7.1070209e-01, -1.1695009e+00, 1.0654371e+00, -6.8039374e-01, -1.7257727e+00, 8.1319960e-01, 1.4418666e+00, 6.7227220e-01, 1.3866496e-01, -8.5953393e-01, -7.5225056e-01, 1.2296151e+00, 1.1507544e+00, -6.0802501e-01, 8.0615792e-01, 2.1713285e-01, -3.7346107e-01, -8.3203043e-01, 2.8686630e-01, -1.8188916e+00, -1.5730514e+00, 2.0156657e+00, -7.1982024e-02, 2.6289094e+00, -2.4331695e-01, 1.7327649e-01, 9.2320708e-01, -1.7855300e-01, -5.2170491e-01, 1.4319620e+00, -8.7011745e-01, 8.0754179e-01, -5.1063464e-01, 7.4351413e-01, 8.4789830e-01, -8.2990072e-01, 5.3299443e-01, 1.0328484e+00, -1.0520240e+00, 3.6211374e-01, -3.6786617e-02, -1.2276356e+00, -2.7509887e-01, -1.6043524e-01, -1.0835748e+00, -1.9542127e+00, -9.0948726e-01, -5.5787277e-03, -1.7234899e+00, 1.2630773e+00, -6.0043337e-01, -2.0639248e+00, 1.1091094e-01, 1.4876143e+00, 5.3002142e-02, 1.6198074e-01, -2.6877919e-02, 1.7357619e-01, 8.8216775e-01, 1.8229442e-01, 7.5529542e-01, 5.0803455e-01, 1.3188014e-01, 2.8010411e-01, -9.8284756e-01, -9.4408743e-01, -1.3057516e-02, 3.5434528e-01, -8.9470883e-01, 8.1211102e-01, 1.0953735e-01, 2.7316440e+00, 4.1107915e-01, -1.3068620e+00, 3.8380649e-01, 4.9950368e-01, -5.1078559e-01, 2.3492205e-01, -5.9782511e-01, 2.0770819e-02, 4.1944276e-01, 1.1911037e+00, 7.7121425e-01, -2.6442219e+00, 2.8542990e-01, 8.2609262e-01, -8.1224925e-03, 8.5843816e-01, 7.7478824e-01, 1.3059453e+00, 1.2315025e+00, 9.5856367e-01, -1.6545477e+00, -9.9039601e-01, 6.8523602e-01, -9.7487001e-01, -6.0672564e-01, 6.8679374e-01, 2.0048856e-02, 1.0638006e+00, -1.3410499e+00, 4.7951044e-01, -1.6339740e+00, -1.4426650e+00, 2.9378142e-01, -1.4036408e-01, -1.1303407e+00, -2.9253805e-01, -5.8253587e-01, -8.9634828e-01, 2.4860108e-01, -1.4896634e+00, 3.1350866e-01, -2.0250844e+00, 5.2898970e-01, 3.4347090e-01, 7.5819261e-01, -6.9194018e-01, 6.8017855e-01, -1.0725410e+00, 8.9977209e-01, -2.1230924e+00, 2.8471227e-01, -7.3332293e-01, -7.7337563e-01, 1.5184172e-01, -3.3684313e-01, 9.7076072e-01, -1.0723636e-01, 1.0134919e+00, -4.7534676e-01, 6.8948145e-02, 3.9859207e-01, 1.1163262e+00, 6.2045113e-01, -2.8767448e-01, -1.3717733e+00, -6.8586843e-01, 3.3168474e-01, -9.9772182e-01, 2.9141810e-01, 1.1070784e+00, 2.4495861e-01, 1.6497578e-01, 4.0623134e-01, 1.2159812e+00, 1.4484243e+00, -1.0251375e+00, 2.0541801e-01, 5.8888219e-01, -2.6402362e-01, 2.4953178e+00, 8.5594767e-01, -8.5095363e-01, 8.1187884e-01, 7.0024172e-01, 7.5993833e-01, -1.7129092e+00, 1.5370212e+00, -1.6098468e+00, 1.1095260e+00, -1.1097040e+00, 3.8546939e-01, 9.6523079e-01, 8.1829723e-01, 3.7048779e-02, -9.2601240e-01, -1.1191866e-01, -8.0302969e-01, -1.6650059e+00, -9.0140060e-01, 5.8834967e-01, 5.5415928e-01, -4.1517261e-01, 6.1794859e-02, 4.5743157e-01, 1.9901391e-01, 2.5755782e-01, 2.0807298e+00, -2.2772365e+00, 3.3902243e-01, 2.8989389e-01, 6.6226085e-01, -5.8086023e-01, 8.8775191e-01, 1.7187104e-01, 8.4882144e-01, 9.6376903e-01, 1.3219180e+00, -6.4344903e-02, 1.3170531e+00, 2.2801733e-01, -1.4296372e+00, -1.4970100e-01, -5.0496756e-01, -1.7291415e+00, -4.1747222e-01, -6.1496852e-01, 7.2077668e-01, 3.3936379e-01, 8.8284508e-01, 2.8424521e-01, -1.4554135e-01, -8.9645900e-02, 2.8916087e-01, 1.1648308e+00, 8.0572935e-01, -1.3556435e+00, 1.2089290e-01, -2.2217791e-01, 5.7173234e-01, -3.0014043e-01, 1.1342768e+00, -1.7935641e-01, -1.4670667e+00, 1.3953457e+00, 4.4083567e-01, 5.6538405e-01, -6.9362259e-01, 8.3386925e-01, -2.2373784e+00, 1.0976441e+00, -1.6169159e-03, -1.6145733e+00, -1.2287268e+00, 2.0740458e-01, 2.2094200e-01, -1.0060731e+00, -4.5306709e-01, 1.3994533e+00, -4.6196398e-01, 3.2715905e-02, 7.9878320e-01, 8.9681579e-01, 1.3789173e-01, -1.6191464e+00, -1.6466057e+00, 4.2870688e-01, -7.3723084e-01, 5.6492614e-01, -1.3841667e+00, 4.6026799e-01, 6.2938365e-01, 3.7984731e-01, -1.0133299e+00, -3.4724278e-01, 4.4191230e-01, -1.5902403e+00, -7.0141671e-01, -1.0776006e+00, 1.0022197e+00, 1.7294805e+00, 7.0903221e-01, -7.4789686e-01, 2.2886239e-01, -2.2349661e-01, -8.5327526e-01, 3.4562680e-01, 1.0976448e-01, -1.1330390e+00, -6.8312354e-01, -2.7785623e-01, 6.5478985e-01, -1.2483940e+00, -5.9753903e-01, -4.8181255e-01, 9.8337217e-01, 1.7621207e+00, 1.4274017e+00, 9.1176303e-01, 3.2682265e-01, 6.9618540e-02, -1.4997627e+00, -4.1822319e-01, -2.1036798e-02, 2.2842453e-01, -1.0081958e+00, -6.6462217e-01, 5.5817658e-01, -1.1885421e+00, -7.7548150e-01, 2.7104207e-01, 1.5349757e+00, -1.0522827e+00, 6.2555857e-01, -7.9762607e-01, -3.1352199e-01, -6.0220975e-01, 1.2590603e+00, 8.5848425e-01, -2.1052919e+00, -3.6093718e-01, 5.5355672e-01, -1.5563838e+00, -2.0666572e-01, -4.2556755e-01, 4.9377825e-01, -8.7090761e-01, 7.9827658e-02, -5.2161914e-01, -1.4138609e+00, -3.8429318e-01, -4.5792222e-01, -2.9147069e-01, -3.0122401e-01, -1.5885938e+00, 1.0942867e+00, 1.3241669e+00, -1.2647963e-01, -7.3716382e-01, 2.1371884e-01, -4.0052913e-01, 6.4937747e-02};//, -1.7579956e+00, 1.6867480e+00, 3.2740021e-01}; 
    4 static const floatx *randn_i=&RArandn[0]; 
    5 const static floatx *randn_last=&RArandn[MAXrandn-1]; 
     1#define MAXrandn 997    //1000 
     2static const float RArandn[MAXrandn]={-0.2895823932,-1.6226019832,0.1683147268,0.3306588368,-1.1034889832,1.2338979168,1.2321466168,0.0053491398,0.3702747768,0.2176215568,-0.1437261632,0.7687729668,-0.5453341232,2.2261682168,-0.0934134632,0.1569137268,1.1097506168,0.1022638778,-0.0526659882,-0.7893670432,0.3373932368,-1.2931994832,0.7573069668,1.6665445168,-0.6487932832,0.9009790868,1.2969838168,-1.5507471832,-1.3979819832,0.6141300368,-0.3569031632,0.7329797968,0.8586047068,0.7548907368,1.3332322168,0.7115829268,1.2338205168,-1.1594746832,0.0231928588,-0.1137348832,-1.5611031832,0.3002866468,-1.0134904832,1.4581239168,-0.7621079832,0.5717254268,0.2623030868,-0.8789192032,-2.1276920832,-0.0162054082,-0.9676512832,0.6574454668,0.5507232068,1.7354123168,0.6342650068,-0.6006127832,0.4233196668,-0.9661330832,0.0234717468,-0.0052383722,0.0430256086,-0.2748770332,1.1379861168,-1.8310078832,0.4711656868,0.9386208868,0.7739397568,0.6208397668,0.0832964488,0.7200716068,0.6118826268,-0.2126630032,-0.3344865432,-0.2529046932,-1.4321520832,-0.1910216332,0.1614272568,0.3577914568,1.4864906168,-0.3079923232,0.6662162668,0.8420310368,0.9838723568,-0.9491093232,0.2550175668,0.2808644868,-0.9647809832,-0.6990623332,1.1252774168,-0.0885172832,0.4328629068,0.1309695238,-0.5924828132,-0.5165908832,0.4866359068,-0.9069213832,0.8241640368,0.6119430668,-0.7787318732,-0.2226244332,-1.1447945832,-2.1593382832,1.0293198068,-0.4756526532,0.3703499768,0.2770394268,0.0644485558,-0.9609620832,-0.9041636432,-0.3314467832,-1.1429037832,-1.0129204832,1.5154623168,0.0987262488,-1.1743350832,0.0017552828,-1.0853614832,-1.3062950832,-0.2181192032,0.9964478668,0.1716268468,0.6994499268,-1.1248369832,-0.4176227632,-0.2194575332,-1.1701696832,-1.2764545832,0.9741999268,0.0542273128,-0.6021634032,0.8487112068,0.2746084268,-0.9467772532,1.3825681168,0.3324844468,1.5218995168,1.1810104168,-0.6411561732,-1.2489535832,-0.0299438592,-0.2876164632,-0.8006452232,0.5407520768,1.5314729168,-0.5034934732,-0.8037757432,-0.2033541132,0.7060065668,-0.8112149532,-1.1583323832,-0.0768870132,-0.0223115982,0.5282779768,-0.5525084832,-0.1066853232,-0.3917695132,-0.0363478062,1.5781347168,-0.5635004432,-1.3043802832,0.5123655368,-0.8605845232,0.0788620558,-0.5845488032,0.5783803668,0.5958659368,-0.1607080632,-2.0113422832,0.1755431468,1.6359231168,1.0613942168,-1.5374200832,-0.0356795022,-0.6386744432,-0.9815706832,-1.1913710832,0.3317894368,-0.3863205832,0.0987836068,-0.3248911532,-0.4219909532,0.4139429968,0.7712653468,2.1551426168,-1.3143152832,-0.9796276832,1.0808166168,-0.3468171332,-1.3382831832,0.3585250468,1.5962250168,0.7508762968,2.0003672168,0.5475247668,1.9075114168,-0.2968293632,-1.0967969832,-0.1681410632,1.2332273168,-1.0732263832,0.6782565468,-0.5584297132,0.5941671268,-1.0568580832,0.1289730098,-1.9615808832,-0.4501055032,0.5050304268,-0.2780222732,1.2795381168,-0.5882972432,-2.2822286832,-1.1886540832,1.0986308168,-0.0702415732,0.4222060368,0.9871821468,-2.0774442832,-0.6016965032,-0.6613193132,-0.9751547832,-0.1390994532,1.5639956168,0.0045436528,1.2704304168,-0.6532223832,0.0505069033,-0.7399106232,0.6299209768,-0.2082249532,0.5231182368,0.7111374468,-0.0353387792,0.9321550368,2.3522699168,0.5676210968,0.0311950928,0.9561232368,0.0989230958,-1.0640874832,0.5284801268,0.0379773430,-0.2332354432,1.3194349168,1.9063830168,-0.4795768832,0.1464068668,-0.7646667132,0.7234209968,-2.3216073832,1.0330972868,0.2618815368,0.3046448768,1.2564269168,-0.2316845732,-0.0901520332,-1.2275177832,-1.6206240832,-0.6605718432,0.3238629068,-0.4982269132,-1.2905482832,1.1156687168,-0.6691030332,0.0316968558,0.0421653876,-0.2064538632,0.4395577368,-0.2210309332,-1.6210284832,-0.9859926832,0.2860771168,-1.2136076832,-0.3042007732,-0.8983897732,-1.1315778832,-0.9781592832,-0.3586843132,0.2166480868,-0.0731360732,1.1071015168,-0.2024038832,-1.4745566832,0.0527165759,0.1143552818,0.3595182268,0.5428080868,1.3210665168,-0.5048337332,0.3037908168,0.0298057448,-0.5372815832,2.1792908168,-0.2146347032,-1.3665460832,1.8130833168,0.3685283968,-1.0760571832,0.6633325568,1.3127642168,-0.8530600932,0.1781578568,-0.0960575932,-1.1204128832,1.2267019168,0.0275527548,0.5792011068,-0.6734462032,-0.6125769732,0.3573451768,0.1497964968,1.8911986168,-0.2321232632,2.2555365168,1.5515082168,-1.9020961832,-1.6375603832,-0.5305517132,-0.1428341132,0.0519165325,0.8799323068,-0.6792882532,-0.6785080632,-0.1581985832,0.0225182558,0.3218724168,1.1012772168,0.6646556968,-1.7076328832,0.7403299668,0.8544682768,0.6793273668,1.3530627168,0.3700799368,-0.6300107432,-0.1063450832,-2.4060353832,0.5162680268,0.1599280768,-0.5481214232,-0.6117252632,-1.0376794832,-0.0047484482,0.4223269568,-0.2873786332,-0.4569158332,0.0070038088,-0.1317779132,-0.9142826632,1.3355303168,0.4838920568,1.3239233168,-0.4547473932,-1.0757341832,0.8506320368,0.0841819958,-0.7132261932,-0.0461467312,-1.9658678832,1.1269004168,-0.9382081432,-0.6455062232,1.3824619168,-0.8662607432,-0.3698753132,-0.4631807732,1.6627302168,0.1238831278,-1.0380740832,-1.0815353832,1.7786587168,1.9804410168,1.6780506168,-1.2129577832,-0.1705550932,-0.1559496332,0.3504815968,-0.5293430432,-0.9346659532,-0.4038269932,1.1250743168,2.4156303168,0.2722707568,-0.2236407232,0.7446545968,-0.4446080732,1.9054621168,1.1498335168,-1.1845832832,-0.6269026932,1.3839119168,0.4310657368,0.4360413468,-1.6643511832,0.2708410568,0.7286152768,-0.5938076932,-0.9596231832,-0.1426382532,-1.0110502832,-0.0285570722,0.3221808368,1.4162577168,0.2228234468,-0.4990341432,1.6771730168,0.8681975668,0.2737435568,0.7146163668,-0.4650954632,0.8993340168,0.3114858668,0.6679575968,-1.0043554832,1.5786528168,0.4774081568,-1.8741534832,0.5129219368,1.3173335168,0.6815248668,1.4237649168,1.3628253168,-0.8664468232,-2.2626226832,1.8317126168,0.4337802268,0.0633060928,-0.3629945832,-1.4919120832,0.2643555168,-1.3314962832,-0.7963031132,-0.1656601432,0.7988954168,0.4187168668,-1.3024305832,1.5248586168,0.0757185548,1.9134352168,-1.1660080832,-0.7396494532,-0.7243161032,-0.0642176832,-0.9340742532,-0.9210057032,-2.3361899832,-0.7952054932,0.3003280468,-0.1408515832,-0.1246328832,-0.0740064932,0.2114705868,-0.4582238832,-0.6620940032,0.5511470968,-0.3779391232,0.2721157868,-0.9165146132,-0.1030601732,0.7875204468,-0.8475136132,0.1820442768,-0.1931618832,-0.0324767122,-0.3155894932,-2.0346524832,-0.1005632932,1.4363239168,0.6947865068,-0.3341511432,-0.6184606432,0.2919403968,-0.3405337432,-0.4854973832,0.0983707808,1.2967510168,-2.4770211832,0.6278385368,-0.9650819832,0.9872672368,-2.3809746832,-0.1808490132,0.1010522998,-0.3816316032,-0.1599355332,-1.4700945832,-1.0833694832,-0.7720197432,0.4095966868,-0.5431243432,1.5803914168,0.1830539468,-1.8197842832,-0.4112106832,-0.6090916932,0.1463002968,-0.1776492032,-0.2360609532,-0.6906795932,-0.0215511632,-1.4010216832,0.6553227068,-1.2805205832,-0.6185942632,-0.1031320932,0.2910673768,-0.0336501402,1.7811524168,1.6649543168,0.6694183168,0.1347960068,-0.7646246332,-0.4183546732,-1.3629867832,-0.3315472132,-0.4279288232,1.7942780168,0.7962070368,0.1079717578,-0.2497818432,0.1258052638,0.8091738968,2.2798323168,0.3698691468,0.9062863568,0.7223690168,0.5977400468,1.0446128168,1.3023475168,0.0871334818,-0.2711552532,0.2696900568,1.0396743468,1.2588941168,-0.4997200032,0.9552107568,-0.1291587132,-0.2929722232,0.5844691968,0.9750936268,-0.5272702332,-1.4556229832,-0.0073632272,0.5960071868,0.1264803038,1.6205059168,-0.2877917832,0.8381372468,-0.7418177332,-1.2201390832,0.7096374068,-1.3496497832,-1.2575800832,-0.5620397432,-1.4455820832,0.6015253768,-0.2343718732,-1.2507023832,-0.8454527632,-0.9435376632,-0.0286352272,-2.3716084832,-0.6513665932,-1.3484065832,0.3726302168,0.6415268968,0.1901578168,-0.0584565732,-2.5919983832,0.0710358308,-0.8333277132,-0.2224950332,-0.2845954932,-1.1152650832,0.6230356268,0.2827381468,-0.3079025032,0.9350808968,1.6212812168,-1.0651914832,0.0170513698,-1.0676452832,0.7938165968,0.5431494468,-0.4742784632,-0.5162270732,-0.7103882032,0.9687951068,-0.2055379032,-0.1068521032,-1.2154329832,0.3556022968,2.7332593168,0.3326787068,-1.3798205832,0.2897681368,-1.3927904832,0.1915555468,-1.6500901832,0.7621708968,1.1847552168,1.5949187168,1.4266125168,-0.7151098332,0.4856451568,0.9540803168,-1.0311031832,0.2447443268,0.8058456368,-1.2452047832,-0.9099794032,0.8211569168,0.0366513472,0.5674692268,1.4072542168,0.5250217468,-0.7440834332,0.7949816868,-0.1239054932,-0.7732456932,2.1370476168,0.1231349488,-0.8943126532,0.6787211968,1.7250104168,0.6366165868,0.8331352968,0.1482362668,-0.1155961432,0.9138898668,-0.1517765132,0.1184569018,-0.4836524232,-0.6425019632,-0.2254052332,-1.1453635832,0.2915613968,0.1454340368,0.0019755398,-2.2045998832,-0.4677940632,0.2922250568,0.4121791268,0.2221790368,0.0056990048,-1.5603274832,0.3823544568,-0.0881523932,0.5281722068,0.6417331168,-0.0430484702,0.3682744668,-0.2921608132,-0.2794667832,-0.3393917132,-0.9103885132,0.2765580168,1.2782271168,-0.5355491832,-0.4585549532,0.7658459368,0.0824809018,1.5842612168,-1.6580703832,-0.9907588832,-0.7207252232,2.2194084168,0.4745945168,-0.4007827632,0.0729782848,-0.2726884532,1.0208281168,0.0612772908,0.8609452468,0.7453235168,-0.1882881432,-0.0707071632,0.1709229368,-0.7564272832,-0.1956295432,-0.0464808212,-0.9802817832,0.9805203368,-1.0887368832,-0.6677196732,-1.1265184832,1.1084195168,-0.6374113232,-1.6827902832,0.8561820168,1.4848490168,0.7152546168,0.1816473768,-0.8165515132,-0.7092681432,1.2725975168,1.1937368168,-0.5650425932,0.8491403368,0.2601152668,-0.3304786532,-0.7890480132,0.3298487168,-1.7759091832,-1.5300689832,2.0586481168,-0.0289996072,2.6718918168,-0.2003345332,0.2162589068,0.9661894968,-0.1355705832,-0.4787224932,1.4749444168,-0.8271350332,0.8505242068,-0.4676522232,0.7864965468,0.8908807168,-0.7869183032,0.5759768468,1.0758308168,-1.0090415832,0.4050961568,0.0061957998,-1.1846531832,-0.2321164532,-0.1174528232,-1.0405923832,-1.9112302832,-0.8665048432,0.0374036891,-1.6805074832,1.3060597168,-0.5574509532,-2.0209423832,0.1538933568,1.5305967168,0.0959845588,0.2049631568,0.0161044978,0.2165586068,0.9251501668,0.2252768368,0.7982778368,0.5510169668,0.1748625568,0.3230865268,-0.9398651432,-0.9011050132,0.0299249008,0.3973276968,-0.8517264132,0.8550934368,0.1525197668,2.7746264168,0.4540615668,-1.2638795832,0.4267889068,0.5424860968,-0.4678031732,0.2779044668,-0.5548426932,0.0637532358,0.4624251768,1.2340861168,0.8141966668,-2.6012394832,0.3284123168,0.8690750368,0.0348599243,0.9014205768,0.8177706568,1.3489277168,1.2744849168,1.0015460868,-1.6115652832,-0.9474135932,0.7282184368,-0.9318875932,-0.5637432232,0.7297761568,0.0630312728,1.1067830168,-1.2980674832,0.5224928568,-1.5909915832,-1.3996825832,0.3367638368,-0.0973816632,-1.0873582832,-0.2495556332,-0.5395534532,-0.8533658632,0.2915834968,-1.4466809832,0.3564910768,-1.9821019832,0.5719721168,0.3864533168,0.8011750268,-0.6489577632,0.7231609668,-1.0295585832,0.9427545068,-2.0801099832,0.3276946868,-0.6903405132,-0.7303932132,0.1948241368,-0.2938607132,1.0137431368,-0.0642539432,1.0564743168,-0.4323643432,0.1119305618,0.4415744868,1.1593086168,0.6634335468,-0.2446920632,-1.3287908832,-0.6428860132,0.3746671568,-0.9547394032,0.3344005168,1.1500608168,0.2879410268,0.2079581968,0.4492137568,1.2589636168,1.4914067168,-0.9821550832,0.2484004268,0.6318646068,-0.2210412032,2.5383002168,0.8989300868,-0.8079712132,0.8548612568,0.7432241368,0.8029207468,-1.6699267832,1.5800036168,-1.5668643832,1.1525084168,-1.0667215832,0.4284518068,1.0082132068,0.8612796468,0.0800311958,-0.8830299832,-0.0689362432,-0.7600472732,-1.6220234832,-0.8584181832,0.6313320868,0.5971416968,-0.3721901932,0.1047772758,0.5004139868,0.2419963268,0.3005402368,2.1237122168,-2.2342540832,0.3820048468,0.3328763068,0.7052432668,-0.5378778132,0.9307343268,0.2148534568,0.8918038568,1.0067514468,1.3649004168,-0.0213624862,1.3600355168,0.2709997468,-1.3866547832,-0.1067185832,-0.4619851432,-1.6861590832,-0.3744898032,-0.5719861032,0.7637590968,0.3823462068,0.9258274968,0.3272276268,-0.1025589332,-0.0466634832,0.3321432868,1.2078132168,0.8487117668,-1.3126610832,0.1638753168,-0.1791954932,0.6147147568,-0.2571580132,1.1772592168,-0.1363739932,-1.4240842832,1.4383281168,0.4838180868,0.6083664668,-0.6506401732,0.8768516668,-2.1943959832,1.1406265168,0.0413655009,-1.5715908832,-1.1857443832,0.2503869968,0.2639244168,-0.9630906832,-0.4100846732,1.4424357168,-0.4189815632,0.0756983218,0.8417656168,0.9397982068,0.1808741468,-1.5761639832,-1.6036232832,0.4716892968,-0.6942484232,0.6079085568,-1.3411842832,0.5032504068,0.6723660668,0.4228297268,-0.9703474832,-0.3042603632,0.4848947168,-1.5472578832,-0.6584342932,-1.0346181832,1.0452021168,1.7724629168,0.7520146268,-0.7049144432,0.2718448068,-0.1805141932,-0.8102928432,0.3886092168,0.1527468968,-1.0900565832,-0.6401411232,-0.2348738132,0.6977722668,-1.2054115832,-0.5545566132,-0.4388301332,1.0263545868,1.8051031168,1.4703841168,0.9547454468,0.3698050668,0.1126009568,-1.4567802832,-0.3752407732,0.0219456188,0.2714069468,-0.9652133832,-0.6216397532,0.6011589968,-1.1455596832,-0.7324990832,0.3140244868,1.5779581168,-1.0093002832,0.6685409868,-0.7546436532,-0.2705395732,-0.5592273332,1.3020427168,0.9014666668,-2.0623094832,-0.3179547632,0.5965391368,-1.5134013832,-0.1636833032,-0.3825851332,0.5367606668,-0.8279251932,0.1228100748,-0.4786367232,-1.3708784832,-0.3413107632,-0.4149398032,-0.2484882732,-0.2582415932,-1.5456113832,1.1372691168,1.3671493168,-0.0834972132,-0.6941814032,0.2567012568,-0.3575467132,0.1079201638/*,-1.7150131832,1.7297304168,0.3703826268*/}; 
     3 
     4static const float *randn_i=&RArandn[0]; 
     5const static float *randn_last=&RArandn[MAXrandn-1]; 
    66 
    77 
    88#define MAXrand 100 
    9 static const floatx RArandu[MAXrand]={0.8147, 0.9058, 0.1270, 0.9134, 0.6324, 0.0975, 0.2785, 0.5469, 0.9575, 0.9649, 
     9static const float RArandu[MAXrand]={0.8147, 0.9058, 0.1270, 0.9134, 0.6324, 0.0975, 0.2785, 0.5469, 0.9575, 0.9649, 
    1010                                      0.1576, 0.9706, 0.9572, 0.4854, 0.8003, 0.1419, 0.4218, 0.9157, 0.7922, 0.9595, 0.6557, 0.0357, 
    1111                                      0.8491, 0.9340, 0.6787, 0.7577, 0.7431, 0.3922, 0.6555, 0.1712, 0.7060, 0.0318, 0.2769, 0.0462, 
    … …  
    1717                                      0.9340, 0.1299, 0.5688, 0.4694, 0.0119, 0.3371 
    1818                                     }; 
    19 static const floatx *randu_i=&RArandu[0]; 
    20 const static floatx *randu_last=&RArandu[MAXrand-1]; 
     19static const float *randu_i=&RArandu[0]; 
     20const static float *randu_last=&RArandu[MAXrand-1]; 
     21 
     22float qrandu(); 
     23float qrandn(); 
     24void rng_init(); 
     25 

  • applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp

    r1468 r1469  
    1 #include "math.h" 
     1#include <math.h> 
    22#include <stdio.h> 
    33#include "fast_exp.h" 
    44#include "fastexp.h" 
    55#include "mpf_double.h" 
     6 
     7//Parameters of PMSM 
     8#define Lsd (3.465e-3*0.9)      //      *0.9)//H        ... zajimave - chodi to i pri Lsd = Lsq 
     9#define Lsq 3.465e-3//H 
     10//#define Ls  3.465e-3//H 
     11#define Rs 0.28//Ohm 
     12#define Fpm (0.1989*1.1)        //*1.15)        //V.s           // !!xx!! 
     13#define pp 4 
     14#define Ism 35.//40. 
    615 
    716static floatx om[N]; 
    … …  
    1423static floatx w[N]; 
    1524static floatx lwi[N]; 
     25static int i_best; 
    1626 
    1727static floatx qth; 
    … …  
    2030static floatx rinv; 
    2131 
    22 #if 1 
    23 #include "RArandn.h" 
    24 floatx qrandu() { 
    25         if ( randu_i==randu_last ) 
    26                 randu_i =&RArandu[0]; 
    27         else 
    28                 randu_i++; 
    29  
    30         return *randu_i; 
    31 } 
    32  
    33 floatx qrandn() { 
    34         if ( randn_i==randn_last ) 
    35                 randn_i =&RArandn[0]; 
    36         else 
    37                 randn_i++; 
    38  
    39         return *randn_i; 
    40 } 
    41  
    42 void rng_init() 
    43 { 
    44 randu_i=&RArandu[0]; 
    45 randn_i=&RArandn[0]; 
    46 } 
    47  
    48 #else 
    49  
    50 #include "rnor.h" 
    51 floatx qrandu() { 
    52         return UNI; 
    53 } 
    54  
    55 floatx qrandn() { 
    56         return PolarRNG(); 
    57 } 
    58  
    59 void rng_init() 
    60 { 
    61 zigset(86947731 ); 
    62 } 
    63  
    64  
    65 #endif 
     32// staticke promenne pro model 
     33static float _ad; 
     34static float _aq; 
     35static float _b; 
     36static float _cd; 
     37static float _cq; 
     38static float _dd; 
     39static float _dq; 
    6640 
    6741 
    … …  
    9569 
    9670        while ( i_from<N ) { 
     71                w[i_from]=1.0/N; 
    9772                while ( N_babies[i_from]>1 ) { // 1 baby stays where it is 
    9873                         
    … …  
    139114        floatx ydC; 
    140115 
    141         floatx maxlwi, sumlwi; 
     116        floatx maxlwi, sumlwi,sumw2,neff_th; 
    142117        int i; 
    143118         
    … …  
    145120        floatx *w_i, *lw_i; 
    146121        floatx *sth_i, *cth_i; 
     122        floatx sumw_inv; 
    147123// implementation starts here 
    148124        th_i = &th[0]; 
    … …  
    155131        for ( i=0; i<N; i++ ) { 
    156132 
     133          /////////////////////////// HACK!!!!!!!!!!! 
     134          //om[i] = 0; 
     135          /////////////////////////// HACK!!!!!!!!!!! 
     136           
    157137                // time update 
    158138                *Pt_i = 1.0*1.0*(*Pt_i)+qom; // Pt is now predictive variance 
    159                 *th_i += *om_i*_dt+qth *  qrandn() ; 
     139                floatx ff =qrandn() ; 
     140//              printf("%f\n", ff); 
     141                *th_i += *om_i*_dt+qth * ff;  
    160142                while ( *th_i>M_PI ) *th_i -=2*M_PI; 
    161143                while ( *th_i<-M_PI ) *th_i+=2*M_PI; 
    … …  
    170152                usq = -(*sth_i)*usa+(*cth_i)*usb; 
    171153 
    172                 Cd = isq*_dt; 
    173                 Cq = -_b - isd* _dt; 
     154                Cd = isq*_dd; 
     155                Cq = -_b - isd* _dq; 
    174156 
    175157                difid=isd- _ad  * (*isdm_i) - _cd *(usd); 
    … …  
    211193 
    212194        } 
    213 /*      maxlwi=-1e10; 
     195        maxlwi=-1e10; 
    214196        lw_i = &lwi[0]; 
     197        i_best = 0; 
    215198        for ( i=0;i<N;i++,lw_i++ ) { 
    216                 if ( *lw_i>maxlwi ) maxlwi=*lw_i; 
    217         }*/ 
     199                if ( *lw_i>maxlwi ) {maxlwi=*lw_i;i_best=i;} 
     200        } 
     201/*!!*/ 
    218202        lw_i = &lwi[0]; 
    219203        w_i = &w[0]; 
    220204        for ( i=0;i<N;i++,lw_i++,w_i++ ) { 
    221 //              *lw_i-=maxlwi; 
    222                 (*w_i)=fasterexp ( *lw_i ); // always resample -> wmin=1/n; 
     205                *lw_i-=maxlwi; 
     206                (*w_i)*=fasterexp ( *lw_i ); // do not always resample  
    223207        } 
    224208 
    225209        sumlwi=0.0; 
     210        sumw2=0.0; 
    226211        w_i = &w[0]; 
    227         for ( i=0;i<N;i++,w_i++ ) sumlwi+=*w_i; 
     212        for ( i=0;i<N;i++,w_i++ ) {sumlwi+=*w_i;sumw2+=*w_i**w_i;} 
     213        sumw_inv = 1.0/sumlwi; // multiplication is faster than division! 
    228214        w_i = &w[0]; 
    229         for ( i=0;i<N;i++,w_i++ ) *w_i/=sumlwi; 
    230          
    231         // NEFF 
    232         floatx sw2=0.0; 
    233         for ( i=0;i<N;i++ ) sw2+=w[i]*w[i]; 
    234         printf("%f\n",1./sw2*N); 
    235          
    236         resample(); 
     215        for ( i=0;i<N;i++,w_i++ ) *w_i*=sumw_inv; 
     216         
     217        neff_th = 0.8*N*sumw2*sumw_inv*sumw_inv; 
     218        if (1<neff_th) 
     219          resample(); 
    237220} 
    238221 
    … …  
    240223void mpf_init(floatx qom0, floatx qth0, floatx r0) { 
    241224         
    242         rng_init(); 
     225//      rng_init(); 
    243226         
    244227        int i; 
    … …  
    259242                w[i]=1.0/N; 
    260243        } 
     244 
     245   // inicializace promennych 
     246   #if (VYBER_MODELU==0) 
     247    // plny model s respektovanim Lsd/Lsq 
     248    _ad = (1.-Rs*_dt/(Lsd)); 
     249    _aq = (1.-Rs*_dt/(Lsq)); 
     250    _b = Fpm/Lsq*_dt; 
     251    _cd = _dt/(Lsd); 
     252    _cq = _dt/(Lsq); 
     253    _dd = _dt*Lsq/Lsd; 
     254    _dq = _dt*Lsd/Lsq; /**/ 
     255  #else 
     256  // parametry modelu identifikovane z namerenych dat z prototypu v EL204 
     257    _ad = 0.9880; 
     258    _aq = 0.9817; 
     259    _b = 0.0076; 
     260    _cd = 0.0289; 
     261    _cq = 0.0292; 
     262    _dd = _dt; 
     263    _dq = _dt; 
     264  #endif    
    261265 
    262266} 
    … …  
    277281 
    278282} 
     283 
     284void mpf_best( floatx *Ecosth, floatx *Esinth, floatx *Eome )  
     285{ 
     286        *Ecosth=cth[i_best]; 
     287        *Esinth=sth[i_best]; 
     288        *Eome=om[i_best]; 
     289} 
     290 
     291 
    279292void mpf_th(floatx th1[N]){ 
    280293        int i; 
    … …  
    330343                usq = -sth*usa+cth*usb; 
    331344 
    332                 Cd = isq*_dt; 
    333                 Cq = -_b - isd* _dt; 
     345                Cd = isq*_dd; 
     346                Cq = -_b - isd* _dq; 
    334347 
    335348                difid=isd- _ad  *isdm - _cd *usd; 

  • applications/pmsm/simulator_zdenek/ekf_example/mpf_double.h

    r1468 r1469  
    11// Bayesian Filtering for PMSM using Marginalized Particle Filter  
    22 
    3 #include "parametry_motoru.h" 
    4 #define Fpm 0.1989 
    5 //#define pi 3.141596 
     3//#define Fpm 0.1989 
     4#ifndef M_PI 
     5#define M_PI 3.141596 
     6#endif 
    67 
    78#define N 4 
    89 
    9 #define Lsd Ls*0.9 
    10 #define Lsq Ls 
    11 #define _dt 0.000125 
    12 #define _ad (1.-Rs*_dt/(Lsd)) 
    13 #define _aq (1.-Rs*_dt/(Lsq)) 
    14 #define _b  Fpm/Lsq*_dt 
    15 #define _cd  _dt/(Lsd) 
    16 #define _cq  _dt/(Lsq) 
     10//#define Lsd Ls*0.9 
     11//#define Lsq Ls 
    1712 
     13#define VYBER_MODELU    0       // 0 = plny model s respektovanim Lsd/Lsq ... implicitne 
     14                                                        // 1 = parametry modelu z identifikace 
     15#define _dt     0.000125 
    1816 
    19 # define floatx float 
     17#define floatx float 
    2018 
    2119floatx qrandn(); 
    … …  
    2826void mpf_th(floatx th1[N]); 
    2927void mpf_om(floatx om1[N]); 
     28void mpf_best( floatx *Ecosth, floatx *Esinth, floatx *Eome); 
    3029 
    3130floatx kalman_om ( const floatx isa, const floatx isb , const floatx usa, const floatx usb, const floatx th );