From d43bbf21d46d735d0ffd23f5b758345fdf68c970 Mon Sep 17 00:00:00 2001 From: Giulia Vezzani Date: Fri, 28 Oct 2016 16:36:57 +0200 Subject: [PATCH] final version --- outputs/outputMUPF0.off | 229 +--------------------------- src/unscentedParticleFilter-rec.cpp | 10 +- src/unscentedParticleFilter.cpp | 14 +- 3 files changed, 17 insertions(+), 236 deletions(-) diff --git a/outputs/outputMUPF0.off b/outputs/outputMUPF0.off index 07f6442..dbf895e 100644 --- a/outputs/outputMUPF0.off +++ b/outputs/outputMUPF0.off @@ -1,231 +1,4 @@ OFF -77 150 0 +0 0 0 --0.0749683 0.0589507 0.279363 --0.0761385 0.0525793 0.289893 --0.0669718 0.0534753 0.287575 --0.0666234 0.0430255 0.277165 --0.0655026 0.0540759 0.279081 --0.0743158 0.0273069 0.293519 --0.0716726 0.0273162 0.276766 --0.0648353 0.00803546 0.293318 --0.0675501 0.0215328 0.292907 --0.063777 -0.0082408 0.287297 --0.06476 -0.0198269 0.29984 --0.068497 -0.0603096 0.308756 --0.051815 -0.193291 0.32903 --0.0814573 0.0473548 0.287685 --0.074873 0.047784 0.27095 --0.0829321 0.0526741 0.276279 --0.0784044 0.0063829 0.293341 --0.0832783 0.0529728 0.28296 --0.0769757 -0.0293879 0.297303 --0.0778015 0.0242381 0.279177 --0.0757775 -0.0387807 0.292893 --0.0688771 -0.111295 0.30352 --0.0611194 -0.0279223 0.293766 --0.058149 -0.217731 0.336731 --0.0626131 -0.10916 0.301392 --0.0518572 -0.215933 0.334637 --0.0489955 -0.215607 0.327056 --0.0551062 -0.224432 0.322374 --0.0500027 -0.218912 0.325279 --0.047841 -0.2311 0.333284 --0.0537466 -0.225692 0.343331 --0.0492416 -0.234097 0.34338 --0.0538066 -0.229441 0.354162 --0.0605938 -0.226929 0.354719 --0.0559456 -0.240866 0.336602 --0.050262 -0.240641 0.345335 --0.0580936 -0.243729 0.351391 --0.069641 -0.156667 0.415975 --0.0680038 -0.154358 0.422424 --0.0764262 -0.157856 0.412239 --0.0769292 -0.1527 0.430723 --0.072135 -0.168875 0.436509 --0.0766468 -0.180895 0.425742 --0.0703631 -0.180528 0.429656 --0.0725637 -0.201098 0.413781 --0.0606622 -0.223791 0.406117 --0.0610471 -0.222981 0.393944 --0.0681715 -0.21935 0.399129 --0.0665603 -0.199474 0.420128 --0.0640368 -0.215817 0.406809 --0.0694674 -0.211708 0.424024 --0.0622519 -0.22395 0.412469 --0.0691133 -0.185325 0.438672 --0.0793378 -0.183208 0.444795 --0.0612689 -0.231124 0.405146 --0.0683517 -0.159993 0.320503 --0.0674059 -0.165422 0.315391 --0.0684001 -0.144622 0.319795 --0.0645411 -0.21706 0.331816 --0.0632535 -0.219906 0.323819 --0.0620905 -0.2295 0.326744 --0.0640529 -0.228767 0.341894 --0.0650854 -0.237891 0.337114 --0.0682338 -0.230966 0.357817 --0.0660134 -0.240429 0.354181 --0.0872114 -0.158819 0.416716 --0.0839132 -0.176465 0.425176 --0.082472 -0.147706 0.423257 --0.0862229 -0.168423 0.43334 --0.0857276 -0.184956 0.428504 --0.0769995 -0.213288 0.408355 --0.0726599 -0.223717 0.392866 --0.0767894 -0.22538 0.403446 --0.077402 -0.217933 0.418496 --0.0850783 -0.185674 0.439298 --0.0746661 -0.232504 0.401789 --0.0686333 -0.2341 0.408042 -3 4 14 0 -3 14 4 3 -3 0 2 4 -3 2 0 1 -3 3 6 14 -3 4 2 7 -3 8 7 2 -3 5 8 2 -3 2 1 5 -3 9 3 4 -3 6 3 9 -3 9 4 22 -3 24 6 9 -3 22 4 7 -3 7 8 10 -3 5 10 8 -3 10 5 11 -3 22 7 12 -3 12 7 10 -3 15 0 14 -3 15 17 0 -3 1 0 17 -3 1 17 13 -3 19 14 6 -3 15 14 19 -3 5 1 13 -3 5 13 16 -3 16 13 17 -3 15 18 17 -3 16 17 18 -3 20 15 19 -3 19 6 24 -3 18 15 20 -3 24 21 19 -3 20 19 21 -3 11 5 16 -3 11 16 57 -3 18 57 16 -3 12 10 25 -3 25 10 11 -3 25 11 23 -3 28 9 22 -3 28 22 26 -3 24 9 28 -3 12 26 22 -3 27 24 28 -3 26 12 29 -3 29 12 25 -3 29 28 26 -3 27 28 29 -3 29 25 30 -3 25 23 30 -3 29 34 27 -3 31 35 29 -3 29 30 31 -3 35 34 29 -3 31 30 32 -3 30 33 32 -3 32 46 31 -3 33 46 32 -3 45 31 46 -3 33 47 46 -3 35 36 34 -3 36 35 54 -3 35 31 45 -3 35 45 54 -3 76 36 54 -3 67 38 37 -3 37 39 67 -3 43 37 38 -3 37 42 39 -3 42 37 43 -3 40 38 67 -3 38 40 41 -3 41 40 53 -3 43 38 41 -3 43 41 52 -3 44 42 43 -3 52 48 43 -3 44 43 48 -3 47 44 49 -3 46 49 45 -3 47 49 46 -3 45 49 51 -3 49 48 51 -3 44 48 49 -3 52 50 48 -3 51 48 50 -3 52 41 53 -3 53 50 52 -3 45 51 54 -3 54 51 76 -3 50 76 51 -3 21 56 20 -3 18 20 56 -3 57 23 11 -3 55 18 56 -3 57 18 55 -3 24 59 21 -3 56 21 59 -3 56 58 55 -3 57 55 58 -3 59 24 27 -3 58 56 59 -3 27 60 59 -3 23 57 58 -3 58 61 23 -3 60 62 58 -3 58 59 60 -3 62 61 58 -3 27 34 60 -3 34 62 60 -3 30 23 61 -3 33 30 61 -3 62 63 61 -3 33 61 63 -3 63 71 47 -3 47 33 63 -3 71 63 72 -3 64 63 62 -3 62 34 36 -3 64 62 36 -3 64 72 63 -3 64 36 76 -3 64 76 75 -3 67 39 65 -3 39 66 65 -3 39 42 66 -3 68 53 40 -3 40 67 68 -3 68 67 65 -3 66 69 68 -3 68 65 66 -3 69 66 42 -3 69 42 44 -3 44 70 69 -3 70 44 47 -3 71 70 47 -3 72 70 71 -3 73 70 72 -3 73 69 70 -3 69 74 68 -3 53 68 74 -3 53 74 73 -3 73 50 53 -3 69 73 74 -3 75 72 64 -3 76 73 75 -3 72 75 73 -3 50 73 76 diff --git a/src/unscentedParticleFilter-rec.cpp b/src/unscentedParticleFilter-rec.cpp index cf542b2..ac9e723 100644 --- a/src/unscentedParticleFilter-rec.cpp +++ b/src/unscentedParticleFilter-rec.cpp @@ -543,7 +543,15 @@ void UnscentedParticleFilter::resampling() i++; } - new_x[j]=x[i]; + Vector tmp(6,0.0); + + tmp[0] = Rand::scalar(x[i].x_corr[0] - x[i].P_corr(0,0), +x[i].x_corr[0] + x[i].P_corr(0,0)); + tmp[1] = Rand::scalar(x[i].x_corr[1] - x[i].P_corr(1,1), +x[i].x_corr[1] + x[i].P_corr(1,1)); + tmp[2] = Rand::scalar(x[i].x_corr[2] - x[i].P_corr(2,2), +x[i].x_corr[2] + x[i].P_corr(2,2)); + tmp[3] = Rand::scalar(x[i].x_corr[3] - x[i].P_corr(3,3), +x[i].x_corr[3] + x[i].P_corr(3,3)); + tmp[4] = Rand::scalar(x[i].x_corr[4] - x[i].P_corr(4,4), +x[i].x_corr[4] + x[i].P_corr(4,4)); + tmp[5] = Rand::scalar(x[i].x_corr[5] - x[i].P_corr(5,5), +x[i].x_corr[5] + x[i].P_corr(5,5)); + new_x[j].x_corr=tmp; new_x[j].weights=1.0/params.N; } diff --git a/src/unscentedParticleFilter.cpp b/src/unscentedParticleFilter.cpp index 14104f6..aa9b150 100644 --- a/src/unscentedParticleFilter.cpp +++ b/src/unscentedParticleFilter.cpp @@ -597,13 +597,13 @@ void UnscentedParticleFilter::resampling() //new_x[j]=x[i]; Vector tmp(6,0.0); - tmp[0] = Rand::scalar(x[i].x_corr[0] - x.P_corr(0,0), +x[i].x_corr[0] + x.P_corr(0,0); - tmp[1] = Rand::scalar(x[i].x_corr[1] - x.P_corr(1,1), +x[i].x_corr[1] + x.P_corr(1,1); - tmp[2] = Rand::scalar(x[i].x_corr[2] - x.P_corr(2,2), +x[i].x_corr[2] + x.P_corr(2,2); - tmp[3] = Rand::scalar(x[i].x_corr[3] - x.P_corr(3,3), +x[i].x_corr[3] + x.P_corr(3,3); - tmp[4] = Rand::scalar(x[i].x_corr[4] - x.P_corr(4,4), +x[i].x_corr[4] + x.P_corr(4,4); - tmp[5] = Rand::scalar(x[i].x_corr[5] - x.P_corr(5,5), +x[i].x_corr[5] + x.P_corr(5,5); - new_x[j]=tmp; + tmp[0] = Rand::scalar(x[i].x_corr[0] - x[i].P_corr(0,0), +x[i].x_corr[0] + x[i].P_corr(0,0)); + tmp[1] = Rand::scalar(x[i].x_corr[1] - x[i].P_corr(1,1), +x[i].x_corr[1] + x[i].P_corr(1,1)); + tmp[2] = Rand::scalar(x[i].x_corr[2] - x[i].P_corr(2,2), +x[i].x_corr[2] + x[i].P_corr(2,2)); + tmp[3] = Rand::scalar(x[i].x_corr[3] - x[i].P_corr(3,3), +x[i].x_corr[3] + x[i].P_corr(3,3)); + tmp[4] = Rand::scalar(x[i].x_corr[4] - x[i].P_corr(4,4), +x[i].x_corr[4] + x[i].P_corr(4,4)); + tmp[5] = Rand::scalar(x[i].x_corr[5] - x[i].P_corr(5,5), +x[i].x_corr[5] + x[i].P_corr(5,5)); + new_x[j].x_corr=tmp; new_x[j].weights=1.0/params.N; }