GitHub

Example: Filtering Preexisting Data

In some cases, there is no need to perform control based on the particle belief and instead the goal is to filter a previously-generated sequence of measurements to remove noise or reconstruct hidden state variables. This tutorial will illustrate use of the runfilter function for that purpose.

First, we import the packages we will use. 

using Distributions
using Random
using DelimitedFiles
using ParticleFilters
using VegaLite

Model

We will use Euler-integrated Van Der Pol Oscillator Equations with noise added as the dynamics (f), and measurements of the position with gaussian noise added (the pdf is encoded in g).

This model is implemented below. For more information on defining models for particle filters, see the documentation.

const dt = 0.2
const mu = 0.8
const sigma = 1.0

function f(x, u, rng)
    xdot = [x[2], mu * (1 - x[1]^2) * x[2] - x[1] + u + 0.1 * randn(rng)]
    return x + dt * xdot
end

g(x1, u, x2, y) = pdf(Normal(sigma), y - x2[1])

Data

Suppose that the data we would like to filter are contained in text files. u.txt contains the control inputs, and y.txt contains the measurements. The code that generated this data can be found in the appendix below.

ys = vec(readdlm("y.txt"));
us = vec(readdlm("u.txt"));

Filtering

We can use an SIR particle filter to get a better estimate of the state. Note that we start with an initial belief consisting of particles with zero velocity and position uniformly distributed in $[-5, 5]$.

n = 10_000

fil = BootstrapFilter(f, g, n)

# construct initial belief
b0 = ParticleCollection([[10.0 * (rand() - 0.5), 0.0] for i in 1:n])

bs = runfilter(fil, b0, us, ys)
100-element Vector{Any}:
 WeightedParticleBelief{Vector{Float64}}([[-4.434308875689813, 0.8954016291953186], [3.4887860241535784, -0.6989839768324005], [-1.3462285886349346, 0.27469103730614464], [4.984118281693066, -0.9696277730779838], [-0.37758650818702333, 0.0569691422770249], [2.7265143184085394, -0.5462206211205124], [2.080225398624169, -0.39056722467232724], [-0.5343201315320389, 0.1066271846005163], [4.998181799506521, -0.9983705361395082], [1.2258527925574914, -0.23352241519234973]  …  [3.8884545446134777, -0.7653966268661422], [-4.821888480568665, 0.9288026209994489], [-2.812935918191326, 0.5728259406142618], [-2.0751777771966795, 0.42033527182355956], [-4.042808894830452, 0.8094651507886925], [-4.252695991970864, 0.8584524719533143], [3.555628670639477, -0.7191453119359813], [-3.8415006100629845, 0.7618297149599682], [3.051020389749012, -0.634353244844112], [-3.688363816959621, 0.7098506737893201]], [3.240621162225223e-11, 0.2163218332145144, 0.0003819353996224891, 0.013522260406208552, 0.008846882277508973, 0.3760070999891427, 0.38113864986808216, 0.005670063651855776, 0.013035142599627977, 0.20438811026675172  …  0.12834353389622938, 2.1408980194922203e-12, 5.492702620561281e-7, 1.9328683392316432e-5, 4.3287708502391844e-10, 1.0993177273309982e-10, 0.20045289034384894, 1.546327860454023e-9, 0.3190307320814428, 3.9640456068897226e-9], 997.9091587094687, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[3.348989228787098, -0.17164176702802225], [3.348989228787098, -0.179945575911446], [2.617270194184437, -0.5573404152142175], [2.617270194184437, -0.49531160100477856], [2.617270194184437, -0.5391691669907301], [2.617270194184437, -0.5459607078077138], [2.002111953689704, -0.5875038275071042], [2.002111953689704, -0.6182424371061082], [2.002111953689704, -0.6107143264615954], [-0.5129946946119357, 0.2527350427998745]  …  [2.2691656441560393, -0.6303174038269479], [2.2691656441560393, -0.5903104370465829], [2.2691656441560393, -0.5956595612922517], [3.7353752192402494, 0.14804667327737409], [3.411799608252281, -0.10863516496221293], [3.411799608252281, -0.10809307424348968], [2.9241497407801895, -0.4327353231142743], [2.9241497407801895, -0.45180833769880124], [2.9241497407801895, -0.41877139549878345], [2.9241497407801895, -0.40205306143584696]], [0.13378206970486325, 0.13378206970486325, 0.3019211822162877, 0.3019211822162877, 0.3019211822162877, 0.3019211822162877, 0.3955147036833723, 0.3955147036833723, 0.3955147036833723, 0.023282730014528137  …  0.3685022456269335, 0.3685022456269335, 0.3685022456269335, 0.07013288369941545, 0.1216794842128175, 0.1216794842128175, 0.22906019305268666, 0.22906019305268666, 0.22906019305268666, 0.22906019305268666], 2756.299650498082, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[3.314660875381494, -0.521057014260299], [2.5058021111415933, -0.5812213544409983], [2.5182078739834814, -0.5549083111006111], [2.509436360786291, -0.5951073911820365], [2.509436360786291, -0.5246487654800966], [2.5080780526228943, -0.5406985193472892], [1.884611188188283, -0.6827081911729062], [1.8784634662684823, -0.7031892193299701], [1.8799690883973847, -0.7434513964320973], [1.8799690883973847, -0.7041467950500656]  …  [2.1544264831827737, -0.6464836970731336], [2.1544264831827737, -0.6466751861728706], [2.1431021633906497, -0.698275461901483], [2.1511035567467225, -0.6481439551399218], [2.150033731897589, -0.6217619625424549], [2.150033731897589, -0.6592583531016758], [3.3900725752598384, -0.608851069166741], [2.8376026761573345, -0.48702157260674744], [2.8337880732404295, -0.5106712192396996], [2.84373912849302, -0.5413259473004982]], [0.19151576777697402, 0.3678823741766235, 0.3660212634025024, 0.3673420416431572, 0.3673420416431572, 0.36754446719956807, 0.3895262436836821, 0.38899584483624283, 0.3891270364325301, 0.3891270364325301  …  0.398418711211957, 0.398418711211957, 0.3986244461952892, 0.3984843670107769, 0.3985045709869722, 0.3985045709869722, 0.17429802511444656, 0.30463861386708535, 0.30549104582826814, 0.30326305745552395], 3253.273259941816, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[3.210449472529434, -0.38308480823246116], [2.389557840253394, -0.6178411811132856], [2.4072262117633594, -0.5922974301097864], [2.3904148825498837, -0.5812229439113956], [2.404506607690272, -0.5788385106151773], [2.3999383487534365, -0.5357420090161067], [1.7480695499537018, -0.805406127216559], [1.7378256224024882, -0.8050961858733182], [1.7312788091109652, -0.8004631948890419], [1.7391397293873716, -0.818265722798698]  …  [2.0251297437681473, -0.7031346966323537], [2.0250914459481995, -0.7112531137044406], [2.003447071010353, -0.7208822925631792], [2.021474765718738, -0.7306097935531198], [2.025681339389098, -0.6626616645503745], [2.018182061277254, -0.7189557315033271], [3.2683023614264903, -0.2688742341855803], [2.740198361635985, -0.5124165639128955], [2.7316538293924895, -0.5036742991987233], [2.7354739390329206, -0.47388965360911106]], [0.00030429452339269093, 0.004873127773952739, 0.004623429687553921, 0.0048607451601301145, 0.004661109198213831, 0.004725015155456901, 0.026630110302213805, 0.027271021453969427, 0.02768715673977484, 0.027188109154703835  …  0.013450610420975017, 0.013451951752235743, 0.014228485450143076, 0.013579136155562324, 0.01343130414657139, 0.013695817258359845, 0.00024398656645424787, 0.0016184895499444054, 0.0016649826475788458, 0.0016440484109613216], 439.2578317549789, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[2.288766725741402, -0.6498668907460969], [1.58698832451039, -0.8810240699231819], [1.5711861701331569, -0.8648263184294785], [0.7921241347298171, -1.0857666015519831], [0.7921241347298171, -1.0440528457107567], [0.7921241347298171, -1.05268757584176], [0.7640037234234761, -1.099323269388643], [0.7640037234234761, -1.0970926562226426], [0.7640037234234761, -1.1007890708236596], [0.5175051644793102, -1.0021134650757646]  …  [1.4801176330455574, -0.9548333977734528], [1.502007705413491, -0.9057322942216433], [1.3326245322261954, -1.0163036862490902], [1.3480929543070181, -1.0107814459660114], [1.334155938720609, -1.0299572399369585], [1.3590599543514856, -0.9709794493408965], [1.3383633690451324, -0.956522471737659], [1.3293946903868818, -1.0398391938535398], [1.8845028044416765, -0.7420849216799026], [1.8753528070081142, -0.7849449838561954]], [0.38544678552803713, 0.3622258165138193, 0.35967435075190907, 0.18624758644449163, 0.18624758644449163, 0.18624758644449163, 0.17982293770944605, 0.17982293770944605, 0.17982293770944605, 0.12779261855711954  …  0.34364016081205695, 0.34769095262059513, 0.3136075996939746, 0.3169534018359766, 0.313940610064065, 0.3193008659152711, 0.31485355263024145, 0.31290400961298886, 0.3949451440130833, 0.39441612254611885], 2216.3655006043755, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[2.1587933475921828, -0.6665271198007929], [1.4107835105257536, -0.9889870686524197], [1.4107835105257536, -0.9777334686858657], [1.398220906447261, -0.9833471056234074], [1.398220906447261, -0.9356039113080584], [0.5833135655876658, -1.2601701515204247], [0.5815866195614652, -1.277702406437943], [0.5441390695457475, -1.327475268085838], [0.5445851921789475, -1.3268541845985211], [0.31708247146415725, -1.2414504533560322]  …  [1.1281644907332173, -1.1691099711195687], [1.1281644907332173, -1.2078901688540056], [1.1648640644833064, -1.1059502989608667], [1.1470588746976005, -1.0815911127152793], [1.1470588746976005, -1.1246916498414135], [1.1214268516161738, -1.163175576946574], [1.736085820105696, -0.8245693187320638], [1.736085820105696, -0.8079078254680528], [1.7183638102368752, -0.8330513156570755], [1.7183638102368752, -0.8479726581547055]], [0.38209909730253916, 0.23187827325259122, 0.23187827325259122, 0.22884542060977722, 0.22884542060977722, 0.0695368097650326, 0.06931260256656982, 0.06457735653130382, 0.06463235276828756, 0.04080386105658533  …  0.16597914262369953, 0.16597914262369953, 0.17412821088979635, 0.17015443351244303, 0.17015443351244303, 0.16450096350010657, 0.30864138025166, 0.30864138025166, 0.304699604575009, 0.304699604575009], 1465.3240907161792, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[2.0254879236320242, -0.6977953800087218], [2.0254879236320242, -0.7005157028469695], [1.2129860967952697, -1.1219619799028737], [1.2129860967952697, -1.0797469993703235], [1.2152368167885803, -1.075454770150016], [1.2015514853225795, -1.1202732708825927], [1.2015514853225795, -1.0974712446418684], [1.2111001241856494, -1.0440457441664093], [1.2111001241856494, -1.0615724762544394], [0.27864401592857996, -1.632856563528903]  …  [0.888791736226859, -1.3486556241508723], [1.5711719563592832, -0.8908173241399467], [1.5711719563592832, -0.9214297375058423], [1.5745042550120854, -0.8397877541293617], [1.5745042550120854, -0.9134353018037822], [1.5517535471054602, -0.9172946485021798], [1.5517535471054602, -0.9091668098459902], [1.5517535471054602, -0.908248464210218], [1.5487692786059342, -0.9736175206416964], [1.5487692786059342, -0.9215613654584568]], [0.20150195814536342, 0.20150195814536342, 0.37440998524090174, 0.37440998524090174, 0.3741089293381827, 0.37591380902785304, 0.37591380902785304, 0.37466097765097867, 0.37466097765097867, 0.33755720326330774  …  0.3987370657784684, 0.30907711276382943, 0.30907711276382943, 0.30834042797782113, 0.30834042797782113, 0.31333594593382547, 0.31333594593382547, 0.31333594593382547, 0.31398513999466643, 0.31398513999466643], 3481.036196993054, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[1.8859288476302798, -0.7822968779931639], [1.8853847830626302, -0.7622523936962661], [0.988593700814695, -1.2460902201923023], [0.997036696921205, -1.2364507263589417], [1.000145862758577, -1.1957096662523186], [0.977496831146061, -1.2573783004834667], [0.9820572363942058, -1.2586968111976233], [1.0022909753523674, -1.2072758806161268], [0.9987856289347615, -1.2038622718788337], [-0.047927296777200645, -1.9132430008846606]  …  [0.6190606113966846, -1.5859386499730932], [1.393008491531294, -0.9968878058286748], [1.3868860088581147, -1.0292002021326931], [1.406546704186213, -0.9199723556783277], [1.391817194651329, -1.0341869125891225], [1.3682946174050241, -0.9996726533464703], [1.3699201851362621, -1.042608873130297], [1.3701038542634165, -0.9942845946385324], [1.3540457744775949, -1.0714337426561773], [1.3644570055142429, -1.0232666814869997]], [0.317958098329972, 0.31807460585676894, 0.3890848936057688, 0.38980653384965214, 0.3900656125417539, 0.3880963747735136, 0.3885081057727935, 0.390242260343592, 0.38995271045871216, 0.1803226209749214  …  0.334574101532885, 0.39248035748274296, 0.3929075008615943, 0.3914853890656599, 0.39256458659654336, 0.3941168539418396, 0.39401639717208703, 0.3940049829406965, 0.39495383004902085, 0.39434997823758944], 3249.0271728919442, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[1.729469472031647, -0.8152568259417038], [1.732934304323377, -0.8021054247226849], [0.7393756567762345, -1.474133549210174], [0.7497465516494166, -1.4729487612618446], [0.7610039295081134, -1.4208811388365825], [0.7260211710493676, -1.4628714594769536], [0.7303178741546812, -1.4199439512074625], [0.7608357992291421, -1.402694049362816], [0.7580131745589948, -1.375248200643957], [-0.4305758969541328, -2.2189394754787526]  …  [0.3018728814020659, -1.8484798915247063], [1.193630930365559, -1.1483678811765294], [1.1810459684315762, -1.1580578077660941], [1.2225522330505474, -1.0789627491616611], [1.1849798121335045, -1.1719768862059563], [1.16836008673573, -1.1135308935629094], [1.1613984105102027, -1.1864418334441638], [1.17124693533571, -1.1540828668507752], [1.1397590259463595, -1.2058469607322622], [1.159803669216843, -1.1437653598969273]], [0.024417785816103668, 0.02421848093504614, 0.15531253883894755, 0.15310750351310842, 0.15073105502089476, 0.15817371766186264, 0.15725051034264925, 0.15076641533958984, 0.15136065691445494, 0.3907555065658919  …  0.25741391972796196, 0.07506123145574511, 0.07680182444320402, 0.07116650406386768, 0.07625474937810811, 0.07858462980997907, 0.0795750698341779, 0.07817643028194583, 0.0827085278097474, 0.07980316137555776], 2082.6286691897412, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.4445489469341997, -1.7260741080192576], [0.4551567993970477, -1.7466751559412586], [0.43344687915397684, -1.7548296470721214], [0.44632908391318865, -1.6754663716457125], [0.48029698935657894, -1.637852053231137], [-0.8743637920498833, -2.4025858470855255], [-0.8743637920498833, -2.4241310078538865], [-0.9651011708041204, -2.0494043558176696], [-0.9651011708041204, -2.041410787271502], [-0.6040442119840584, -1.3696755330797377]  …  [-0.07554485872270367, -2.2089491346276686], [-0.07495819395456049, -2.135404938645798], [0.033425510198757113, -2.0918717427058144], [0.06499031639332892, -1.9926436270461902], [0.03290868280975734, -2.034140933116327], [-0.06782309690287536, -2.182593701693306], [-0.06782309690287536, -2.17979165866302], [1.006759683218215, -1.2036147621926931], [0.9456539080231481, -1.2743831237135967], [0.8985896337999071, -1.347227816431146]], [0.39121315796998407, 0.3903711612174733, 0.39204907839704045, 0.3910748059184472, 0.3882084088257382, 0.21280575393062848, 0.21280575393062848, 0.19143285745866614, 0.19143285745866614, 0.27779954357726383  …  0.3787524986467018, 0.37882405218085996, 0.38996819801765387, 0.39240727093436323, 0.38992515555959817, 0.37968491915002067, 0.37968491915002067, 0.29886795373968905, 0.3124912530357406, 0.3225837612940191], 3236.7245974019843, nothing, nothing)
 ⋮
 WeightedParticleBelief{Vector{Float64}}([[0.8017621109288064, -1.4453069084992165], [0.9490170784921133, -1.2504521581620378], [0.8279197795438535, -1.3951869561936594], [0.9714414414918718, -1.3056682221145834], [0.8957344878549641, -1.3193785111530807], [0.8462682241796675, -1.3686627285380806], [0.8312937422360535, -1.3720618005183656], [0.7618874923754121, -1.4596110622486473], [0.8571188990091044, -1.4101561907237952], [0.8277253066200302, -1.4396549012422928]  …  [0.8620360155789472, -1.326764752132009], [0.9084664799887365, -1.2956453382565682], [0.8615043859671216, -1.4029583552346567], [0.8493492582718835, -1.3783070837650977], [0.957096372555204, -1.3366247935018478], [0.7947896139856382, -1.400356998554181], [0.8492530663628485, -1.393319154969041], [0.9733759984976007, -1.2939286494656166], [0.9412554804733303, -1.3647153869641342], [0.8718752312100506, -1.342144281813565]], [0.35530805519618563, 0.37729185359164896, 0.3596866061933819, 0.3800332199559654, 0.3701099000923279, 0.36264202531860595, 0.36023734379915706, 0.3482769409193385, 0.36434345594397854, 0.3596547629336249  …  0.3651029523981656, 0.37190950821116514, 0.36502118627648744, 0.3631286811871924, 0.378299178042802, 0.3541090463462665, 0.3631135297273853, 0.3802616899874971, 0.376303531544454, 0.36660085738071657], 3822.31052319048, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.5127007292289631, -1.6519548326456448], [0.6989266468597057, -1.472666019157609], [0.5488823883051217, -1.6602353020165856], [0.7103077970689551, -1.4797216674030493], [0.631858785624348, -1.5222200834078332], [0.5725356784720514, -1.6071207651100161], [0.5568813821323804, -1.603874746091023], [0.4699652799256826, -1.7110886856647456], [0.5750876608643454, -1.6617983917905739], [0.5397943263715717, -1.6643959154864918]  …  [0.5966830651525454, -1.5716252265043549], [0.6493374123374229, -1.5163663077396197], [0.5809127149201903, -1.6093253524255142], [0.573687841518864, -1.6124089278509626], [0.6897714138548343, -1.515785856634523], [0.5147182142748019, -1.6368792000943002], [0.5705892353690403, -1.612331474996522], [0.7145902686044774, -1.5146261879106582], [0.6683124030805034, -1.5812258048400514], [0.6034463748473375, -1.559366043861805]], [0.3609178476997275, 0.38554670296108196, 0.366570254343147, 0.38667020331584867, 0.3779963194591302, 0.37005124934293426, 0.3677667911323861, 0.3537567173419003, 0.3704164021338065, 0.3651871759570418  …  0.37342348408667736, 0.3801143649659693, 0.37124217808496, 0.3702163616247041, 0.38460914770719895, 0.36124316745592544, 0.36977136385584336, 0.38708081579053405, 0.38229493556291155, 0.3743343609318614], 3881.151806970357, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.18230976269983418, -1.9430508318000959], [0.4043934430281839, -1.7326565908204634], [0.21683532790180454, -1.9448466537697506], [0.4143634635883452, -1.7520709591188197], [0.3274147689427814, -1.7614917105141237], [0.2511115254500482, -1.8847117911794238], [0.23610643291417577, -1.9086064347243092], [0.12774754279273348, -2.0260071055205224], [0.24272798250623057, -1.944614491025158], [0.2069151432742733, -1.9657309081224315]  …  [0.2823580198516744, -1.8562985660200173], [0.346064150789499, -1.7869475462761237], [0.2590476444350874, -1.9026263571592523], [0.25120605594867146, -1.8838394065509525], [0.3866142425279297, -1.7894464568953716], [0.18734237425594186, -1.9568244799158174], [0.2481229403697358, -1.9060398505018545], [0.41166503102234575, -1.7734565551464598], [0.35206724211249313, -1.8221243394875684], [0.2915731660749765, -1.8342933797417924]], [0.09722611807674743, 0.13776587660021258, 0.10297211117472664, 0.1397765224627801, 0.12277309825139174, 0.10888400515478829, 0.1062706595135085, 0.08857658836402268, 0.10741905318685993, 0.1012995695640337  …  0.11445136868014615, 0.12631708252739077, 0.11028203508214877, 0.10890059330548935, 0.13421875272426084, 0.09805055696765938, 0.10836037163969729, 0.13923081918707036, 0.1274700556016303, 0.11612534854703181], 1500.513872594859, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-0.20630040366018504, -2.301167170723354], [0.0578621248640912, -2.0227723217221554], [-0.17213400285214558, -2.2729414886153974], [0.0639492717645812, -2.0711095505158523], [-0.02488357316004336, -2.098438393961647], [-0.12583083278583657, -2.257214530353141], [-0.14561485403068608, -2.2516235795620236], [-0.277453878311371, -2.328039260601228], [-0.14619491569880105, -2.326545283488619], [-0.18623103835021304, -2.303427518104317]  …  [-0.08890169335232906, -2.2182115819458335], [-0.011325358465725799, -2.10201105427554], [-0.12147762699676312, -2.2109137636299745], [-0.12556182536151905, -2.223354820066011], [0.02872495114885537, -2.131051471385652], [-0.20402252172722163, -2.276263194410384], [-0.13308502973063513, -2.2501817634733583], [0.056973719993053784, -2.1181183419776626], [-0.012357625785020587, -2.1273608326733044], [-0.075285509873382, -2.1748148326776566]], [0.24248972998877474, 0.17991284072455554, 0.23422508871855718, 0.17853272221127192, 0.19903399088935583, 0.22305641296222506, 0.22782154077545635, 0.25967446972671876, 0.2279614320993483, 0.23763399430982748  …  0.21420237165296716, 0.19585897485342682, 0.22200969854356462, 0.22299171066680148, 0.18657245353727173, 0.2419385500436251, 0.22480217205545194, 0.18011459791357298, 0.19610018389735295, 0.2109546179517396], 1685.1695704098552, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-0.6665338378048559, -2.5971359179724884], [-0.3466923394803399, -2.344759484890311], [-0.6267223005752252, -2.5775678429048234], [-0.3502726383385893, -2.4228589353925822], [-0.4445712519523728, -2.436398958063579], [-0.5772737388564648, -2.5807152663490935], [-0.5959395699430908, -2.59765937498357], [-0.7430617304316167, -2.60630506770923], [-0.6115039723965249, -2.660640441819403], [-0.6469165419710765, -2.6569523299427393]  …  [-0.5325440097414957, -2.5545712562503717], [-0.43172756932083384, -2.449871089148708], [-0.5636603797227581, -2.530559895394514], [-0.5702327893747213, -2.5442233556037275], [-0.39748534312827505, -2.4938012176078463], [-0.6592751606092984, -2.5941635656904727], [-0.5831213824253068, -2.5845420242493775], [-0.3666499484024788, -2.4771100924944496], [-0.4378297923196815, -2.455873556384023], [-0.5102484764089134, -2.5376564963332493]], [0.3411210965552304, 0.2709957539974783, 0.3333411026428205, 0.27184863624203015, 0.29394490062675543, 0.32321036721121776, 0.32709178306599485, 0.3550060081290934, 0.330275902263477, 0.3373318515380951  …  0.3136505418013066, 0.2909849778146576, 0.3203382598207113, 0.32172913145175847, 0.2830102023052642, 0.33972932430131536, 0.32443351824627203, 0.27573924671802486, 0.2923935599902308, 0.3087607844494422], 2566.7107591446784, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-1.1859610213993537, -2.6597504358231276], [-0.815644236458402, -2.62346100307775], [-1.14223586915619, -2.711358828884648], [-0.8348444254171057, -2.701689592591096], [-0.9318510435650886, -2.696681117106675], [-1.0934167921262836, -2.7339927796980534], [-1.1154714449398049, -2.7457450209574468], [-1.2643227439734628, -2.6466683287513084], [-1.1436320607604056, -2.783319000856908], [-1.1783070079596243, -2.7965292911412227]  …  [-1.0434582609915701, -2.7208933551706775], [-0.9217017871505755, -2.6943828329221793], [-1.0697723588016608, -2.6673536399690274], [-1.079077460495467, -2.6894642402310422], [-0.8962455866498443, -2.7256095159035008], [-1.178107873747393, -2.7256824516444764], [-1.1000297872751823, -2.698951230676846], [-0.8620719669013688, -2.7536500989493233], [-0.929004503596486, -2.665336576547081], [-1.0177797756755633, -2.715776618090418]], [0.2980691835075748, 0.36926234368388294, 0.30789390521856064, 0.36641743106952035, 0.35039495727582953, 0.31852595124263194, 0.313770657412447, 0.27989753730840417, 0.307584330772424, 0.2998074445008888  …  0.328973639524205, 0.35219314278591934, 0.32352926220335704, 0.3215723862287688, 0.3565823909282339, 0.29985256936318955, 0.3171087711268179, 0.3621916379014041, 0.3509020092268167, 0.33415168666742384], 3695.7991006600296, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-1.717911108563979, -2.272962684136828], [-1.340336437073952, -2.5763529008550776], [-1.6845076349331198, -2.350718417816367], [-1.3751823439353248, -2.665770171467515], [-1.4711872669864237, -2.582409989173499], [-1.6402153480658943, -2.4329623716171427], [-1.6646204491312941, -2.426625212674036], [-1.7936564097237246, -2.125148767934091], [-1.7002958609317873, -2.4183543783183294], [-1.737612866187869, -2.384384110264556]  …  [-1.5876369320257058, -2.447519085477433], [-1.4605783537350114, -2.57859528753457], [-1.6032430867954663, -2.3768627732281598], [-1.6169703085416756, -2.3844221300949577], [-1.4413674898305446, -2.628562357883307], [-1.7232443640762882, -2.3274317059550715], [-1.6398200334105515, -2.3946341467323213], [-1.4128019866912336, -2.7000881698712607], [-1.462071818905902, -2.5213731974484217], [-1.5609350992936468, -2.47149450750537]], [0.16852616581912686, 0.09559577133712129, 0.1612056616844599, 0.10133422447421075, 0.11824454838676542, 0.15172557926010766, 0.1569160494248134, 0.1856124689354708, 0.16464810968112406, 0.17290831265038056  …  0.140833878311073, 0.11629775612474297, 0.14402402643259254, 0.14686020198708935, 0.11282134778232941, 0.1697078307714756, 0.15164218913052652, 0.1077697597928487, 0.11657065771785817, 0.13546207762898757], 918.7222530331451, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-2.1725036453913447, -1.230995935856867], [-2.1725036453913447, -1.2248142768918655], [-1.8556070172449675, -1.9890658564617891], [-2.154651318496393, -1.2872379763150419], [-1.9083363782288278, -2.0040453480396114], [-1.9083363782288278, -1.9733707796155806], [-1.9876692648211236, -1.8161932555088522], [-2.1268078223893228, -1.445329644276073], [-2.1499454916661014, -1.3891359982201816], [-2.1499454916661014, -1.388528355090614]  …  [-2.0938547345606673, -1.4379731871041073], [-1.967079961407206, -1.8984648429195325], [-2.1887307052673024, -1.2645696820423795], [-2.1887307052673024, -1.2413946703867103], [-2.118746862757016, -1.4149858185368345], [-2.118746862757016, -1.4199257365364946], [-1.9528196206654858, -1.9933581007823444], [-1.9663464583955865, -1.739356707877619], [-2.0552340007947207, -1.5876628902239722], [-2.0552340007947207, -1.5808225981447492]], [0.2117636443899304, 0.2117636443899304, 0.14097746897026708, 0.20751820762054396, 0.15190649654633492, 0.15190649654633492, 0.16907841223122352, 0.20093816989194638, 0.206402411011915, 0.206402411011915  …  0.19322602734929634, 0.16454305794517463, 0.21563825205237366, 0.21563825205237366, 0.1990436217008234, 0.1990436217008234, 0.16143321888516413, 0.16438246228502357, 0.18430845408831537, 0.18430845408831537], 1517.6592275920195, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-2.418702832562718, -0.0788704384917307], [-2.417466500769718, -0.08473594255895156], [-2.2534201885373255, -0.8438094821818518], [-2.4120989137594013, -0.11303101209807287], [-2.30914544783675, -0.7618666398843548], [-2.303010534151944, -0.76775457803514], [-2.3509079159228943, -0.5548225180644104], [-2.415873751244537, -0.206161466913668], [-2.4277726913101376, -0.16574416221926747], [-2.4276511626842243, -0.15870282749383224]  …  [-2.3814493719814886, -0.2575120800724455], [-2.3467729299911126, -0.6237781634861035], [-2.4416446416757784, -0.024238144819918928], [-2.4370096393446445, -0.0648262148795613], [-2.4017440264643826, -0.24957528262169082], [-2.4027320100643146, -0.24053385237925418], [-2.3514912408219546, -0.6910474689455952], [-2.3142177999711104, -0.5618023920025312], [-2.3727665788395154, -0.3666174293139981], [-2.3713985204236705, -0.3698852245759907]], [0.08280340431310287, 0.08298508011543258, 0.10949845910347858, 0.0837769817672132, 0.09995789367647326, 0.10098149777448426, 0.0931670185002009, 0.0832195307652077, 0.08147890906398825, 0.08149655972105806  …  0.0883969883793412, 0.09382558593638171, 0.07948136889033294, 0.08014500989909047, 0.08531914860163148, 0.08517118409960028, 0.0930743587171015, 0.09911660001813934, 0.08973619405250252, 0.08994842288326356], 1058.0740980503952, nothing, nothing)

Plotting the Results

We can now plot the observations, the smoothed estimate of the position (the mean of the belief at every time step), and the true state. Note that the estimated position is much closer to the true position than the noisy measurements.

xmat = readdlm("x.txt");

plot_data = [(; t, x1=x[1], x2=x[2], y, b_mean=mean(b)[1]) for (t, (x, y, b)) in enumerate(zip(eachslice(xmat; dims=1), ys, bs))]

plot_data |> @vlplot(x = :t, width = 700) +
             @vlplot(:line, y = :x1, color = {datum = "True State"}) +
             @vlplot(:point, y = :y, color = {datum = "Observation"}) +
             @vlplot(:line, y = :b_mean, color = {datum = "Mean Belief"})
Example block output

Appendix

Code for Generating Data

The following code can be used to generate the data in the text files:

rng = MersenneTwister(1)

k = 100
h(x, rng) = x[1] + sigma*randn(rng)

x = [3.0, 0.0]
xs = Array{typeof(x)}(undef, k)
us = zeros(k)
ys = zeros(k)
for t in 1:k
    x = f(x, us[t], rng)
    ys[t] = h(x, rng)
    xs[t] = x
end

using DelimitedFiles
writedlm("x.txt", xs)
writedlm("y.txt", ys)
writedlm("u.txt", us)