-
Notifications
You must be signed in to change notification settings - Fork 3
/
pf_samson.m
168 lines (153 loc) · 6.18 KB
/
pf_samson.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
function [xhk, pf] = pf_samson(fx, hx ,yk, pf, resampling_strategy)
%% Generic particle filter
%
% Note: when resampling is performed on each step this algorithm is called
% the Bootstrap particle filter
%
% Usage:
% [xhk, pf] = particle_filter(sys, yk, pf, resamping_strategy)
%
% Inputs:
% sys = function handle to process equation
% yk = observation vector at time k (column vector)
% pf = structure with the following fields
% .k = iteration number
% .Ns = number of particles
% .w = weights (Ns x T)
% .particles = particles (nx x Ns x T)
% .gen_x0 = function handle of a procedure that samples from the initial pdf p_x0
% .p_yk_given_xk = function handle of the observation likelihood PDF p(y[k] | x[k])
% .gen_sys_noise = function handle of a procedure that generates system noise
% resampling_strategy = resampling strategy. Set it either to
% 'multinomial_resampling' or 'systematic_resampling'
%
% Outputs:
% xhk = estimated state
% pf = the same structure as in the input but updated at iteration k
%
% Reference:
% [1] Arulampalam et. al. (2002). A tutorial on particle filters for
% online nonlinear/non-gaussian bayesian tracking. IEEE Transactions on
% Signal Processing. 50 (2). p 174--188
%% Programmed by:
% Diego Andres Alvarez Marin ([email protected])
% Universidad Nacional de Colombia at Manizales, February 29, 2012
%%
k = pf.k;
if k == 1
error('error: k must be an integer greater or equal than 2');
end
%% Initialize variables
Ns = pf.Ns; % number of particles
nx = size(pf.particles,1); % number of states
wkm1 = pf.w(:,k-1); % weights of last iteration
if k == 2
for i = 1:Ns % simulate initial particles
pf.particles(:,i,1) = pf.xsys_init; % at time k=1
end
wkm1(:,1) = repmat(1/Ns, Ns, 1); % all particles have the same weight
end
%%
% The importance sampling function:
% PRIOR: (this method is sensitive to outliers) THIS IS THE ONE USED HERE
% q_xk_given_xkm1_yk = pf.p_xk_given_xkm1;
% OPTIMAL:
% q_xk_given_xkm1_yk = q_xk_given_xkm1^i_yk;
% Note this PDF can be approximated by MCMC methods: they are expensive but
% they may be useful when non-iterative schemes fail
%% Separate memory
%xalg = pf.algebraic(:,k-1);
xkm1 = pf.particles(:,:,k-1); % extract particles from last iteration;
xk = zeros(size(xkm1)); % = zeros(nx,Ns);
wk = zeros(size(wkm1)); % = zeros(Ns,1);
%% Algorithm 3 of Ref [1]
for i = 1:Ns
% xk(:,i) = sample_vector_from q_xk_given_xkm1_yk given xkm1(:,i) and yk
% sys is geni_fx_k = geni_fx_kplus1(xsysi_kminus1,Vi_kminus1,Idi_kminus1,Iqi_kminus1);
% xalg_kminus1 is geni_pseudo_input_kminus1 = [Vi_kminus1; teta_kminus1];
xk(:,i) = fx(xkm1(:,i))+ pf.gen_sys_noise(k);
% xk(:,i) = fx(xkm1(:,i));
% Equation 48, Ref 1.
% wk(i) = wkm1(i) * p_yk_given_xk(yk, xk(:,i))*p_xk_given_xkm1(xk(:,i), xkm1(:,i))/q_xk_given_xkm1_yk(xk(:,i), xkm1(:,i), yk);
% weights (when using the PRIOR pdf): eq 63, Ref 1
% yk
% xk
% xal
% pf.p_yk_given_xk(k, yk, xk(:,i),xal')
% pf.p_yk_given_xk_pdf is gen1_ykgivenxk_pdf = @(gen1_ykgivenxk_sigma, gen1_y_k, xsys30_k, V30_k, teta30_k)...
% 10^-20+normpdf(gen1_y_k,gen1_hx_k(xsys30_k,V30_k, teta30_k),gen1_ykgivenxk_sigma);
prior=10^-20+normpdf(yk, hx(xk(:,i)), pf.yk_given_xk_sigma);% k, yk, xk, xalgk)
% prior=10^-20+normpdf(yk, hx(xk(:,i)));
% pause
wk(i) = wkm1(i) * prod(prior(1:2),1); % prod(prior(1:2),1) is y(k+1)/x(k)
% weights (when using the OPTIMAL pdf): eq 53, Ref 1
% wk(i) = wkm1(i) * p_yk_given_xkm1(yk, xkm1(:,i)); % we do not know this PDF
end;
%% Normalize weight vector
if sum(wk)==0
error('All weights are zero');
end
wk = wk./sum(wk);
%% Calculate effective sample size: eq 48, Ref 1
Neff = 1/sum(wk.^2);
%% Resampling
% remove this condition and sample on each iteration:
% [xk, wk] = resample(xk, wk, resampling_strategy);
%if you want to implement the bootstrap particle filter
resample_percentage = 0.5;
Nt = resample_percentage*Ns;
if Neff < Nt
disp('Resampling ...')
[xk, wk] = resample(xk, wk, resampling_strategy);
% {xk, wk} is an approximate discrete representation of p(x_k | y_{1:k})
end
%% Compute estimated state
xhk = zeros(nx,1);
for i = 1:Ns;
xhk = xhk + wk(i)*xk(:,i); % mean
end
%% Store new weights and particles
pf.w(:,k) = wk;
pf.particles(:,:,k) = xk;
return; % bye, bye!!!
%% Resampling function
function [xk, wk, idx] = resample(xk, wk, resampling_strategy)
Ns = length(wk); % Ns = number of particles
% wk = wk./sum(wk); % normalize weight vector (already done)
switch resampling_strategy
case 'multinomial_resampling'
with_replacement = true;
idx = randsample(1:Ns, Ns, with_replacement, wk);
%{
THIS IS EQUIVALENT TO:
edges = min([0 cumsum(wk)'],1); % protect against accumulated round-off
edges(end) = 1; % get the upper edge exact
% this works like the inverse of the empirical distribution and returns
% the interval where the sample is to be found
[~, idx] = histc(sort(rand(Ns,1)), edges);
%}
case 'systematic_resampling'
% this is performing latin hypercube sampling on wk
edges = min([0 cumsum(wk)'],1); % protect against accumulated round-off
edges(end) = 1; % get the upper edge exact
u1 = rand/Ns;
% this works like the inverse of the empirical distribution and returns
% the interval where the sample is to be found
[~, idx] = histc(u1:1/Ns:1, edges);
% case 'regularized_pf' TO BE IMPLEMENTED
% case 'stratified_sampling' TO BE IMPLEMENTED
% case 'residual_sampling' TO BE IMPLEMENTED
case 'Ripley Method'
%Multinomial sampling with Riply's method
u=rand(Ns,1);
wc=cumsum(wk);
wc=wc/wc(Ns);
[dum,ind1]=sort([u;wc]);
ind2=find(ind1<=Ns);
idx=ind2-(0:Ns-1)';
otherwise
error('Resampling strategy not implemented')
end;
xk = xk(:,idx); % extract new particles
wk = repmat(1/Ns, 1, Ns); % now all particles have the same weight
return; % bye, bye!!!