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}}([[-1.7890708999240679, 0.36140500243077345], [3.4292250034547687, -0.6704594436882321], [-1.5323580047900076, 0.2878754912845604], [3.9985327073469024, -0.7907736976763087], [-3.9522914999997685, 0.7921323802775904], [-2.452448919456942, 0.4497577513418651], [-0.09856845116030022, -0.007521949542157763], [-0.0688965291488497, -0.006683587905525335], [-0.645693883325954, 0.13833862682920225], [-2.7283235738922693, 0.5275644792482843]  …  [0.45776300386773383, -0.09070498123665396], [-3.6587380177190965, 0.7331886136090521], [3.661350366189484, -0.7363737850120313], [0.6557789524285063, -0.1176798273442782], [-4.590006935872165, 0.9447492409424758], [3.0349019333036074, -0.6411752439396109], [2.9329418507914182, -0.5759442287141611], [-4.992083943196205, 1.0139071164903746], [-4.786707885087351, 0.9776757917492775], [3.780735844093729, -0.7207587199233134]], [6.642039151984545e-5, 0.23064765218381866, 0.0001875269731458142, 0.10807957542516541, 7.712016828661347e-10, 3.3491597145226247e-6, 0.018379290633907915, 0.019774619915257596, 0.0040720748982539835, 8.494594337418433e-7  …  0.06259658493526554, 4.743035769024065e-9, 0.17608284053972134, 0.08985646706777839, 1.1077117092113702e-11, 0.32244569300157633, 0.34284085542190934, 6.191582429665322e-13, 2.7567640770832814e-12, 0.15007565610079643], 1013.9303538959592, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[3.2951331147171223, -0.1990326323315063], [3.2951331147171223, -0.17479742095505663], [3.2951331147171223, -0.21317554087917107], [3.8403779678116408, 0.31477708381696323], [1.0918080643191643, -0.4243094557146108], [1.0918080643191643, -0.42655105622591727], [3.3588688414131784, -0.041765854297847405], [3.3588688414131784, -0.0777044840471709], [2.1043895194778828, -0.6063798195514516], [2.1043895194778828, -0.5788267856615569]  …  [3.5140756091870777, 0.012585797537769783], [0.6322429869596506, -0.2692243587818781], [2.906666884515685, -0.39652742301694915], [2.906666884515685, -0.4223601962134833], [2.906666884515685, -0.41818466653543407], [2.817753005048586, -0.4707116192650161], [2.817753005048586, -0.44398527871140203], [2.817753005048586, -0.493234014694759], [2.817753005048586, -0.4465629083007009], [3.6365841001090664, 0.060288074941616365]], [0.14465830419753703, 0.14465830419753703, 0.14465830419753703, 0.05734511032034265, 0.29454925009698985, 0.29454925009698985, 0.13183601451763005, 0.13183601451763005, 0.3882003645190957, 0.3882003645190957  …  0.10339402804601873, 0.18528116477563378, 0.2332821204306526, 0.2332821204306526, 0.2332821204306526, 0.25478081593100904, 0.25478081593100904, 0.25478081593100904, 0.25478081593100904, 0.0839079993264113], 2770.838817475994, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[3.260173630526111, -0.555742209242001], [1.0069461731762421, -0.6207463044106569], [1.0064978530739808, -0.6315722470936826], [3.3505156705536088, -0.6367189361030763], [1.9831135555675925, -0.6989177846434513], [1.9831135555675925, -0.6662326740345788], [1.9886241623455714, -0.6598754332652804], [1.9821278162512659, -0.687684045127682], [1.9821278162512659, -0.6881807723496296], [1.9827487724855288, -0.6631471712383046]  …  [2.4870643663647662, -0.5791050054830285], [0.40238204982265197, -0.28620830701241956], [0.578398115203275, -0.4128175818212291], [2.827361399912295, -0.49647924391774684], [2.8221948452729886, -0.49189884636024894], [2.823029951208598, -0.5037588958819165], [2.7289559493063056, -0.5147278448191369], [2.7191062021096344, -0.5031074929246738], [2.728440423388446, -0.4991217891648373], [3.6486417150973898, -0.7952805654172688]], [0.20428092557199792, 0.21875573541079799, 0.2186482296524561, 0.18325705764163971, 0.3960771959940515, 0.3960771959940515, 0.3963333185082801, 0.3960301301297958, 0.3960301301297958, 0.39605982298635084  …  0.3706032142481431, 0.09392203578066086, 0.12475370546094923, 0.306922482587243, 0.30806888410748917, 0.3078838504395562, 0.32800114289498883, 0.330013099327456, 0.32810693126309537, 0.12085435434594227], 3247.147934975216, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[3.1490251886777108, -0.3808099147827359], [0.8827969122941107, -0.8376508235036905], [0.8801834036552443, -0.8478609281444696], [3.2231718833329936, -0.30539683129152817], [1.8433299986389022, -0.757581759678916], [1.8498670207606767, -0.7546353643706133], [1.8566490756925154, -0.7219177948203557], [1.8445910072257294, -0.7495703534996738], [1.84449166178134, -0.7986510088170632], [1.850119338237868, -0.7597744940910298]  …  [2.3712433652681604, -0.5772706459842877], [0.34514038842016803, -0.35914697565937714], [0.4958345988390292, -0.5742987981520745], [2.7280655511287457, -0.5233703830480316], [2.723815076000939, -0.5130821335237977], [2.722278172032215, -0.5071684017797021], [2.6260103803424784, -0.5123978699308432], [2.6184847035246994, -0.504746767048975], [2.6286160655554784, -0.5558329782309435], [3.489585602013936, 0.0412151891279986]], [0.00038331277988385375, 0.13712966041025437, 0.137653953302949, 0.000289950104784113, 0.0212394958810785, 0.02090542436977794, 0.02056345436878952, 0.02117471009383673, 0.021179808125237534, 0.02089261776295546  …  0.005144504952889274, 0.2603787714706442, 0.22398421756604336, 0.0016848667772394554, 0.0017086994177890776, 0.0017173920196149786, 0.002348894127252486, 0.0024061631662405363, 0.002329353926905038, 0.00010163292374044083], 448.2869200382449, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.7152667475933726, -1.0446236155936113], [0.7152667475933726, -1.058462568311874], [0.7152667475933726, -1.0306269234956198], [0.7106112180263504, -1.0653942057509853], [0.7106112180263504, -1.0715483771090373], [0.7106112180263504, -1.0211336925783896], [1.691813646703119, -0.8419192901074676], [1.7122655167284442, -0.8264447053063624], [1.6847614600179275, -0.8215317859176051], [1.0510993044240307, -1.1115287453444318]  …  [0.2733109932882926, -0.48767673131942507], [0.2733109932882926, -0.48379142502409167], [0.2733109932882926, -0.49479301976217605], [0.2733109932882926, -0.44640151328038224], [0.2733109932882926, -0.4995519323068016], [0.3809748392086143, -0.7358494661365833], [0.3809748392086143, -0.7326196397006348], [0.3809748392086143, -0.7429958751608008], [0.3809748392086143, -0.7574567048527602], [0.3809748392086143, -0.7424257218435806]], [0.1688918922621689, 0.1688918922621689, 0.1688918922621689, 0.1678622801479248, 0.1678622801479248, 0.1678622801479248, 0.37722315408246276, 0.3797340216214738, 0.3763247165943758, 0.2479418193405147  …  0.08580916594288908, 0.08580916594288908, 0.08580916594288908, 0.08580916594288908, 0.08580916594288908, 0.10303577404314977, 0.10303577404314977, 0.10303577404314977, 0.10303577404314977, 0.10303577404314977], 2220.8315176147144, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.5063420244746504, -1.255273573102619], [0.5091413628942486, -1.2643253395938685], [0.49753237687615337, -1.3016295095120436], [0.49630154260454296, -1.3302272035998368], [0.5063844795106724, -1.2686070998318908], [1.5234297886816255, -0.9261241720293102], [1.5469765756671716, -0.915514716382152], [1.5469765756671716, -0.918695344461195], [1.5204551028344064, -0.9205638988243029], [1.5204551028344064, -0.8931125438914223]  …  [0.44699157897388775, -1.1999567239133302], [2.307540424045313, -0.6145859564856468], [2.307540424045313, -0.5699247401459061], [0.3543819865304989, -1.1310096521970698], [0.35201429461127365, -1.1758229928719537], [0.17316253209275623, -0.6064499933975611], [0.1743523893358574, -0.6491910883215215], [0.17340060682693229, -0.6509208686402488], [0.23445091126848733, -0.9092697652137537], [0.22948349823806224, -0.9381177803963787]], [0.06004044876728405, 0.060368205363562716, 0.05901753255494267, 0.05887564703877015, 0.06004540975558678, 0.2591003254104472, 0.26475771731463904, 0.26475771731463904, 0.2583840843461722, 0.2583840843461722  …  0.05339680428993136, 0.39477171489038054, 0.39477171489038054, 0.044156129515192995, 0.04393719427151962, 0.029698265607135618, 0.02977889870146176, 0.029714385100703525, 0.03408673982616229, 0.0337128159365891], 1450.5187845973915, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.2562762949754749, -1.551389410231037], [0.23025610188457557, -1.571019255486742], [1.3382049542757635, -1.0210910825264754], [1.3382049542757635, -1.0369014720488576], [1.3638736323907412, -1.0294157149998548], [1.3638736323907412, -1.00634855276751], [1.3632375067749325, -0.9996652497616278], [1.3632375067749325, -1.0294936106097952], [1.336342323069546, -1.0216508411302636], [1.341832594056122, -0.9881970018642223]  …  [1.8841970691510728, -0.7756987405269695], [1.8841970691510728, -0.764057521137879], [0.20830910256974544, -1.4154186058085203], [2.1846232327481836, -0.6461416812535143], [2.1846232327481836, -0.6521706929113901], [2.1846232327481836, -0.6434154919375427], [2.1935554760161318, -0.6242320586203738], [2.1935554760161318, -0.6452051455557749], [0.12818005609108493, -1.334738163322132], [0.04321643309888251, -0.7768980567506798]], [0.33313731035915545, 0.3278619873060545, 0.3552775793946343, 0.3552775793946343, 0.35079804589140573, 0.35079804589140573, 0.3509111669133831, 0.3509111669133831, 0.3555957334320653, 0.35465523116019254  …  0.235321998472068, 0.235321998472068, 0.3233072370271616, 0.16519782333294003, 0.16519782333294003, 0.16519782333294003, 0.16324343835867464, 0.16324343835867464, 0.30595461299818755, 0.28655445203042573], 3487.1761141469533, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-0.05400158707073255, -1.840813006186981], [-0.08394774921277282, -1.8306724554766245], [1.1339867377704684, -1.1685094093330628], [1.130824659865992, -1.141787926506046], [1.1579904893907702, -1.1611869746096681], [1.1626039218372393, -1.1719565720099439], [1.163304456822607, -1.1223096240327073], [1.1573387846529735, -1.1601663540011198], [1.1320121548434932, -1.157386058812022], [1.1441931936832774, -1.167146122912413]  …  [1.7290573210456788, -0.8180758685114098], [1.731385564923497, -0.8386548532454456], [-0.07477461859195861, -1.669241503720529], [2.055394896497481, -0.69200888414685], [2.054189094165906, -0.7045396755846222], [2.055940134360675, -0.6833501885636775], [2.068709064292057, -0.6694754975830921], [2.064514446904977, -0.6796120541893934], [-0.13876757657334146, -1.5605878516925638], [-0.11216317825125346, -0.9462104032919898]], [0.17894423725103129, 0.17220840231521997, 0.39772123078152766, 0.3976207835799991, 0.3983546655743293, 0.3984502216811787, 0.39846399190956766, 0.39834048551621315, 0.39765896902546255, 0.39801846946753827  …  0.34907632830061713, 0.3486556380786859, 0.17426094084278693, 0.27961137617494825, 0.27989557639959184, 0.2794828282958466, 0.2764657031583006, 0.2774582142168905, 0.16015521695517815, 0.1659577984046394], 3245.715258016444, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-0.42216418830812874, -2.0890736498008415], [-0.45008224030809774, -2.1064181666461037], [0.9002848559038559, -1.3384444994706095], [0.9024670745647827, -1.3314861989874929], [0.9257530944688366, -1.317299610903491], [0.9282126074352505, -1.3333614991959164], [0.9388425320160655, -1.2871033733227244], [0.9253055138527495, -1.302284351160301], [0.9005349430810887, -1.3387096796088853], [0.9107639691007948, -1.3313969649603044]  …  [1.5654421473433968, -0.8789674361301116], [1.563654594274408, -0.9063266345094964], [-0.4086229193360644, -1.921116755527336], [1.916993119668111, -0.758860590577537], [1.9132811590489813, -0.7714286707712087], [1.9192700966479397, -0.743544048808876], [1.9348139647754388, -0.7401804312847043], [1.9285920360670983, -0.740676728031964], [-0.4508851469118542, -1.7829297280766996], [-0.3014052589096514, -1.0688722621912397]], [0.3900729294669503, 0.3922361532822127, 0.12291216779713098, 0.12250097871817782, 0.11816295697237368, 0.11771010609903328, 0.11576472235683656, 0.11824547673338258, 0.12286500434958518, 0.12094487959704346  …  0.035501265630679874, 0.03564107467441877, 0.3889188010287738, 0.0154017675285231, 0.015548207778363042, 0.015312517638927168, 0.014714873430072592, 0.014951681172050788, 0.3922940202409999, 0.37744889731611536], 2098.1801181186606, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-0.8399789182682971, -2.263101583659621], [-0.8713658736373184, -2.2833623980222697], [-0.8713658736373184, -2.292872684564872], [0.632595956009734, -1.5988350727208362], [0.6361698347672842, -1.58689619314393], [0.6615403075960672, -1.5593274375154749], [0.6648486436206893, -1.5041394764897715], [0.6444845761087339, -1.5316831568082536], [-0.48372707991624303, -2.403371029741943], [-0.48372707991624303, -2.3715852191888214]  …  [-0.8190427958284998, -2.3942714351565506], [-0.9715681986230391, -2.152045876319864], [-0.9715681986230391, -2.195330468902394], [1.3896486601173745, -0.9731398608430228], [-0.7928462704415317, -2.11574000383566], [-0.7928462704415317, -2.1073826266884077], [-0.8074710925271942, -1.9302903071228075], [-0.8074710925271942, -1.9345557428371976], [-0.5151797113478993, -1.2274432468814092], [-0.5151797113478993, -1.1714996235094768]], [0.22103866737768724, 0.21352123315541857, 0.21352123315541857, 0.3703225447573838, 0.3698098594113176, 0.36605629486847213, 0.36555230036999203, 0.3686016069941612, 0.30552316663319923, 0.30552316663319923  …  0.22607576199995738, 0.1899344876807419, 0.1899344876807419, 0.20761735335890422, 0.2323969387268756, 0.2323969387268756, 0.22886587751949058, 0.22886587751949058, 0.29843610583923214, 0.29843610583923214], 3218.2606260073894, nothing, nothing)
 ⋮
 WeightedParticleBelief{Vector{Float64}}([[1.0085342202035963, -1.2604774127557268], [0.8811378693620046, -1.2903678632669688], [0.8968659994572434, -1.3841474567244523], [1.0120477918351178, -1.2312798292795868], [0.8120384472286745, -1.3557961772781721], [0.8684164685516051, -1.3781650322784502], [0.9659831568859145, -1.2993830777396138], [0.8438318346535288, -1.3927853085308253], [0.9581850282271139, -1.2854378720400212], [1.0112881666809903, -1.2359247867152339]  …  [0.7904681546641588, -1.4561208071453375], [0.9380080971912735, -1.3357169039625034], [0.6686490748530052, -1.5144240026668907], [0.7844922949411587, -1.4715604261128485], [0.8555404116141938, -1.3930685196885388], [0.719940790115722, -1.4866419345926511], [0.7232304609876283, -1.5220430538464922], [0.7376252960198451, -1.5147146621579377], [0.837571947585736, -1.360457658609269], [0.682256317802877, -1.5500046912332774]], [0.3841872744719056, 0.36798404477355545, 0.3702719109386859, 0.3845556752529551, 0.35705096012402104, 0.3660776416123462, 0.3793816850266237, 0.3622552194253536, 0.3784332282982287, 0.3844764001872791  …  0.35335932554382754, 0.37588407594749734, 0.33031821230126235, 0.3523143607090367, 0.3640981117843311, 0.34044584412157947, 0.3410752691778016, 0.343799439773876, 0.36125344544480736, 0.33306063105017386], 3851.636405729093, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.7564387376524508, -1.4288865043181231], [0.6230642967086109, -1.521485846709089], [0.6200365081123529, -1.6024876119285227], [0.7657918259792005, -1.416723658788958], [0.5408792117730401, -1.6017752794601068], [0.5927834620959151, -1.6060139757827154], [0.7061065413379917, -1.4770698782239213], [0.5652747729473637, -1.6563300967101509], [0.7010974538191097, -1.4919693975765873], [0.7641032093379435, -1.4236116108020949]  …  [0.4992439932350912, -1.7182189598673387], [0.6708647163987729, -1.5615098730212127], [0.365764274319627, -1.772598377140884], [0.49018020971858894, -1.7203637333004023], [0.576926707676486, -1.617307105359453], [0.42261240319719173, -1.7220662249148928], [0.41882185021832985, -1.7757352104194957], [0.4346823635882575, -1.7783528695598196], [0.5654804158638822, -1.578360209693463], [0.37225537955622146, -1.806614391805469]], [0.3907390123217096, 0.37689153324124086, 0.37650518374631003, 0.39146758409134025, 0.36535359205247026, 0.37289154388899015, 0.3862609175735979, 0.369001137769122, 0.3857646003115995, 0.3913384789534649  …  0.3587180780942433, 0.3825786851106132, 0.3343166436609869, 0.35720752637076936, 0.37067827056391045, 0.3452504803587863, 0.3445450731835178, 0.3474730274780158, 0.3690311056273212, 0.33560222887219665], 3907.605127393568, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.4706614367888262, -1.7155527893689597], [0.318767127366793, -1.80852130348649], [0.29953898572664833, -1.8964681666082885], [0.48244709422140886, -1.6662124971420638], [0.2205241558810187, -1.8571660099142029], [0.27158066693937205, -1.8814757246392957], [0.4106925656932074, -1.7213738512167966], [0.23400875360533352, -1.9340327124060008], [0.4027035743037922, -1.7569282243114026], [0.47938088717752453, -1.6947600353134153]  …  [0.15560020126162344, -2.075623231387785], [0.3585627417945303, -1.8262300245599783], [0.011244598891450186, -2.1244004183273346], [0.14610746305850847, -2.0450084066584107], [0.2534652866045954, -1.9309179056867152], [0.07819915821421314, -2.029115791657211], [0.06367480813443072, -2.107052368741233], [0.07901178967629358, -2.0683165826548553], [0.24980837392518956, -1.8565676532305266], [0.010932501195127675, -2.1144261474493216]], [0.1514106884747772, 0.1211493809964756, 0.11758405190944596, 0.15390446082451448, 0.10359846595079265, 0.11251207266070928, 0.13903443230463958, 0.10590845165851827, 0.13742660630877993, 0.15325379324807809  …  0.09292581047570449, 0.12872423398450644, 0.07187722646542033, 0.09142799959409542, 0.10929750384416832, 0.0811807604883378, 0.07909540348002868, 0.08129853955724581, 0.1086554873602079, 0.0718357028343266], 1572.6729414279598, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[0.12755087891503425, -2.0088196058745487], [-0.04293713333050497, -2.1539312134823434], [-0.07975464759500939, -2.273240275215881], [0.14920459479299608, -1.966453327483485], [-0.15090904610182188, -2.183242365829286], [-0.1047144779884871, -2.2195479382307868], [0.06641779544984805, -2.012420485250447], [-0.15279778887586665, -2.247745992145801], [0.0513179294415117, -2.0890701707661936], [0.14042888011484145, -1.997840475770051]  …  [-0.25952444501593364, -2.4324558100363634], [-0.006683263117465366, -2.1396205645450537], [-0.41363548477401674, -2.5063559427674127], [-0.26289421827317366, -2.3980619113961876], [-0.1327182945327477, -2.2691091065708897], [-0.3276240001172291, -2.3575490796907412], [-0.3577356656138159, -2.4592988182760904], [-0.33465152685467753, -2.445679458148843], [-0.12150515672091577, -2.179478017805009], [-0.41195272829473667, -2.4389049866994497]], [0.16436602801393665, 0.20328373441185235, 0.2120194717562939, 0.159656556449512, 0.22909865297017515, 0.2179861385468803, 0.17797418446603713, 0.22955444574343029, 0.18140098942969196, 0.16155783359926418  …  0.2553545163972105, 0.19477535687747918, 0.2918838082991369, 0.2561671996117258, 0.2247138721625512, 0.2716903555816654, 0.2788303370899139, 0.27336234184167635, 0.2220163157398107, 0.2914953745171076], 1593.5381345894514, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-0.27421304225987553, -2.2993291517874948], [-0.4737233760269737, -2.489706321201908], [-0.5344027026381857, -2.6067916909950064], [-0.24408607070370092, -2.2981601539012093], [-0.5875575192676791, -2.4672375891767078], [-0.5486240656346445, -2.583107938082036], [-0.33606630160024137, -2.3480372195182486], [-0.6023469873050269, -2.5928036848052933], [-0.36649610471172706, -2.4546233425381465], [-0.25913921503916876, -2.2938268566509854]  …  [-0.7460156070232064, -2.723226566180232], [-0.43460737602647614, -2.466442566088599], [-0.9149066733274993, -2.7629328220469143], [-0.7425066005524112, -2.7062720165385588], [-0.5865401158469257, -2.6148450665425838], [-0.7991338160553774, -2.634420842396398], [-0.849595429269034, -2.7222291091431643], [-0.8237874184844461, -2.7175946380227276], [-0.5574007602819175, -2.4935747797073735], [-0.8997337256346266, -2.6666723713192386]], [0.25359406555744757, 0.30059137559877674, 0.3140546123481125, 0.24631281776957142, 0.32535708983702566, 0.3171272811082906, 0.26845992787784234, 0.32840849323112853, 0.2757027856830051, 0.24995278435720486  …  0.3555113952419855, 0.29165023214496516, 0.3800807884156002, 0.3549107639385294, 0.32514560719729024, 0.3641805817788455, 0.3716394089380951, 0.3679228152502624, 0.31900636044142844, 0.37824661506873364], 2453.8120760696065, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-0.7340788726173745, -2.5659272967128075], [-0.9716646402673553, -2.6718449612689814], [-1.055761040837187, -2.803357370352548], [-0.7037181014839429, -2.6012699108908754], [-1.0810050371030206, -2.633468947887938], [-1.0652456532510517, -2.766967593136418], [-0.8056737455038911, -2.6427967733283704], [-1.1209077242660856, -2.744580409807368], [-0.8574207732193564, -2.7187810517742785], [-0.7179045863693658, -2.6014426613102195]  …  [-1.2906609202592527, -2.7592665998944295], [-0.927895889244196, -2.694391647219354], [-1.4674932377368823, -2.6224732520425067], [-1.283761003860123, -2.7426432604775823], [-1.1095091291554424, -2.766616859558764], [-1.3260179845346571, -2.61047955155346], [-1.3940412510976667, -2.6925734906516348], [-1.3673063460889918, -2.6737551778829807], [-1.0561157162233923, -2.6634687266059363], [-1.4330681998984742, -2.5442978789252426]], [0.3800313306997298, 0.34308782150098877, 0.3264450224963923, 0.38346745832071116, 0.3211650195136596, 0.32447537763459383, 0.37069447312639314, 0.3125861076865508, 0.3629291039891506, 0.3819018432426503  …  0.2736644670926248, 0.3510989145712518, 0.23107361940041915, 0.2753022848402587, 0.31506428465010955, 0.2652252765166581, 0.24883769556677682, 0.25529492920021984, 0.32637168166033903, 0.2393962712711715], 3769.346457935778, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-1.247264331959936, -2.6204950207940114], [-1.5060336325211516, -2.5111912386749418], [-1.6164325149076966, -2.530470379325347], [-1.223972083662118, -2.661537361241775], [-1.6076988266806083, -2.3196483131690964], [-1.6186391718783353, -2.4920979839671134], [-1.3342331001695653, -2.6709711362783186], [-1.6698238062275594, -2.419224027833047], [-1.4011769835742123, -2.6726598779842785], [-1.23819311863141, -2.6668863471862823]  …  [-1.8425142402381387, -2.1928776472051403], [-1.4667742186880668, -2.5935898961657586], [-1.9919878881453839, -1.8224736753807735], [-1.8322896559556394, -2.2169729561221634], [-1.6628325010671952, -2.4806033729182198], [-1.848113894845349, -2.038071216507299], [-1.9325559492279938, -1.9842704584286723], [-1.9020573816655881, -2.0335049763838184], [-1.5888094615445796, -2.370262406614337], [-1.941927775683523, -1.8645233017918708]], [0.08132637496261172, 0.12477244371879985, 0.14674856504376105, 0.077996079469979, 0.1449415631132006, 0.147206899950922, 0.09461282304741378, 0.15803328579497625, 0.10575459781369605, 0.08001796590987408  …  0.196941633136088, 0.11743240830069997, 0.2326047761869013, 0.1945533243468993, 0.15653299878201649, 0.19825324657712293, 0.21829365312184815, 0.21100542838341305, 0.1410722810853484, 0.22054219126071964], 825.2226596341225, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-1.7713633361187382, -2.1179502425784724], [-2.00827188025614, -1.71457071038835], [-2.00827188025614, -1.7631522650292593], [-2.122526590772766, -1.555736446168389], [-1.756279555910473, -2.196099723718214], [-2.0716284893144277, -1.4329516282389911], [-2.0716284893144277, -1.408503181332284], [-2.117058768671758, -1.5342095846529522], [-2.117058768671758, -1.519031423729996], [-1.8684273274252292, -2.072327527431823]  …  [-2.329410040913728, -0.7109423804078334], [-2.329410040913728, -0.7221937563668492], [-2.329410040913728, -0.7541301729494276], [-2.308758376942352, -0.7986224290210977], [-2.308758376942352, -0.7860908746569508], [-2.308758376942352, -0.8259308097758036], [-2.062861942867447, -1.4452098472401804], [-2.3148324360418973, -0.6181014314909183], [-2.3148324360418973, -0.5936436183893692], [-2.3148324360418973, -0.6477431847960387]], [0.1244048248158637, 0.173668121970802, 0.173668121970802, 0.19993134529118514, 0.12155904207828472, 0.18807681944477309, 0.18807681944477309, 0.1986475060553317, 0.1986475060553317, 0.14359682452892406  …  0.2495738950819211, 0.2495738950819211, 0.2495738950819211, 0.2445792197462558, 0.2445792197462558, 0.2445792197462558, 0.18605851472453463, 0.24604867775357547, 0.24604867775357547, 0.24604867775357547], 1396.2774696300937, nothing, nothing)
 WeightedParticleBelief{Vector{Float64}}([[-2.1949533846344327, -1.0287542620877523], [-2.35118602233381, -0.49034961840154456], [-2.360902333261992, -0.4855566469409074], [-2.433673880006444, -0.2870080061430025], [-2.195499500654116, -1.0903491327817378], [-2.358218814962226, -0.2663853147352333], [-2.3533291255808844, -0.23571045681307168], [-2.4239006856023484, -0.3064907107629016], [-2.4208650534177574, -0.29329268986337764], [-2.282892832911594, -0.8758716117479104]  …  [-2.4715985169952948, 0.28513960683010375], [-2.473848792187098, 0.25706641522412155], [-2.4802360755036137, 0.23916272834608432], [-2.468482862746572, 0.24381307416826603], [-2.465976551873742, 0.21340480715872956], [-2.4739445388975128, 0.19375043634074096], [-2.3519039123154832, -0.27413187391796723], [-2.438452722340081, 0.26721004647885416], [-2.4335611597197713, 0.27848542969441414], [-2.444381073001105, 0.24981843059335307]], [0.12008725631542094, 0.09312283440532779, 0.09158779481271052, 0.0806249793711946, 0.11998565755485781, 0.09201007385096076, 0.09278281078231626, 0.08204256221032868, 0.08248632394694481, 0.10438466579181757  …  0.07528408489684886, 0.07497514937215435, 0.07410309740093265, 0.07571329610041101, 0.07605980386401864, 0.07496202424460814, 0.09300884337428302, 0.07993798107229752, 0.08064123269291572, 0.07909135093266295], 1118.2930795951056, 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)