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}}([[-0.9971368899215949, 0.22490555464222656], [-0.7521259720228424, 0.15068161791982038], [-1.5220575202092068, 0.32144741022863316], [-1.7675257312078052, 0.3556043720435032], [-3.3135071117087844, 0.6573624225792352], [2.6275889775716763, -0.522366029280643], [-1.0291020441576204, 0.19074418872005428], [3.759261840323843, -0.7631376364543668], [-1.6813872479287073, 0.3264080440335622], [-1.242268445129927, 0.2440026596459342]  …  [-2.6233301702930376, 0.5369712889361744], [-1.8218862163663396, 0.3401725627538903], [0.24932391285703126, -0.047116704672120986], [-2.072568714372567, 0.4211938062535235], [4.28525411315081, -0.8756028861538607], [-0.7736673400153982, 0.1933492214856735], [-1.1234623821003609, 0.24515257453191], [2.9381671825233067, -0.5728158500858702], [-1.2020613196712637, 0.2495409302257381], [2.8021339872466333, -0.5430518964372775]], [0.0013207444799857408, 0.002933519363533438, 0.00019523295855500365, 7.26495336491139e-5, 3.596950003877884e-8, 0.38712860084497086, 0.0011848988390042092, 0.15461481048507963, 0.00010348275832314561, 0.0005597420046633626  …  1.4447335932188014e-6, 5.789191182907408e-5, 0.041010107061018854, 1.9554723433469193e-5, 0.06526018885817485, 0.0027413457359567934, 0.000854956542371645, 0.34185132633098136, 0.0006470379848283039, 0.36530285281703073], 988.0773261210024, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[2.5231157717155477, -0.564406708014632], [2.5231157717155477, -0.5458007817326955], [2.5231157717155477, -0.5514778798448455], [2.5231157717155477, -0.5454497691545878], [3.6066343130329694, 0.11159606379700693], [3.655523280339952, 0.1815454441101727], [3.655523280339952, 0.12398682274460693], [1.2858615431849287, -0.5487598282697362], [1.2858615431849287, -0.50444265875072], [2.482498643588909, -0.5809663234344329]  …  [2.6375270721395387, -0.5044853989698824], [0.23990057192260705, -0.11836816040473291], [2.8236040125061326, -0.4480369996692768], [2.8236040125061326, -0.4705994197393921], [2.8236040125061326, -0.475468301375539], [2.8236040125061326, -0.4612877026258255], [2.693523607959178, -0.5222257966099194], [2.693523607959178, -0.5209290221842111], [2.693523607959178, -0.5047213292756165], [2.693523607959178, -0.5218504480239631]], [0.3224739059042439, 0.3224739059042439, 0.3224739059042439, 0.3224739059042439, 0.0884253767204265, 0.08113366993019368, 0.08113366993019368, 0.336222470636242, 0.336222470636242, 0.3308598318944133  …  0.2973287814700194, 0.10552962862167097, 0.25336865327880503, 0.25336865327880503, 0.25336865327880503, 0.25336865327880503, 0.28438630076505744, 0.28438630076505744, 0.28438630076505744, 0.28438630076505744], 2771.635870087268, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[2.410234430112621, -0.6120127183473986], [2.410234430112621, -0.5820339037489666], [2.4139556153690087, -0.6149586339095646], [2.4128201957465785, -0.586210739506736], [2.4140258178846303, -0.6107978165189223], [3.691832369161987, -0.897018233472473], [1.1761095775309816, -0.7238301519188104], [1.1849730114347847, -0.7107932117606384], [2.3663053789020223, -0.6122351547384939], [2.3663053789020223, -0.5786566318328138]  …  [2.5341079454538877, -0.5521343900670762], [2.536629992345562, -0.5361620446225366], [0.21622693984166047, -0.20173591462611057], [2.733996612572277, -0.5221860917079657], [2.728510352231025, -0.5110321252974206], [2.7313464719809675, -0.5189167465277645], [2.5890784486371943, -0.5432625341717895], [2.589337803522336, -0.5255326760132111], [2.5925793421040546, -0.5666523759567983], [2.5891535183543852, -0.5230407431095884]], [0.3805717054007895, 0.3805717054007895, 0.3801344738908441, 0.3802683882704662, 0.3801261794830385, 0.1129453178646362, 0.25958644075264864, 0.26171796975723594, 0.38536790340190913, 0.38536790340190913  …  0.36356787346314984, 0.3631717973160307, 0.06725736302881319, 0.3269639916835012, 0.32809258464406404, 0.3275099042894146, 0.354520663005495, 0.3544759768795436, 0.3539159355192207, 0.3545077305747596], 3236.940544976451, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[2.2878318864431413, -0.5951766711652657], [2.293827649362828, -0.6057074497431205], [2.290963888587096, -0.6077082530343763], [2.2955780478452312, -0.6049611980815304], [2.291866254580846, -0.6521421368440643], [3.5124287224674924, 0.18158487649376553], [1.0313435471472194, -0.8861203381754232], [1.042814369082657, -0.890916098336085], [2.2438583479543235, -0.6552415474657873], [2.2505740525354594, -0.6179686864710147]  …  [2.4236810674404725, -0.5637931090926568], [2.429397583421055, -0.5704775666603188], [0.17587975691643837, -0.29769438674724774], [2.629559394230684, -0.5171222372455849], [2.6263039271715405, -0.5154633835207034], [2.6275631226754146, -0.5218177334650368], [2.4804259418028365, -0.576075473191174], [2.4842312683196934, -0.5635746958503499], [2.479248866912695, -0.5725980357296383], [2.4845453697324675, -0.5852361071029468]], [0.006556808816035891, 0.00644496625449121, 0.006498174831248975, 0.006412632777813225, 0.006481367407576485, 9.258950568127754e-5, 0.10915874960955425, 0.10715428244510844, 0.007430445570937291, 0.0072907633495574746  …  0.004401166017718571, 0.004326203595885269, 0.30011670133853013, 0.002322316105098382, 0.002346685429583214, 0.002337232275526859, 0.0037057791753483064, 0.003662864820318995, 0.0037191442072851385, 0.0036593424743452717], 461.90172518315546, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.8541194795121347, -1.0703962412556887], [0.8541194795121347, -1.1098103600753506], [0.8541194795121347, -1.0638641348170508], [0.86463114941544, -1.0834680528196425], [0.86463114941544, -1.1045355115122275], [2.1140895929262675, -0.6838823609914826], [0.785005106464221, -1.0743953453354362], [0.785005106464221, -1.0522720939091252], [0.8117157802671198, -1.009386557171787], [0.8117157802671198, -1.0094132189835971]  …  [0.5052535172598022, -0.845647716701697], [0.5052535172598022, -0.8832125026775394], [2.3109224456219413, -0.6055804902531106], [0.11634087956698883, -0.34445220777250596], [0.11634087956698883, -0.36834930905104035], [0.11634087956698883, -0.39438316252132294], [0.11634087956698883, -0.38290712551227185], [0.11634087956698883, -0.3823114621943913], [0.11634087956698883, -0.3887736965522153], [2.367498148311878, -0.6517307171314856]], [0.20067277961448324, 0.20067277961448324, 0.20067277961448324, 0.20314970693198509, 0.20314970693198509, 0.3974120028184483, 0.1846135257655321, 0.1846135257655321, 0.19076967627230768, 0.19076967627230768  …  0.12544244797594706, 0.12544244797594706, 0.3831188160251583, 0.06436826424669062, 0.06436826424669062, 0.06436826424669062, 0.06436826424669062, 0.06436826424669062, 0.06436826424669062, 0.37639859833698674], 2186.293076608545, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.640040231260997, -1.2797835865602065], [0.6413466525487246, -1.3050801963021725], [0.6479375388515114, -1.3021075704762477], [0.6437240471129945, -1.3217715595222963], [1.977313120727971, -0.6852161361430456], [1.977313120727971, -0.7484119803512935], [0.5701260373971337, -1.315451731626332], [0.5745506876823959, -1.2665782092700708], [0.6098384688327624, -1.2149973049753737], [0.6098331364704004, -1.212610511061274]  …  [0.49353944432266267, -1.2359800240296868], [0.48696678483839567, -1.2279374164619585], [0.32959614699250894, -1.1105286389695852], [0.33612397391946275, -1.0483028058710901], [2.189806347571319, -0.6021074532390712], [2.189806347571319, -0.6759077209599728], [0.042671017756780755, -0.4507197403573684], [0.03987858712811056, -0.45315236163994976], [2.2371520048855813, -0.7004937834965543], [2.2371520048855813, -0.6562959453586321]], [0.07719085279445011, 0.07737378031524522, 0.07830124197923796, 0.0777074401655212, 0.35634787979336935, 0.35634787979336935, 0.06783777020816305, 0.06840447557827217, 0.07304567283757456, 0.07304495510577531  …  0.058558161190869214, 0.057807768932827304, 0.041905645185184755, 0.04248951334270765, 0.385410244266473, 0.385410244266473, 0.021870489793699613, 0.021723725491861294, 0.3897968606231739, 0.3897968606231739], 1433.9841618168766, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.38033061328829004, -1.5465788443685686], [0.38751602475626185, -1.5295495421120515], [1.840269893499362, -0.7627413742571818], [1.840269893499362, -0.7762750873784602], [1.840269893499362, -0.7383777472926315], [1.8276307246577124, -0.7836965691706546], [1.8276307246577124, -0.7931109165531672], [0.3070356910718673, -1.6180723387138896], [0.36683900783768764, -1.4794496783006155], [0.6629055871534206, -1.4540825099079302]  …  [2.069384856923505, -0.6434802871731287], [2.054624803379325, -0.762501452182614], [2.054624803379325, -0.7185988883276486], [2.054624803379325, -0.7316921684993081], [2.0970532481862705, -0.7370261564767268], [2.0970532481862705, -0.7076580285266835], [2.0970532481862705, -0.7088779594999561], [2.105892815813855, -0.6953657306774738], [2.105892815813855, -0.6778156763566036], [2.105892815813855, -0.7288013420399994]], [0.35614813455255045, 0.3573600938071496, 0.24594906611683776, 0.24594906611683776, 0.24594906611683776, 0.24900573693136646, 0.24900573693136646, 0.34300474626479266, 0.3538342490291548, 0.39151981539966246  …  0.19124015324958077, 0.1946727921187343, 0.1946727921187343, 0.1946727921187343, 0.18485921404048505, 0.18485921404048505, 0.18485921404048505, 0.1828363312520825, 0.1828363312520825, 0.1828363312520825], 3481.1275836411364, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.07101484441457628, -1.818815240123871], [0.08160611633385151, -1.8591886083146565], [1.6877216186479256, -0.8422069865881492], [1.6850148760236698, -0.8524668367377874], [1.6925943440408358, -0.8491721775611653], [1.6708914108235815, -0.8422183827557782], [1.669008541347079, -0.8971008580878991], [-0.016578776670910622, -1.9009039510009413], [0.07094907217756452, -1.7742531724651305], [0.3720890851718346, -1.7167802746990577]  …  [1.9406887994888793, -0.7290474546518063], [1.9021245129428022, -0.8114416454365156], [1.9109050257137952, -0.7423394829174039], [1.9082863696794632, -0.7498303993503238], [1.9496480168909252, -0.7407564887254205], [1.9555216424809339, -0.756709028863326], [1.9552776562862793, -0.759416601453587], [1.9668196696783602, -0.7310902479779531], [1.9703296805425343, -0.7366043794191002], [1.960132547405855, -0.7664117658710098]], [0.20800590587753967, 0.21052362967019222, 0.35630865969545655, 0.3567661747935568, 0.3554799468550022, 0.35912029790618194, 0.35942989276624615, 0.1874968512394325, 0.20799029226313012, 0.28029736628497137  …  0.30598364366434017, 0.3144667568250454, 0.31255569802770067, 0.31312695033470916, 0.30398112125703486, 0.3026622090414447, 0.30271708951914644, 0.3001122124489319, 0.2993165969326973, 0.30162355833869386], 3230.0894873193706, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-0.29274820361019793, -2.1032793217785253], [-0.2902316053290798, -2.161010365393951], [1.5192802213302956, -0.9464442401726588], [1.5145215086761123, -0.9286229926188109], [1.5227599085286028, -0.9364971208298378], [1.5024477342724258, -0.9283067548411573], [1.4895883697294992, -0.9667150799270714], [-0.3967595668710989, -2.217120959694859], [-0.2839015623154616, -2.048727817510522], [0.028733030232023005, -2.004074319907306]  …  [1.794879308558518, -0.8019981393592321], [1.739836183855499, -0.8521175911025674], [1.7624371291303145, -0.8383798859405713], [1.7583202898093984, -0.7908359606521802], [1.801496719145841, -0.8357567488981053], [1.8041798367082686, -0.7922095939116977], [1.8033943359955618, -0.8230933211969661], [1.8206016200827695, -0.7782087800142704], [1.8230088046587143, -0.7730574213964384], [1.806850194231653, -0.8264881189758417]], [0.37634886785341204, 0.37602440525233266, 0.039253607509893165, 0.03965749261300064, 0.038960324316405476, 0.04069683743912513, 0.0418270744470229, 0.38785203821131214, 0.3751990093999025, 0.3202384620321094  …  0.020875250644835072, 0.023825451765902037, 0.02257500186870446, 0.02279865047501776, 0.020541927888148463, 0.02040804440373292, 0.020447164343155744, 0.019604350538137853, 0.01948878965950818, 0.02027551908839197], 2112.8861509665117, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-0.713404067965903, -2.3459632640560666], [-0.713404067965903, -2.353266912228775], [-0.7224336784078701, -2.411081466103659], [1.3299913732957638, -1.0346585765030387], [-0.8401837588100708, -2.462133300384134], [-0.8401837588100708, -2.4182515373645117], [-0.693647125817566, -2.303649347194555], [-0.693647125817566, -2.3037853794823215], [-0.37208183374943826, -2.3032672065957605], [-0.3722539200414905, -2.325531679920727]  …  [-0.5583576939878396, -2.426659241106373], [-0.5583576939878396, -2.3950324204080626], [-0.5333694481974897, -2.461456106524846], [-0.55683994998541, -2.4129094352773217], [-0.55683994998541, -2.4164338039971445], [-0.8078797951899361, -2.241034344495841], [-0.8078797951899361, -2.2058220062973404], [-0.8349085976959387, -1.890712749798059], [-0.8349085976959387, -1.8974946784374995], [1.6649598640799155, -0.9075643989689034]], [0.2516095170660419, 0.2516095170660419, 0.24942739122464616, 0.22187187308865863, 0.2209894641018711, 0.2209894641018711, 0.25637794267932273, 0.25637794267932273, 0.3294242597532012, 0.32938917610101287  …  0.2885087833514495, 0.2885087833514495, 0.2942798853015449, 0.28886120433302104, 0.28886120433302104, 0.2287672707497255, 0.2287672707497255, 0.2222570981033765, 0.2222570981033765, 0.14593233747147222], 3195.8855302590687, nothing, nothing)
 ⋮
 WeightedParticleBelief{Vector{Float64}}([[0.7029646809414798, -1.5295650421810847], [0.6509212730436472, -1.5946971894060606], [0.6769020231022359, -1.542877189681584], [0.8318046364437944, -1.3391919554633458], [0.7962493302561646, -1.4617534409171755], [0.6334679919916959, -1.5742280949233547], [0.978530779158325, -1.2193396530481764], [0.9715322883309085, -1.2943064583456039], [0.8712320139101624, -1.4104570069453242], [0.7676007027130907, -1.449010211871226]  …  [0.7469424004910896, -1.4328984078643134], [0.9383411375957031, -1.3425749092709924], [0.8301002937599473, -1.4446451238609257], [0.8095523155331004, -1.41686908724742], [0.8529671297780158, -1.4086246231072643], [0.8374992301330687, -1.378592621675674], [0.8647594644238992, -1.318647716403109], [0.745425544647432, -1.5173566707189072], [0.752446330684066, -1.5126134758591678], [0.822450401344393, -1.425201348676929]], [0.3371581555036427, 0.32668842625980177, 0.3319861490422075, 0.36032045382733524, 0.3543611534450898, 0.3230546188851067, 0.3808641781246402, 0.3800439776689989, 0.36650383157391, 0.3493098358645748  …  0.34553608521594675, 0.37592725477448763, 0.36004290735181393, 0.3566319740392451, 0.36369655814795016, 0.3612417415393494, 0.3655204936088226, 0.3452547993486921, 0.34655196446192427, 0.3587869416199376], 3943.0050478135195, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.39705167250526285, -1.7837740077259816], [0.33198183516243507, -1.8954533461459526], [0.3683265851659191, -1.8142135834489144], [0.5639662453511253, -1.5440647099892257], [0.5038986420727295, -1.6957797301204347], [0.31862237300702495, -1.868975846544556], [0.7346628485486897, -1.4425076161131414], [0.7126709966617877, -1.4928307794866043], [0.5891406125210975, -1.6402789099903667], [0.47779866033884544, -1.7287526964745918]  …  [0.46036271891822694, -1.676888075580049], [0.6698261557415046, -1.5557587131774793], [0.5411712689877621, -1.679820409414168], [0.5261784980836164, -1.6691836508153122], [0.5712422051565629, -1.6490258330630352], [0.5617807057979338, -1.6410180391758473], [0.6010299211432774, -1.545889048517342], [0.4419542105036506, -1.757418723428179], [0.44992363551223236, -1.7977932722850607], [0.5374101316090072, -1.6533423200126425]], [0.3404268164169656, 0.3274821504067137, 0.3348252138880655, 0.3688101407556303, 0.35948481568055957, 0.32471585410866405, 0.38891616054058264, 0.38689761543506235, 0.37239019576613175, 0.3551071685609044  …  0.35207879376807816, 0.38246349994603596, 0.36539833167816715, 0.3630686708557934, 0.36986538912987754, 0.3684899456320834, 0.3740106244531886, 0.34879442692920176, 0.35022708774286515, 0.36482020976406426], 3963.203821549786, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.04029687096006651, -2.04639506020566], [-0.04710883406675548, -2.2082892911185317], [0.005483868476136189, -2.1380289797300747], [0.2551533033532801, -1.7910912245743065], [0.16474269604864256, -1.974000389539841], [-0.05517279630188626, -2.1994083720639503], [0.4461613253260614, -1.6857745571749014], [0.4141048407644668, -1.739628851509223], [0.2610848305230241, -1.9056555169269518], [0.13204812104392705, -2.0235449316704086]  …  [0.12498510380221711, -1.9738391037587366], [0.3586744131060087, -1.8632600621709323], [0.20520718710492852, -1.9701813425311274], [0.1923417679205539, -1.9747015820793408], [0.24143703854395582, -1.9469321470920653], [0.23357709796276432, -1.9475700605320834], [0.29185211143980894, -1.8054821936180778], [0.09047046581801477, -2.094112178057228], [0.09036498105522017, -2.1105017656375002], [0.20674166760647866, -1.90661543966295]], [0.07581719322117984, 0.06440690544424747, 0.07111351588380797, 0.10959464009206021, 0.09438350949284877, 0.06342055945489213, 0.14629013124847753, 0.13972417278318086, 0.11064266874311261, 0.08923911334669032  …  0.08815275799684974, 0.12874585600603952, 0.10101335897771699, 0.09887399635781921, 0.10719456032504908, 0.10583401287680501, 0.11617624659732005, 0.08297178196590578, 0.08295627235049609, 0.10127047591280319], 1964.9812415103133, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-0.36898214108106553, -2.3372788572699243], [-0.4887666922904618, -2.5436732869363077], [-0.42212192746987875, -2.488304393142827], [-0.10306494156158119, -2.1481683811654233], [-0.23005738185932567, -2.2899469951039606], [-0.49505447071467634, -2.564308075592302], [0.10900641389108107, -1.9703084349856805], [0.06617907046262217, -2.0427559684063947], [-0.12004627286236624, -2.2338539712148826], [-0.2726608652901547, -2.387910060184348]  …  [-0.2697827169495302, -2.2934755072180786], [-0.01397759932817777, -2.1831093842432923], [-0.188829081401297, -2.341208268081634], [-0.20259854849531428, -2.3200645092615235], [-0.14794939087445724, -2.2686445310800307], [-0.15593691414365235, -2.2891123892074625], [-0.06924432728380664, -2.162648833437244], [-0.32835196979343084, -2.439963830928712], [-0.3317353720722799, -2.438720694203398], [-0.17458142032611135, -2.2526533226372853]], [0.28147946157057113, 0.3088717615822422, 0.293837971821738, 0.21759086809719758, 0.24823681344025503, 0.31025812672699354, 0.16844674604195287, 0.17802817028189832, 0.22166569050210938, 0.25852066369212784  …  0.2578274353973498, 0.19647889870683327, 0.23826247153444285, 0.24159399051310623, 0.2283846077012277, 0.23031217106967838, 0.20951703986608247, 0.2718637013173451, 0.2726689207003424, 0.23481673600249878], 1189.1088738279266, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-0.8364379125350504, -2.5906961022444963], [-0.9975013496777234, -2.7435887614244896], [-0.9197828060984442, -2.741142847844274], [-0.5326986177946659, -2.470935515434443], [-0.6880467808801178, -2.5683268894572513], [-1.0079160858331369, -2.77092427075556], [-0.28505527310605505, -2.3103466543598508], [-0.3423721232186568, -2.344384344873678], [-0.5668170671053427, -2.573534197251896], [-0.7502428773270243, -2.6798168408446528]  …  [-0.728477818393146, -2.5820214934569083], [-0.45059947617683627, -2.538819900012391], [-0.6570707350176239, -2.6400987970305945], [-0.666611450347619, -2.6633903718156384], [-0.6016782970904634, -2.61857219851108], [-0.613759391985145, -2.589301607448087], [-0.5017740939712554, -2.46893462139448], [-0.8163447359791733, -2.7296692489085568], [-0.8194795109129595, -2.7004548735043636], [-0.6251120848535685, -2.5634496999562426]], [0.36977070940687246, 0.388649947185066, 0.3806535162236381, 0.3136841743088061, 0.3451727084608728, 0.3895553777468218, 0.2562099050609178, 0.26996556642139874, 0.32100726534905466, 0.3562304956851165  …  0.3524762364695869, 0.2953277142232052, 0.33930423581860836, 0.34113591156187034, 0.3282714613723321, 0.33073322710520775, 0.306882265924594, 0.36681252994724955, 0.3672822406898468, 0.33301908834240285], 1887.9970066187152, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-1.3545771329839496, -2.5118494514626413], [-1.5462191019626212, -2.5775186229584763], [-1.468011375667299, -2.6356947588770807], [-1.0268857208815545, -2.6610641177629804], [-1.201712158771568, -2.619983677263513], [-1.562100939984249, -2.5876108015360737], [-0.7471246039780253, -2.6024667261661185], [-0.8112489921933923, -2.574312134452141], [-1.0815239065557218, -2.7709418104653807], [-1.2862062454959549, -2.700114573833031]  …  [-1.2448821170845277, -2.633908300168638], [-0.9583634561793145, -2.7491442628569214], [-1.1850904944237428, -2.728851200378011], [-1.1992895247107467, -2.724602891245064], [-1.1253927367926795, -2.787066163538926], [-1.1316197134747623, -2.7676653400616846], [-0.9955610182501513, -2.6797719461968543], [-1.3622785857608846, -2.7221821502488184], [-1.3595704856138322, -2.651422623508971], [-1.137802024844817, -2.714287340595913]], [0.25836311032318304, 0.21216490695788928, 0.23094849872471993, 0.3323312938202357, 0.29446939054286697, 0.20838552611449468, 0.3784571638457148, 0.36989751139280164, 0.3210552480230621, 0.27472224122126604  …  0.28446254283336536, 0.3455728542094898, 0.298267255737143, 0.295024984435047, 0.3116052707825081, 0.3102382337010123, 0.33851763062071194, 0.2565073818188961, 0.2571601352157038, 0.308875086813302], 3904.200431988148, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-1.856947023276478, -1.9273880721665537], [-2.0617228265543166, -1.6902003630886813], [-1.9951503274427151, -1.8122660122244336], [-1.5590985444341507, -2.457223285433039], [-1.7257088942242707, -2.1921145107023805], [-2.079623100291464, -1.6962279031270868], [-1.2676179492112492, -2.6575535826740464], [-1.3261114190838206, -2.5454967274584352], [-1.6357122686487982, -2.482247444425976], [-1.826229160262561, -2.171384964087947]  …  [-1.7716637771182553, -2.195039991102252], [-1.5081923087506988, -2.60069344419674], [-1.7308607344993452, -2.352090992066572], [-1.7442101029597596, -2.300002796744775], [-1.6828059695004647, -2.4262270762731397], [-1.6851527814870992, -2.457023519107141], [-1.5315154074895223, -2.4599897746185233], [-1.9067150158106483, -2.094618149171512], [-1.8898550103156264, -2.034140941227666], [-1.6806594929639997, -2.346934195647769]], [0.2003272507567263, 0.2494715285581267, 0.23336895092758567, 0.13509668606858813, 0.17025504497902244, 0.2537957455435055, 0.08431526232553635, 0.09331508330224542, 0.1507769850363442, 0.19314184675682117  …  0.18058706367850624, 0.12518349105063103, 0.17140128179427075, 0.1743858899473777, 0.16083656270296554, 0.16134569610469168, 0.12967334140015072, 0.21211527627948693, 0.20810382710119188, 0.16037152492074605], 467.1666770441299, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-2.2424246377097887, -0.7775110572830384], [-2.2424246377097887, -0.812202568900189], [-2.2424246377097887, -0.8140562176313813], [-2.2424246377097887, -0.8262443326340723], [-2.3997628991720528, -0.35762752304575907], [-2.3997628991720528, -0.413096728245117], [-2.3997628991720528, -0.409833389381844], [-2.3997628991720528, -0.421019957410556], [-2.3997628991720528, -0.41447740466484717], [-2.3997628991720528, -0.4157064716313754]  …  [-2.3256386456449505, -0.840871804369288], [-2.3256386456449505, -0.8334235311162361], [-2.2966831985611598, -0.8284077317167569], [-2.2966831985611598, -0.8456206065119949], [-2.2966831985611598, -0.8349308921828675], [-2.2966831985611598, -0.8468425817446081], [-2.2966831985611598, -0.8377107624682836], [-2.1500463320935537, -1.319293659125064], [-2.1500463320935537, -1.3253951901959233], [-2.1500463320935537, -1.3277534720306532]], [0.22854212807494426, 0.22854212807494426, 0.22854212807494426, 0.22854212807494426, 0.2665126709630724, 0.2665126709630724, 0.2665126709630724, 0.2665126709630724, 0.2665126709630724, 0.2665126709630724  …  0.24866213329054293, 0.24866213329054293, 0.24165748158891895, 0.24165748158891895, 0.24165748158891895, 0.24165748158891895, 0.24165748158891895, 0.20642630615296234, 0.20642630615296234, 0.20642630615296234], 983.7953853700276, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-2.3979268491663963, 0.1679999581509377], [-2.3979268491663963, 0.17916309478897274], [-2.4048651514898265, 0.14378556162808698], [-2.4048651514898265, 0.1536757591391381], [-2.4048651514898265, 0.11728221427266816], [-2.405235881236065, 0.17557656383685927], [-2.405235881236065, 0.12420363380513244], [-2.407673504236603, 0.17406461728562195], [-2.407673504236603, 0.1627770959363144], [-2.4712884037812044, 0.42453593592464256]  …  [-2.4660517149100816, 0.17440084955674817], [-2.4642253510548167, 0.20016089458117525], [-2.4642253510548167, 0.21949525663715885], [-2.4642253510548167, 0.16554023267872253], [-2.4139050639185666, -0.11504184597075096], [-2.4139050639185666, -0.12117028132125651], [-2.4151253701327384, -0.12196746970769934], [-2.4151253701327384, -0.11632014843130234], [-2.415597026499684, -0.15530324037836984], [-2.415597026499684, -0.10060670363689561]], [0.08589245625324178, 0.08589245625324178, 0.08485230810767218, 0.08485230810767218, 0.08485230810767218, 0.08479697153203514, 0.08479697153203514, 0.08443373101171317, 0.08443373101171317, 0.07532672946437571  …  0.07604939618443719, 0.07630257080619264, 0.07630257080619264, 0.07630257080619264, 0.08350994174675486, 0.08350994174675486, 0.08332984710562608, 0.08332984710562608, 0.0832603102347691, 0.0832603102347691], 1309.6718942135572, 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)