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}}([[3.2725645317466157, -0.678284555282005], [-0.2487361095173457, 0.021669158490426922], [-2.9813643686925353, 0.5900224364590501], [-3.4990941102070727, 0.702326062247605], [-0.8601195703849474, 0.16570641543230502], [4.2344519114382395, -0.9095864069206684], [2.6825398345291274, -0.5686853382176366], [4.40177969134469, -0.9036098881307759], [0.2001100155770752, -0.04232856198087373], [-2.9327932475074228, 0.5946653187270664]  …  [1.2685709690641467, -0.257553911483696], [-3.894158878377908, 0.7784922678975761], [-1.0349297663150425, 0.23223678924659216], [-2.323996906978153, 0.45221815677708604], [3.4806526426087228, -0.6829897680679804], [-0.4396650501897059, 0.07232692924280175], [-1.4621262063794993, 0.29930257761631396], [-1.4959978730105317, 0.28327700469548495], [-3.3034018783009267, 0.6708345796614111], [0.0769886923141716, 0.0008740828793287414]], [0.268437123508923, 0.012520768450927667, 2.2573569644322641e-7, 1.2284822224756644e-8, 0.002078950581191117, 0.07179114477166414, 0.38137135116266163, 0.05192817087293371, 0.03687857492808459, 2.9257150739820077e-7  …  0.21454375034519957, 1.1126637722789093e-9, 0.0011615544594339516, 6.181154577897549e-6, 0.21827001207801594, 0.007439491882690147, 0.00024626215787552246, 0.0002160700439313384, 3.809864550428899e-8, 0.027976514570281028], 1005.078587634186, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[3.1369076206902147, -0.3067004281703241], [3.1369076206902147, -0.32379895395107094], [3.1369076206902147, -0.3169411536904867], [4.052534630054105, 0.7055595091343031], [2.5688027668856, -0.5270765232427701], [2.5688027668856, -0.5413746187618103], [2.5688027668856, -0.5740433626229035], [2.5688027668856, -0.5564322889327691], [0.1447089843701587, -0.0962412374626725], [3.763504715807888, 0.1672904355144107]  …  [2.310804936414058, -0.6155197073122245], [2.310804936414058, -0.5643632924736777], [1.7378498026268276, -0.5915678268501356], [1.7378498026268276, -0.5591809383477606], [1.7378498026268276, -0.6195259388059002], [1.7378498026268276, -0.5684462217492087], [1.2170601867674076, -0.5102792682319055], [1.2170601867674076, -0.47672932169925875], [3.3440546889951266, -0.15075583361714295], [3.3440546889951266, -0.157506229160288]], [0.17897237623555534, 0.17897237623555534, 0.17897237623555534, 0.03691844781232581, 0.3126779227807612, 0.3126779227807612, 0.3126779227807612, 0.3126779227807612, 0.08994680753262445, 0.06652281733748122  …  0.36212515753014, 0.36212515753014, 0.3954349602482609, 0.3954349602482609, 0.3954349602482609, 0.3954349602482609, 0.3221977677382849, 0.3221977677382849, 0.13475986490460817, 0.13475986490460817], 2780.209229167388, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[3.07556753505615, -0.5097998477890799], [3.0721478299000005, -0.4747329801243634], [2.463387462237046, -0.5700992851054608], [2.4605278431332382, -0.5813144867224793], [2.4605278431332382, -0.5630695310515073], [2.4539940943610197, -0.592657630915024], [2.4575163090990464, -0.555767421154822], [0.39266091447392876, -0.31953053767224915], [2.7615617840851003, -0.47661317609181014], [2.7673846187995514, -0.4910968881565284]  …  [2.1979322779193224, -0.6557119181972006], [1.6195362372568005, -0.7287290774174726], [1.6195362372568005, -0.7652444165288375], [1.6260136149572755, -0.7248964574400235], [1.6139446148656476, -0.785282489433386], [1.6139446148656476, -0.7623962916893104], [1.6241605582769858, -0.7219263122361685], [1.1150043331210264, -0.7167025229937027], [1.1217143224275559, -0.6959498449320018], [3.313903522271698, -0.5631155878638499]], [0.24864969529121467, 0.249476444890069, 0.37388226695487453, 0.3742660580480443, 0.3742660580480443, 0.3751329233449612, 0.374667351354703, 0.09237756290741148, 0.32120561949293697, 0.3199711574270224  …  0.39715532994871455, 0.3549094040072577, 0.3549094040072577, 0.356015511859794, 0.35394537288574346, 0.35394537288574346, 0.35570024753136525, 0.24483246146942508, 0.2464557001388987, 0.19169151319498393], 3257.676704899543, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[2.973607565498334, -0.44894529496798263], [2.9772012338751277, -0.4427220313111013], [2.349367605215954, -0.6235188805550477], [2.3442649457887423, -0.6297131409256418], [2.3479139369229367, -0.5881863764956619], [2.335462568178015, -0.6290028770732405], [2.346362824868082, -0.6478495465663155], [0.3287548069394789, -0.4491229477574443], [2.6662391488667385, -0.5035908687642918], [2.669165241168246, -0.5242772366751611]  …  [2.066789894279882, -0.7207173728630071], [1.473790421773306, -0.8648789112179903], [1.466487353951033, -0.8592671941807475], [1.4810343234692707, -0.87090163698901], [1.4568881169789702, -0.9295368381501835], [1.4614653565277855, -0.8815949575194607], [1.479775295829752, -0.8274046364676494], [0.9716638285222858, -0.8959733369303241], [0.9825243534411555, -0.887888118189742], [3.2012804046989283, -0.3104990752651604]], [0.000725864036286479, 0.0007166522276678008, 0.0054861167617068505, 0.005568625478342376, 0.0055095114990417575, 0.005713535269598289, 0.005534571587591881, 0.2643145094045309, 0.002063114157055441, 0.002043609259216527  …  0.012057459630948508, 0.04854999947643119, 0.049281882064206174, 0.04783226384816362, 0.050256607935919385, 0.0497900165272822, 0.04795642442985975, 0.11995364792514107, 0.11794393441866209, 0.0003150389824080743], 441.2240058598107, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[2.218322317603614, -0.6639600826661773], [0.23893021738799003, -0.565229865680364], [0.23893021738799003, -0.5834184952690845], [0.23893021738799003, -0.5818340784148147], [0.23893021738799003, -0.5797911212962762], [0.23893021738799003, -0.5499241562544752], [0.23893021738799003, -0.5684974792271579], [2.2547493936904632, -0.6170860331211451], [0.8131643982414426, -1.0347603581187566], [0.8131643982414426, -1.0713019345488572]  …  [1.3068539960714687, -1.029759374000028], [1.3068539960714687, -0.9937904695689963], [1.2709807493489336, -1.0345504649613266], [1.2851463650238935, -0.982326499066384], [1.3142943685362223, -0.9835141068285836], [0.792469161136221, -1.1058527597459618], [0.792469161136221, -1.1295452694958887], [0.792469161136221, -1.1225089798716823], [0.8049467298032071, -1.076526972875433], [0.8049467298032071, -1.0705350090645445]], [0.3916634730072415, 0.08074222357222337, 0.08074222357222337, 0.08074222357222337, 0.08074222357222337, 0.08074222357222337, 0.08074222357222337, 0.38867712276818417, 0.19110545693838513, 0.19110545693838513  …  0.3079480343902999, 0.3079480343902999, 0.2999076312046641, 0.30310383095005894, 0.30959258429095965, 0.18632690834018056, 0.18632690834018056, 0.18632690834018056, 0.18920319263639454, 0.18920319263639454], 2238.8682451320446, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[2.0855303010703787, -0.7090227822077573], [2.0855303010703787, -0.7007862154413396], [0.12224651833417312, -0.731331448345168], [0.128945386137095, -0.6710499113274202], [2.131332187066234, -0.6825676281704932], [2.131332187066234, -0.6929754278336477], [0.6062123266176913, -1.237934259657049], [0.5989040113316711, -1.3111580944924999], [0.5939518673054058, -1.3165540470616597], [0.5993209265059583, -1.2745238323776242]  …  [1.1009021212714631, -1.1736919149845084], [1.1080959021576695, -1.1627232745073615], [1.0640706563566682, -1.1994325572427866], [1.0886810652106167, -1.1696889405324096], [1.0886810652106167, -1.181457055596544], [1.1175915471705056, -1.1237641546928927], [0.5712986091870287, -1.321347011712746], [0.5665601072370432, -1.3817871329508253], [0.5679673651618845, -1.3512843741792622], [0.5908397279902983, -1.3044361396223934]], [0.37296223762621233, 0.37296223762621233, 0.026409843222070774, 0.026824738110973988, 0.37888659759502263, 0.37888659759502263, 0.0725587452030023, 0.07158434687282926, 0.07092938546850354, 0.07163968236311208  …  0.1600338781668515, 0.16159333096686262, 0.1521588896876921, 0.1574003712532215, 0.1574003712532215, 0.16366210009656004, 0.06798762297087475, 0.06738350738195671, 0.06756251570093261, 0.07051996458569447], 1471.0588849512242, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[1.9437257446288272, -0.7403886424199289], [1.9437257446288272, -0.7377612374562064], [1.945373057982111, -0.7704243589757749], [1.945373057982111, -0.7264014637783476], [1.945373057982111, -0.7529329465810204], [1.9948186614321355, -0.7361100148586079], [1.9948186614321355, -0.7116007745604795], [1.9948186614321355, -0.7393370836359578], [1.9927371014995046, -0.7307021910084084], [1.9927371014995046, -0.7100584545875339]  …  [0.8577102179785122, -1.4163414003646229], [0.8661637382745615, -1.3987965247122727], [0.8755512472561973, -1.3164393036711184], [0.8241841449081109, -1.3764013480199142], [0.8547432771041348, -1.3630272620146808], [0.8547432771041348, -1.363489918627426], [0.8523896540913078, -1.3705467749045295], [0.892838716231927, -1.265408784358527], [0.2902026806468781, -1.6501491550825373], [0.3299525000658196, -1.541661546375659]], [0.22096806155482127, 0.22096806155482127, 0.22057243965797935, 0.22057243965797935, 0.22057243965797935, 0.20875770393477877, 0.20875770393477877, 0.20875770393477877, 0.20925239129645304, 0.20925239129645304  …  0.398942081958482, 0.3989244639845482, 0.3988714969366927, 0.3987312731567843, 0.3989415066507531, 0.3989415066507531, 0.3989385523875482, 0.39868203913829825, 0.33979751258469026, 0.3472616532644602], 3491.8227059055425, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[1.7956480161448414, -0.8004588495633237], [1.796173497137586, -0.8009553185550023], [1.7912881861869558, -0.8225454355003433], [1.8000927652264413, -0.8278458377538261], [1.7947864686659067, -0.7832887562044396], [1.847596658460414, -0.8067650078223025], [1.8524985065200397, -0.7708321693349904], [1.8469512447049439, -0.8139774559568651], [1.8465966632978228, -0.8035502518494557], [1.8507254105819977, -0.7959586008562285]  …  [0.5744419379055876, -1.6585807342305097], [0.586404433332107, -1.6326807034787085], [0.6122633865219735, -1.5194524265055243], [0.548903875304128, -1.6318449812331473], [0.5821378247011986, -1.5917182310143445], [0.5820452933786495, -1.573707539400291], [0.5782802991104019, -1.5997894664919339], [0.6397569593602216, -1.4549923218198515], [-0.039827150369629416, -1.9498574702395297], [0.021620190790687777, -1.822174731123039]], [0.33652125179139886, 0.3364180620811691, 0.3373750286627949, 0.3356465017982677, 0.3366903028434063, 0.326035777693837, 0.32501811329403757, 0.3261694242432963, 0.3262428132877536, 0.32538675809184703  …  0.32551027493145107, 0.3279800185357749, 0.33322002072928025, 0.3201464739224425, 0.32710237407350395, 0.32708330038959754, 0.32630578689175765, 0.33863465766576845, 0.18216678960341165, 0.1963647400666216], 3261.18295665186, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[1.6355562462321767, -0.8379632503199846], [1.6359824334265856, -0.8979531609515304], [1.6267790990868871, -0.8755658422158267], [1.6345235976756762, -0.9323240337584429], [1.6381287174250188, -0.8747727608490052], [1.6862436568959533, -0.870647084163309], [1.6983320726530415, -0.8603382682979646], [1.6841557535135707, -0.8445070801286214], [1.6858866129279315, -0.9098319560898402], [1.6915336904107519, -0.8853705274377905]  …  [0.24272579105948566, -1.9525151867355737], [0.2598682926363653, -1.961718245395708], [0.3083729012208687, -1.7765427962904825], [0.22253487905749852, -1.9017370385746721], [0.26379417849832965, -1.8948912816931887], [0.2673037854985913, -1.846432674189421], [0.2583224058120151, -1.9060517316563832], [0.3487584949962513, -1.6869001766531113], [-0.4297986444175354, -2.258977122784629], [-0.34281475543392004, -2.1231433827847312]], [0.030352633716097182, 0.030323283601162734, 0.03096219361347967, 0.030423843781892248, 0.03017582354353633, 0.0270192560144022, 0.02626995916506626, 0.02715041994417865, 0.027041649187521066, 0.026689240187310767  …  0.2715925940799592, 0.26750097618241475, 0.2558470101562226, 0.27638796513140196, 0.26656161981411164, 0.2657211932857774, 0.2678706391249834, 0.24608995284614985, 0.3906935446915205, 0.38235876682764464], 2084.899020198536, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[1.4679635961681798, -0.9155992437687777], [1.5152543374878464, -0.9155485492938721], [-0.7039749885525459, -2.2915438278446825], [-0.7039749885525459, -2.335345504564784], [-0.793161603263238, -2.423071112632548], [-0.8221800183783198, -2.0472862993743837], [-0.8221800183783198, -2.027320073436317], [-0.5787823763431976, -2.4140970004586513], [-0.5787823763431976, -2.426682549567735], [-0.5702595748126796, -2.3569588471087037]  …  [-0.15781252865743595, -2.244232751147795], [-0.15781252865743595, -2.246509749928991], [-0.1151840778403081, -2.2310807916529245], [-0.10198274933929291, -2.1812311027834026], [-0.12288794051926155, -2.258614098367421], [0.011378459665629004, -1.9739980805720447], [0.011378459665629004, -2.021241872626364], [-0.8815940689744612, -2.4951686517403187], [-0.8815940689744612, -2.509831478394389], [-0.7674434319908663, -2.360533541711722]], [0.1892602990991504, 0.17844002298778852, 0.2538864632398317, 0.2538864632398317, 0.23232075629742946, 0.22532000901125146, 0.22532000901125146, 0.2837441864173654, 0.2837441864173654, 0.2857372051222362  …  0.367596304691507, 0.367596304691507, 0.37365099624041115, 0.375407813906519, 0.3725995713828288, 0.38804417990119067, 0.38804417990119067, 0.21108222854814135, 0.21108222854814135, 0.2385389583946262], 3235.5457810493413, nothing, nothing)
 ⋮
 WeightedParticleBelief{Vector{Float64}}([[0.9116220054947012, -1.3502157531529513], [0.9921886307032818, -1.2107026718699139], [0.8575391289201137, -1.3899337770906892], [0.7705336777723559, -1.5108982460418725], [0.7237742956556196, -1.4555791268853653], [0.8586069540251481, -1.3491160387480967], [0.8106089141922073, -1.415099874636397], [0.6563964943156498, -1.5471949105273997], [0.7863099610300133, -1.5037956386728824], [0.6559340354648229, -1.5998272924101387]  …  [1.068843149161737, -1.2268115410342493], [0.9337992797291124, -1.360149568142462], [1.081413486577479, -1.175703907714028], [1.0798958087451909, -1.2209620012006326], [1.0076608797884319, -1.281006962642814], [0.972840283698171, -1.3091674281185677], [1.0025494457928188, -1.2435774270971731], [0.9715729758161122, -1.2808443015406978], [0.9868780508286612, -1.2276165947029603], [1.0724110771493978, -1.2126961419387996]], [0.3723475449276019, 0.38241598796992426, 0.3644086470659833, 0.3498368438879019, 0.3411790794915014, 0.36457406378170737, 0.3568102512550101, 0.3278162015629322, 0.35263321090427985, 0.3277211747153921  …  0.3898916461304841, 0.37533524075235924, 0.3909121458046276, 0.39079207192194154, 0.3840950230257571, 0.38019855109717204, 0.38354967679712615, 0.38004879479720527, 0.381820310910205, 0.39018729706450805], 3868.191378344457, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.6415788548641109, -1.4957335047842861], [0.750048096329299, -1.4007356027966864], [0.5795523735019759, -1.6208795079555416], [0.4683540285639814, -1.7630553216778841], [0.4326584702785465, -1.6889340553089978], [0.5887837462755288, -1.597686173923421], [0.5275889392649278, -1.6670543295131943], [0.3469575122101699, -1.828739711759046], [0.4855508332954368, -1.7280170491139968], [0.33596857698279514, -1.882382913933151]  …  [0.823480840954887, -1.379060981402935], [0.6617693661006199, -1.5944967695197236], [0.8462727050346733, -1.3537781058581642], [0.8357034085050643, -1.4097482237917156], [0.751459487259869, -1.48164170514156], [0.7110067980744574, -1.5407709804841574], [0.7538339603733841, -1.459064851518887], [0.7154041155079727, -1.4996110031057466], [0.7413547318880691, -1.4227089724707893], [0.8298718487616379, -1.4200262103441599]], [0.37918702618175526, 0.39022235325495436, 0.3710502944662929, 0.3534768903600949, 0.3471028830844764, 0.37234085248855936, 0.36329067848886193, 0.33054099766409406, 0.35642716243789746, 0.32830087550702547  …  0.3952264497150167, 0.38155713066409, 0.3963577473896153, 0.3958582958121266, 0.390337771618834, 0.3867376796197435, 0.39053026901053434, 0.3871580955603484, 0.38949508611820166, 0.39556407838335], 3920.4718707238926, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.3424321539072537, -1.7614092412197755], [0.46990097576996176, -1.669259234394489], [0.25537647191086754, -1.8986994560051804], [0.11574296422840452, -2.090531193560719], [0.0948716592167469, -2.008305373537251], [0.26924651149084455, -1.9038286701820655], [0.19417807336228887, -1.9403098245663586], [-0.018790430141639336, -2.118119036362795], [0.13994742347263744, -2.0341717094299336], [-0.04050800580383507, -2.2122018830146724]  …  [0.5476686446743, -1.6233392886276492], [0.3428700121966751, -1.8737425065668556], [0.5755170838630406, -1.5805544095077508], [0.5537537637467211, -1.662727094312], [0.455131146231557, -1.7723893031192315], [0.40285260197762596, -1.8030173607182065], [0.4620209900696067, -1.715337546875702], [0.41548191488682334, -1.7177961900239866], [0.4568129373939112, -1.6748726349358969], [0.545866606692806, -1.6259888998697465]], [0.12562238097993794, 0.1512504529056207, 0.10963396077732115, 0.08674464730776696, 0.08362065912206249, 0.11209469366179259, 0.09917756126176824, 0.06795877742650767, 0.09046461763381658, 0.0652225378537619  …  0.16804373877645692, 0.12570601623299807, 0.17424403219384352, 0.16939065575119833, 0.1481547449516209, 0.13745650834422052, 0.1495949300711252, 0.14000303422129393, 0.1485056561103246, 0.16764573274572608], 1626.133322985002, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-0.009849694336701464, -2.073977159272336], [0.13604912889106396, -1.966291196775976], [-0.12436341929016859, -2.1866882607960947], [-0.3023632744837393, -2.4598872371479388], [-0.3067894154907033, -2.3498282527774297], [-0.11151922254556856, -2.2416785617092465], [-0.19388389155098285, -2.289741855343829], [-0.4424142374141984, -2.452666884294869], [-0.2668869184133493, -2.413460741248524], [-0.48294838240676957, -2.6012455320651537]  …  [0.22300078694877012, -1.9227788026264334], [-0.031878489116695996, -2.2121237599490415], [0.2594062019614904, -1.8766922717026213], [0.22120834488432112, -1.9853918667481847], [0.10065328560771064, -2.076473348235793], [0.042249129833984655, -2.1287907360736344], [0.11895348069446626, -2.0288675914762733], [0.07192267688202597, -2.018645027754245], [0.12183841040673182, -2.0232308596056843], [0.22066882671885663, -1.933489892302375]], [0.1955143109958216, 0.16251048703248214, 0.2227034993845578, 0.2656559752595167, 0.2667158367367387, 0.2196180144069805, 0.23948538553713283, 0.2984767014479858, 0.25712968228439564, 0.3075835936644887  …  0.14408872073745216, 0.2006776846752041, 0.13670295120417783, 0.14445755315483502, 0.17029866952063258, 0.18347061296989955, 0.1662525589100453, 0.17673103516976385, 0.1656184806024117, 0.1445686640965596], 1532.7872924506005, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-0.4246451261911687, -2.3683504369360255], [-0.25720911046413125, -2.310545669247287], [-0.5617010714493875, -2.495221490387085], [-0.794340721913327, -2.7631031866852553], [-0.7767550660461893, -2.632026346285611], [-0.5598549348874179, -2.5537130962742567], [-0.6518322626197486, -2.637579028022815], [-0.9329476142731723, -2.666217962978307], [-0.7495790666630542, -2.717340482738039], [-1.0031974888198003, -2.8461240761930164]  …  [-0.1615549735765166, -2.2609411229947547], [-0.47430324110650435, -2.5604693190883094], [-0.11593225237903387, -2.1969326227105657], [-0.17587002846531585, -2.31137236725585], [-0.314641384039448, -2.3919153463893736], [-0.3835090173807423, -2.4958462742566074], [-0.2868200376007884, -2.3578411964302743], [-0.33180632866882304, -2.296755041878077], [-0.28280776151440506, -2.353541406106864], [-0.16602915174161836, -2.3063465243192183]], [0.28934512676801244, 0.2494862399731659, 0.3199221162038462, 0.36343180887069126, 0.36062678690854605, 0.31952938175042056, 0.3382896172139883, 0.38215872211752616, 0.3561178999292769, 0.38915012363232515  …  0.2263645409565901, 0.30072250161048, 0.21540861442989317, 0.22981711429539006, 0.26332852613759866, 0.2797240941262779, 0.25663537030433703, 0.2674415124244222, 0.2556679230795526, 0.2274430424625411], 2372.3657123383055, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-0.8983152135783739, -2.5584149768923448], [-0.7193182443135887, -2.566338400172947], [-1.0607453695268045, -2.6040790861815593], [-1.3469613592503782, -2.7798887511679826], [-1.3031603353033114, -2.667189874892835], [-1.0705975541422692, -2.697719654556533], [-1.1793480682243116, -2.7397453481601213], [-1.4661912068688339, -2.554219650031462], [-1.293047163210662, -2.7280567573516614], [-1.5724223040584038, -2.6453114369170674]  …  [-0.6137431981754675, -2.57022538286091], [-0.9863971049241662, -2.7891961685262405], [-0.555318776921147, -2.528055928206724], [-0.638144501916486, -2.6488863275719887], [-0.7930244533173227, -2.6827531747643856], [-0.8826782722320639, -2.7575629806082786], [-0.7583882768868433, -2.6351976959046324], [-0.7911573370444385, -2.538002498394821], [-0.7535160427357779, -2.629160088487154], [-0.627298456605462, -2.5554905908943684]], [0.3562321257557725, 0.381741973782593, 0.3254121034538221, 0.26019621893348466, 0.27068946154157913, 0.3233563736781633, 0.2995714518944803, 0.23138806073654306, 0.27309729461550625, 0.20593768299085097  …  0.3917077578305979, 0.3402859609282704, 0.395435149616347, 0.3897673414297277, 0.3724660833947858, 0.35884901039963957, 0.3770516514698729, 0.3727232493754191, 0.3776648609381843, 0.3906573567293717], 3804.5587520501995, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-1.4099982089568428, -2.491533626441111], [-1.232585924348178, -2.6187271092663615], [-1.5815611867631163, -2.3061939297965086], [-1.9029391094839747, -2.1427882593502288], [-1.8365983102818784, -2.1671393665825947], [-1.610141485053576, -2.4075223067263507], [-1.7272971378563358, -2.2951961284874516], [-1.9770351368751262, -1.79249219305746], [-1.8386585146809944, -2.150389406626271], [-2.1014845914418174, -1.69975811686505]  …  [-1.1277882747476495, -2.7078494637100348], [-1.5442363386294145, -2.6040889454371605], [-1.0609299625624917, -2.688969056849779], [-1.1679217674308837, -2.774323553641888], [-1.3295750882701998, -2.6982299565779764], [-1.4341908683537197, -2.6694347714344766], [-1.2854278160677697, -2.679639602396699], [-1.2987578367234027, -2.5408500119588493], [-1.2793480604332088, -2.649976602762342], [-1.1383965747843359, -2.7061356299497126]], [0.10728157300135636, 0.07921648730187515, 0.13960190016901394, 0.21121543663736397, 0.19555870206097756, 0.1454458139922828, 0.17060807760635868, 0.22899431276215076, 0.19603997663088304, 0.25906606207211036  …  0.06525179660747651, 0.13215940673702894, 0.05732817198667558, 0.07037367701851148, 0.09386709889221613, 0.11153893070565693, 0.08699101232313694, 0.08903064736736646, 0.08607124740227964, 0.06657866088732112], 767.311309964526, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-1.908304934245065, -1.8401381002186734], [-1.908304934245065, -1.8124686297121568], [-1.7563313462014505, -2.186270141600204], [-2.042799972722418, -1.4438118108377036], [-2.042799972722418, -1.4202927290266085], [-2.3314967613540203, -0.8498198294241797], [-2.3314967613540203, -0.8641311983188498], [-2.2700261835983975, -0.9488637451242203], [-2.2700261835983975, -0.9738007457922053], [-2.2700261835983975, -0.9503342911305537]  …  [-2.065054127716847, -1.7383708260042103], [-2.065054127716847, -1.6902015745481402], [-1.7227864781592612, -2.3739518589951922], [-1.869221079585795, -2.0872814849748806], [-1.869221079585795, -2.0810630753459147], [-1.968077822640615, -1.931711105792433], [-1.8213557365471096, -2.10471406695773], [-1.8069278391151726, -1.9904982663146078], [-1.8093433809856774, -2.1295114299952944], [-1.6796237007742785, -2.325924574422757]], [0.15189985893115066, 0.15189985893115066, 0.1215687481842089, 0.1814683252229482, 0.1814683252229482, 0.25007828199941395, 0.25007828199941395, 0.23520901308544648, 0.23520901308544648, 0.23520901308544648  …  0.1865625215037865, 0.1865625215037865, 0.11537676345181985, 0.14375981229266774, 0.14375981229266774, 0.16476164270773025, 0.13410330867605488, 0.13126285376724353, 0.13173609040921386, 0.10769270360070107], 1336.2441181574366, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-2.2763325542887998, -0.6680016848184926], [-2.2707986601874963, -0.6697632974334251], [-2.1935853745214913, -1.1146748444042018], [-2.331562334889959, -0.2816377568106656], [-2.32685851852774, -0.2934975567333873], [-2.501460727238856, 0.23422355882251567], [-2.50432300101779, 0.2053588399799693], [-2.4597989326232415, 0.1459699818437148], [-2.4647863327568387, 0.12976560899310208], [-2.4600930418245084, 0.1474657749543804]  …  [-2.4127282929176888, -0.4227672099182711], [-2.403094442626475, -0.4328778015850063], [-2.1975768499583, -1.2826656420534925], [-2.286677376580771, -0.9094780889944982], [-2.2854336946549783, -0.8517541024897888], [-2.3544200437991014, -0.6455399631082614], [-2.2422985499386554, -0.9436512806256436], [-2.205027492378094, -0.8810243102168358], [-2.2352456669847363, -1.0067723343124189], [-2.14480861565883, -1.315631940038637]], [0.10550980072027227, 0.1064647651070141, 0.12034197951981797, 0.09627427980672962, 0.09703984393823703, 0.07125662379002112, 0.07087877728339187, 0.07691860860240829, 0.07622474377541742, 0.07687756945304458  …  0.08368386200151516, 0.08511694823005833, 0.11959964884660025, 0.10373902416798489, 0.10395091830123315, 0.0926100400448415, 0.1114674633900948, 0.11822117577854975, 0.11272720562043326, 0.12962746279130055], 1161.0471454578897, 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)