133 lines
4.9 KiB
Matlab
133 lines
4.9 KiB
Matlab
function [x, X, w, w_kkm1] = pf_opt(t_km1, t_k, X_km1, w_km1, u_km1, y_k, Rv, Rn, robotName, Geometry, Gravity, h_tune, integrationAlgorithm, dt_control, Xd, Kp, Ki, Kd, Ktau, antiWindup, limQ_L, limQ_U, limQp_L, limQp_U, limQpp_L, limQpp_U, limTau_L, limTau_U, useComputedTorque)
|
|
%#codegen
|
|
|
|
% PF: Particle Filter
|
|
% Authors: Quentin Leboutet, Julien Roux, Alexandre Janot and Gordon Cheng
|
|
% This code is inspired of the work of Wan, Eric A. and Rudoph van der Merwe and www.anuncommonlab.com
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
% INPUTS:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
% X_km1: Matrix of particles at time k-1|nx-by-np|
|
|
% w_km1: Matrix of weights at time k-1, |1-by-np|
|
|
% u_km1: control input at time k-1
|
|
% y_k: noisy observation at time k
|
|
% Rv: process noise covariance matrix
|
|
% Rn: observation noise covariance matrix
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
% OUTPUTS:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
% x: Estimated state
|
|
% X: Matrix of particles at time k |nx-by-np|
|
|
% w: Matrix of weights at time k, |1-by-np|
|
|
% w_kkm1: Updated weights, prior to resampling
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
% Problem dimensions:
|
|
[Xdim, Np] = size(X_km1); % Number of states and number of particles
|
|
Ydim = numel(y_k); % Number of observations
|
|
|
|
%% Sequential importance sampling step
|
|
|
|
% Propagate particles:
|
|
X_kkm1 = zeros(Xdim, Np);
|
|
w_kkm1 = zeros(1, Np);
|
|
invRn = inv(Rn);
|
|
|
|
parfor particle = 1:Np
|
|
% Propagate the particles:
|
|
X_kkm1(:, particle) = process_Model(t_km1, t_k, X_km1(:,particle), u_km1, 1e-2*(w_kkm1(particle))*randn(Xdim,1), robotName, Geometry, Gravity, integrationAlgorithm, dt_control, Xd, Kp, Ki, Kd, Ktau, antiWindup, limQ_L, limQ_U, limQp_L, limQp_U, limQpp_L, limQpp_U, limTau_L, limTau_U, useComputedTorque);
|
|
% Update the weight based on the difference between the true and predicted measurement:
|
|
w_kkm1(particle) = w_km1(particle) * prob_obs_error(invRn, y_k - measurement_Model(X_kkm1(:, particle), zeros(Ydim,1)), X_kkm1(:, particle));
|
|
end
|
|
|
|
% Normalize the weights:
|
|
w_kkm1 = w_kkm1 ./ sum(w_kkm1);
|
|
|
|
% Calculate the updated state:
|
|
x = sum(bsxfun(@times, w_kkm1, X_kkm1), 2);
|
|
|
|
% Resample:
|
|
[X_kkm1,w_kkm1] = resampleParticles(X_kkm1, w_kkm1, Np, h_tune, Xdim);
|
|
|
|
X = X_kkm1;
|
|
w = w_kkm1;
|
|
|
|
end % pf
|
|
|
|
function [X_k,w] = resampleParticles(X_kkm1, w_kkm1, Np, h_tune, Xdim)
|
|
% Randomly pick particles from X_kkm1 according to their weights.
|
|
% The weights act as a probability mass function.
|
|
% outIndex = zeros(1,Np);
|
|
|
|
% outIndex = randsample(1:Np, Np, true, w_kkm1);
|
|
% X_k = X_kkm1(:, outIndex);
|
|
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/Np.
|
|
w = (1/Np)*ones(1,Np);
|
|
|
|
X_twiddle_k = bsxfun(@minus, X_k, sum(X_k, 2)/Np);
|
|
Sigma = 1/(Np-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.
|
|
% h_tune = (4/(Np * (Xdim + 2)))^(1/(Xdim + 4));
|
|
|
|
[u, s] = svd(Sigma);
|
|
X_k = X_k + h_tune * u * sqrt(s) * randn(Xdim, Np);
|
|
|
|
end
|
|
% 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
|
|
function p = prob_obs_error(invRn, dy, varargin)
|
|
p = 2*chi2pdf(norm(dy), 2);
|
|
% p = exp(-0.5e-6 * dy.'*invRn*dy); % Normal probability density function
|
|
|
|
end
|
|
|