This test data is based on the robot navigation systems we talked about in class. The robot's true position is given in x, y coordinates. The robot moves by specifying a direction (in radians from vertical) and a distance. The robot senses its position as the distance from 2 beacons ($d_{A}$ the distance from beacon A and $d_{B}$ the distance from beacon B)

$x_{0} \sim N(0,1) \!$

$y_{0} \sim N(0,1) \!$

The movement of the robot is random in $d$, the distance travelled, and $\theta$, the angle chosen. The intended distance (5) and direction $\left(\frac{\pi}{5}\right)$ are fixed.

$ d \sim N(5,1)\!$

<!– Warning this line is broken. The current version of the wiki will not parse it. I have commented it out. The line following it seems to work. $ \theta \sim \mathit{Uniform} ((\pi/5-\pi/36), (\pi/5+\pi/36))\!$ –> $ \theta \sim \mathit{Uniform} \left(\frac{\pi}{5}-\frac{\pi}{36},\frac{\pi}{5}+\frac{\pi}{36}\right)$

$x_{t+1}|x_{t} = x_{t} + d \cdot \cos \left(\theta\right)$

$y_{t+1}|y_{t} = y_{t} + d \cdot \sin \left(\theta\right)$

The robot senses its position as the distance from beacon A ($d_{A}$) and the distance from beacon B($d_{B}$). Both are imperfect measures as shown below:

$d_{A} = \sqrt{(-100-x)^{2}+(100-y)^{2}} $

$d_{B} = \sqrt{(150-x)^{2}+(90-y)^{2}} $

$r_{A} \sim N (d_{A}, 1)\!$

$r_{B} \sim N (d_{B}, 1)\!$

Your test data is given below. Columns 1 and 2 are the TRUE x and y (the output should be close to this). Columns 3 and 4 are the input data.

6.23506410875 & 3.47428776487 & 143.69345025 & 166.824055471 \\
10.1040856095 & 6.77685868518 & 145.664295064 & 164.501752829 \\
14.0195541954 & 10.0769108846 & 144.591594567 & 157.474359865 \\
18.0100054682 & 13.0579715054 & 146.50065381 & 152.469730508 \\
23.4951921318 & 17.5821848851 & 148.997244853 & 145.093420417 \\
27.0628509846 & 20.6762744234 & 148.636960211 & 142.497183979 \\
31.2664169184 & 23.6241997406 & 152.357648068 & 135.919999894 \\
33.479183402 & 25.2895006721 & 152.367509975 & 133.667699492 \\
38.1615374 & 28.4013522307 & 155.708667162 & 126.696182098 \\
42.9530563425 & 32.0141204627 & 159.03213926 & 122.950895656 \\
46.6419839935 & 35.0986332487 & 159.068406295 & 116.296800819 \\
51.2820776483 & 38.372507025 & 164.570101706 & 111.140841502 \\
54.1752013771 & 40.6643076262 & 164.408405506 & 107.94706493 \\
59.1125982069 & 43.8680603051 & 170.250724817 & 101.808614448 \\
62.951248513 & 46.4661999927 & 171.489734721 & 96.7475872389 \\
67.0126051217 & 49.4875173971 & 173.631500934 & 91.6295157657 \\
71.6046962629 & 52.7219441693 & 178.451332801 & 85.3706909502 \\
75.9683899617 & 56.0179471343 & 182.747629193 & 80.8448050049 \\
78.6262726091 & 57.9096442404 & 183.013423256 & 78.7848074168 \\
81.4209412171 & 60.0226353414 & 185.554608457 & 74.7064611403 \\

Use the noisy input values given above (NOT THE TRUE VALUES GIVEN ABOVE! They are included for reference only!) to infer the location of the robot. Report you findings in the form of a mean and a variance for x and a mean and a variance for y for each time step.

Given below is sample output from five separate runs of the particle filter. Note that columns 1 and 2 are the mean x and mean y of the particles. Columns 3 and 4 are the variances of the x and y values of the particles. These tests were run with 100 particles.

These answers are based on simulation. As with any simulation based approach, it is highly unlikely that you will get the same answer. It is also highly unlikely that you will get very different answers. We have listed results for 5 runs so that you can get a feel for how widely the results are varying.

5.88606083114 & 3.82965157521 & 0.406162538 & 0.595028642435 \\
9.5895510708 & 5.96734479066 & 0.238958305025 & 0.511479417689 \\
13.9570561381 & 9.51319610885 & 0.389455944163 & 0.403564484278 \\
18.2636571862 & 12.7184190759 & 0.386009572459 & 0.377006008423 \\
23.4404892475 & 16.5467159461 & 0.24622051089 & 0.295367855259 \\
26.5832671078 & 19.14311067 & 0.396216711862 & 0.319908203942 \\
31.2955615779 & 22.7975256661 & 0.335389341978 & 0.534330351755 \\
33.7744730723 & 24.8619576825 & 0.3590174446 & 0.472543788055 \\
38.5493198326 & 28.4463186853 & 0.30825053786 & 0.396619283176 \\
42.4626316089 & 31.0271340659 & 0.406454404817 & 0.450008133305 \\
46.663814675 & 34.3545973368 & 0.303326033837 & 0.481012206298 \\
51.655023511 & 37.7682439691 & 0.367525996288 & 0.339870054005 \\
54.4571680282 & 40.1109639611 & 0.503938147108 & 0.45904147466 \\
59.6515839972 & 43.6211044183 & 0.439974227915 & 0.444299201888 \\
63.525034013 & 46.5594323783 & 0.361265534216 & 0.51375962392 \\
67.1743765276 & 49.5905225991 & 0.342189543936 & 0.429901304069 \\
72.1086561999 & 53.4556264678 & 0.378237525225 & 0.728710474767 \\ 
76.567742618 & 56.5293887642 & 0.289293515868 & 0.634180285706 \\
79.1270188235 & 57.9586543756 & 0.434339157277 & 0.803813497982 \\
82.0501163903 & 59.9415422436 & 0.366907666406 & 0.465639620991 \\

5.90120838885 & 3.48596079416 & 0.410220615472 & 0.640784190638 \\
9.65532627562 & 5.55317907568 & 0.431598761339 & 0.409372161322 \\
13.9047771233 & 9.24301171167 & 0.46877884686 & 0.393973997427 \\
18.1052641089 & 12.5548082786 & 0.382545903571 & 0.397583942926 \\
23.3499557467 & 16.5337033997 & 0.51289009183 & 0.55295880543 \\
26.3749129298 & 19.2347099885 & 0.46760420909 & 0.379873082476 \\
31.3115447069 & 22.7263976816 & 0.406034435131 & 0.396638439314 \\
33.9069846622 & 24.9513693827 & 0.311779662461 & 0.343403036264 \\
38.5066940161 & 28.4330795066 & 0.313941338174 & 0.408480442687 \\
42.4892746032 & 31.160354343 & 0.283057889157 & 0.549393655945 \\
46.8399976247 & 34.817415363 & 0.296337436505 & 0.561697511571 \\
51.5850536408 & 38.182910186 & 0.444321371028 & 0.746256859658 \\
54.4506139762 & 40.4473473645 & 0.269935037186 & 0.592328016062 \\
59.3156000509 & 43.5976772484 & 0.368267616909 & 0.731878454584 \\
63.2361696794 & 46.5055299961 & 0.283364284395 & 0.632700183679 \\
67.1646955188 & 49.7421940939 & 0.382679959684 & 0.695686010777 \\
72.2482270843 & 53.6356055617 & 0.3959014085 & 0.593108608392 \\
76.7521381502 & 56.5277348525 & 0.274855073516 & 0.556879352183 \\
78.8545306973 & 58.0688166127 & 0.247260219156 & 0.71518980039 \\
81.7222351678 & 60.1914263853 & 0.364485885583 & 1.11304011619 \\

5.98902166948 & 3.58887246005 & 0.439557450996 & 0.649198363974 \\
9.82425442442 & 5.46466258757 & 0.518735113352 & 0.446136745264 \\
13.9702174177 & 9.13153583768 & 0.507673804535 & 0.381702840145 \\
18.2108484341 & 12.3852376486 & 0.4986167737 & 0.423690851041 \\
23.3841245814 & 16.2194020335 & 0.396810321937 & 0.43986251429 \\
26.516224844 & 19.0639727645 & 0.291081872578 & 0.498476628174 \\
31.3225648253 & 22.7580953532 & 0.362950612632 & 0.620546621764 \\
33.9103675143 & 24.8106617289 & 0.376705726112 & 0.493838108635 \\
38.6439400389 & 28.5018409621 & 0.341111826772 & 0.571963072568 \\
42.5122229017 & 31.0432880513 & 0.270567498035 & 0.363991258437 \\
46.6834638194 & 34.4812703565 & 0.361872008625 & 0.563079974759 \\
51.5468834744 & 37.7607758157 & 0.351657407328 & 0.495894910709 \\
54.6519142065 & 40.2998931445 & 0.254043098169 & 0.388952216323 \\
59.438225706 & 43.4164088412 & 0.325076050347 & 0.48123020336 \\
63.2809368027 & 46.4454477146 & 0.369398455849 & 0.477788797306 \\
67.1185144134 & 49.5098458725 & 0.287811719328 & 0.410881626057 \\
72.1160042732 & 53.154701325 & 0.305105230299 & 0.438417479307 \\
76.6860064979 & 56.4604756723 & 0.278462069637 & 0.673003200095 \\
79.1162202166 & 58.2436435972 & 0.385564002066 & 0.200646667707 \\
81.929435887 & 60.3720849318 & 0.497895179587 & 0.311079162131 \\

5.88624337826 & 3.7439412516 & 0.310305563726 & 0.799427213409 \\
9.72452350615 & 5.6432423142 & 0.425372845898 & 0.636990917597 \\
13.8056056582 & 9.40620470543 & 0.404299083744 & 0.561936848698 \\
18.2311876057 & 12.7340026949 & 0.495547059337 & 0.410157271091 \\
23.6137962987 & 16.8677780634 & 0.221409808251 & 0.640081132743 \\
26.5003326347 & 19.5067150665 & 0.443841603573 & 0.436342944161 \\ 
31.1956518529 & 22.9130020412 & 0.507011725133 & 0.659247609133 \\
33.6163815494 & 24.9654390529 & 0.362743525132 & 0.29037704597 \\
38.4836438467 & 28.6080350467 & 0.387104056228 & 0.263519094321 \\
42.6055174717 & 31.3152461963 & 0.298788003045 & 0.393150520971 \\
46.72466167 & 34.7980049482 & 0.290479450733 & 0.399775602304 \\
51.530625137 & 38.1107577278 & 0.628768266492 & 0.597395763999 \\
54.4302228826 & 40.4436930241 & 0.349733107788 & 0.385677663206 \\
59.3285591272 & 43.8337373736 & 0.315102083366 & 0.372032350668 \\
63.3400429555 & 46.8806733312 & 0.241533135065 & 0.422926971 \\
67.0758437093 & 49.8737580421 & 0.321896328971 & 0.541575119893 \\
72.2498874692 & 53.6336043395 & 0.359548745166 & 0.604659514166 \\ 
76.8409852247 & 56.7624890478 & 0.440644713233 & 0.597188327263 \\
78.9412870183 & 58.5165898119 & 0.316463444459 & 0.319149439168 \\
81.9398635026 & 60.8522637713 & 0.246963848953 & 0.296186397182 \\

6.38357429484 & 4.3800656729 & 0.644583591163 & 0.993175692646 \\
9.63475106939 & 5.58268205225 & 0.456138464441 & 0.822490016545 \\
13.8820831265 & 9.66877285293 & 0.311661998347 & 0.468247134261 \\
18.1389174346 & 12.8970507138 & 0.475964885255 & 0.40154409429 \\
23.3517720275 & 16.701293073 & 0.39343636963 & 0.534702420785 \\
26.4159221543 & 19.3953262656 & 0.27320782791 & 0.467396802386 \\
31.0662554668 & 22.9257405819 & 0.470024581844 & 0.553494979869 \\
33.6427788147 & 24.9711557306 & 0.413800784277 & 0.725307074361 \\
38.3437049454 & 28.6596621046 & 0.395653749895 & 1.03392573405 \\
42.5285118239 & 31.1764024807 & 0.427130472647 & 0.657584335168 \\
46.5499232425 & 34.7039885957 & 0.466276132943 & 0.759819718423 \\
51.6343753958 & 38.0388212788 & 0.357101111261 & 0.435864413829 \\
54.3950877875 & 40.4677690995 & 0.323428220716 & 0.577846325276 \\
59.2770809917 & 43.5460444248 & 0.289253869865 & 0.474706015275 \\
63.3040062615 & 46.6767275246 & 0.253139794956 & 0.502057232244 \\
67.0961869079 & 49.5942998355 & 0.313269794211 & 0.490845771671 \\
72.1399797793 & 53.2413153121 & 0.281981424515 & 0.892160581338 \\
76.7465840448 & 56.3529243844 & 0.307196238799 & 0.829030694347 \\
79.0789993143 & 57.8942031449 & 0.158316301897 & 1.34352719789 \\
82.1240491058 & 59.7617686609 & 0.286444087997 & 0.868816885527 \\