In this lab you will implement a simple particle filter and test your implementation. There will be no report, just print out a few runs. Please do not take the time to do anything fancy. Just hard code the model and run it a few times.

## Messy, Non-linear, Non-normal Model

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)

### Prior

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

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

### System or Transition Model

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)$

### Observation or Sensor Model

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)\!$

### Sample data

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.

### True values and 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 your 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.

#### Run #1

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 \\

#### Run #2

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 \\

#### Run #3

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 \\

#### Run #4

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 \\

#### Run #5

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 \\