Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
190 lines (162 sloc) 6.83 KB
function [x_hat_k, X_k, w_k, w_kkm1] = bf( ...
t_km1, t_k, X_km1, w_km1, u_km1, z_k, ...
f, h, draw_proc_noise, prob_obs_error, ...
h_tune, resample)
% bf
%
% Run one update of a basic boostrap filter, a type of particle filter,
% which can estimate well enough on many types of problems.
%
% [x_k, X_k, w_k] = bf(t_km1, t_k, X_km1, w_km1, u_km1, z_k, ...
% f, h, draw_proc_noise, prob_obs_error, ...
% h_tune);
%
% Inputs:
%
% t_km1 Time corresponding to the input states (sample k-1)
% t_k Time at the current measurement (sample k)
% X_km1 Matrix of particles at sample k-1, |nx-by-np|
% w_km1 Matrix of weights at sample k-1, |1-by-np|
% u_km1 Input vector at sample k-1
% z_k Measurement at sample k
% f Propagation function with an interface defined as:
%
% x_k = fcn([t_km1 t_k], x_km1, u_km1, q_km1)
%
% where |x_km1| is the prior state of a single particle
% and |q_km1| is the process noise acting on the
% particle, yielding an updated state, |x_k|, for the
% particle, and |t_k| is the time of current sample
% h Measurement function with an interface defined as:
%
% z_k = fcn(t_k, x_k, u_km1)
%
% where |x_k| is the state of a particle at sample k and
% |z_k| is the corresponding measurement
% draw_proc_noise A function to draw process noise given a time and
% corresponding particle state
%
% q_km1 = fcn(t_km1, x_km1)
%
% prob_obs_error A function to return the probability density of the
% measurement error -- the difference between the
% actual measurement and a perfect measurement for the
% true state, with the interface:
%
% p_k = fcn(t_k, dz_k, x_k)
%
% Only relative scores matter, so constant coefficients
% can be dropped. For instance, for a measurement model
% with normally distrubted errors with covariance
% matrix, |R|, one can use:
%
% p_k = @(t, dz, x) exp(-0.5 * dz.' * inv(R) * dz)
%
% (noting that the preceding constant
% |(2*pi)^(-k/2) / sqrt(det(R))| has been dropped since
% it won't affect the results).
% h_tune An optional regulation tuning parameter, defaulting
% to:
%
% h_tune = (4/(N * (nx + 2)))^(1/(nx + 4));
%
% which is optimal for a Gaussian error distribution.
% This parameter scales the spread of particles when
% they are redrawn about the mean during regularization.
% resample Set to true to resample and regularize (true by default)
%
% Outputs:
%
% x_hat_k Mean state estimate for sample k
% X_k Set of particles for sample k
% w_k Particle weights at sample k
% w_kkm1 Updated weights, prior to resampling (for analysis only)
%
% See <https://www.anuncommonlab.com/doc/starkf/bf.html> for more.
% Copyright 2016 An Uncommon Lab
% Dimensions
[nx, N] = size(X_km1);
nz = size(z_k, 1);
% If no tuning parameter is provided, use the optimal regularization
% parameter for a Gaussian kernal.
if nargin < 11 || isempty(h_tune)
h_tune = (4/(N * (nx + 2)))^(1/(nx + 4));
end
% Resample and regularize by default.
if nargin < 12, resample = true; end;
% Propagate.
X_kkm1 = zeros(nx, N);
Z_kkm1 = zeros(nz, N);
w_kkm1 = zeros(1, N);
for k = 1:N
% Predict the state.
X_kkm1(:, k) = f(t_km1, t_k, X_km1(:,k), u_km1, ...
draw_proc_noise(t_km1, X_km1(:,k)));
% Predict the measurement.
Z_kkm1(:, k) = h(t_k, X_kkm1(:,k), u_km1);
% Update the weight based on the difference between the true and
% predicted measurement.
w_kkm1(k) = w_km1(k) * prob_obs_error(t_k, ...
z_k - Z_kkm1(:, k), ...
X_kkm1(:, k));
end
% Normalize the weights.
w_kkm1 = w_kkm1 ./ sum(w_kkm1);
% Calculate the updated state. This now includes information from the
% observation since we updated the weights with the probability of each
% observation error.
x_hat_k = sum(bsxfun(@times, w_kkm1, X_kkm1), 2);
% If we're resampling and regularizing...
if resample
% Randomly pick particles from X_kkm1 according to their weights.
% The weights act as a probability mass function.
indices = pmfdraw(w_kkm1);
X_k = X_kkm1(:, indices);
% By virtue of randomly selecting particles, all of the current
% particles now have the same weight of 1/N.
w_k = repmat(1/N, 1, N);
% Regularize by creating the sample covariance matrix and drawing
% random perturbations from it.
X_twiddle_k = bsxfun(@minus, X_k, sum(X_k, 2)/N);
Sigma = 1/(N-1) * (X_twiddle_k * X_twiddle_k.');
% % Alternately, we could have used the weights instead of the
% % newly selected particles. The expected result is the same; the
% % actual result may be slightly different.
% X_twiddle_k = bsxfun(@minus, X_kkm1, x_hat_k);
% X_twiddle_k = bsxfun(@times, X_twiddle_k, sqrt(w_kkm1));
% Sigma = X_twiddle_k * X_twiddle_k.';
% Multiply those perturbations by the tuning parameter and add them
% to the particles.
[u, s] = svd(Sigma);
X_k = X_k + h_tune * u * sqrt(s) * randn(nx, N);
% Otherwise, use the updated values.
else
X_k = X_kkm1;
w_k = w_kkm1;
end
end % bf
% Draw n indices randomly from a probability mass function defined by the
% vector p.
function indices = pmfdraw(p, n)
% Default number of draws is the number of elements in p.
if nargin < 2, n = length(p); end;
% Create the discrete cumulative distribution function.
cdf = cumsum(p);
% If it's not actually a normalized input, normalize it.
if cdf(end) ~= 1
cdf = cdf ./ cdf(end);
end
% Make a bunch of normal, uniform random draws and sort them.
draws = sort(rand(1, n));
% Find the CDF "bin" containing each draw. This will be the appropriate
% index of the CDF.
np = length(p);
indices = zeros(1, n); % Output randomly drawn indices
ci = 1; % Index of PMF bin
for di = 1:n
while cdf(ci) < draws(di) && ci < np
ci = ci + 1;
end
indices(di) = ci;
end
end % pmfdraw