From 9d354bdb03d22c0f4608348a1bb845d907699f32 Mon Sep 17 00:00:00 2001 From: Gina Sierra Date: Tue, 28 Aug 2018 15:54:15 -0500 Subject: [PATCH 1/2] Update ParticleFilter This modifies the method 'initialize' so that the initial states are sampled from a Gaussian distribution whose variance vector, x0Variance, is a new property of PF. --- MATLAB/+Observers/ParticleFilter.m | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/MATLAB/+Observers/ParticleFilter.m b/MATLAB/+Observers/ParticleFilter.m index 8dc60f3..665c3fa 100644 --- a/MATLAB/+Observers/ParticleFilter.m +++ b/MATLAB/+Observers/ParticleFilter.m @@ -42,6 +42,7 @@ outputEqn; % Handle to output equation n; % Sensor noise variance vector v; % Process noise variance vector + x0variance; % Initial states variance vector numParticles; % Number of particles used by the filter particles = []; % Particles structure minNEff; % Effective particle number resampling threshold @@ -50,7 +51,7 @@ methods - function PF = ParticleFilter(stateEqn,outputEqn,processVariance,sensorVariance,numParticles,varargin) + function PF = ParticleFilter(stateEqn,outputEqn,processVariance,sensorVariance,numParticles,x0Variance,varargin) % ParticleFilter Constructor % Construct a particle filter given the state and output % equations, process and sensor noise variance vectors, the @@ -68,6 +69,7 @@ PF.v = processVariance; PF.n = sensorVariance; PF.numParticles = numParticles; + PF.x0variance=x0Variance; % Set additional optional properties args = struct(varargin{:}); @@ -100,9 +102,9 @@ function initialize(PF,t0,x0,u0) % states, and inputs % Generate initial particle population - x0 = repmat(x0,1,PF.numParticles); - PF.particles.x = x0 + PF.initGain*PF.generateProcessNoise(); - PF.particles.z = PF.outputEqn(t0,PF.particles.x,u0,0); + x0 = repmat(x0,1,PF.numParticles); + PF.particles.x = x0 + sqrt(diag(PF.x0variance))*randn(length(PF.x0variance),PF.numParticles); + PF.particles.z = PF.outputEqn(t0,PF.particles.x,u0,0); PF.particles.w = PF.likelihood(PF.outputEqn(t0,x0,u0,0),PF.particles.z); % Normalize weights From d3abbf7ab5b617bfb83590055027143fafc9400e Mon Sep 17 00:00:00 2001 From: Gina Sierra Date: Tue, 28 Aug 2018 15:57:57 -0500 Subject: [PATCH 2/2] Update testParticleFilter This include initial states variances in the parameters of PF --- MATLAB/testParticleFilter.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MATLAB/testParticleFilter.m b/MATLAB/testParticleFilter.m index 2d5d976..694e9d9 100644 --- a/MATLAB/testParticleFilter.m +++ b/MATLAB/testParticleFilter.m @@ -14,7 +14,7 @@ % Create PF numParticles = 100; PF = Observers.ParticleFilter(@battery.stateEqn,@battery.outputEqn,... - battery.V,battery.N,numParticles); + battery.V,battery.N,numParticles,battery.x0Variance); % Get initial state for battery t0 = 0;