代写ECE5550: Applied Kalman Filtering PARTICLE FILTERS代做留学生Matlab程序

ECE5550: Applied Kalman Filtering

PARTICLE FILTERS

7.1: Numeric integration to solve Bayesian recursion

■ Recall from Chap. 4 that the optimal Bayesian recursion is to:

Compute the pdf for predicting xk given all past observations

Update the pdf for estimating xk given all observations via

■ If we knew how to compute these pdfs, then we could find any desired estimator; e.g., mean, median, mode, whatever.

■ So far, we have assumed that computing these pdfs is intractable, so have made approximations to arrive at the KF, EKF, and SPKF.

■ We now revisit this assumption.

Numeric integration

■ The prediction step requires evaluating an integral.

■ Normalization of f (xk Zk ) via its denominator requires an integral

■ If state dimension n is large (e.g., n 3), evaluation of these multi-  dimensional integrals may be impractical, even with modern CPUs.

■ Computation needed for a given precision scales exponentially with n. The particle filter will explore some clever ways to avoid this issue.  But, first, we look at numeric integration for low-order problems.

■ Suppose that we wish to evaluate the definite integral

■ The rectangular rule approximates g(x) as piecewise constant.

■ Let Nr be the number of constant regions.

■ Then, the width of each region is w     (xmax xmin)/Nr , and the integral can be approximated as

■ The trapezoidal rule approximates the function as piecewise linear.

■ The integral approximation is integral

Application to Bayesian inference

■ Can write prediction step as

■ If we can compute this integral for every value of xk , can directly estimate posterior distribution f (xk Zk ).

■ Limiting factor is precision vs. speed: Each integral requires O(Nr )

calculations at each of Nr evaluation points, or O(Nr(2)) operations.

TRACKING EXAMPLE: We wish to track time-varying xk for model

xk+1 = αxk + wk

zk = 0.1xk 3 + vk

where wk ~ N(0,σw(2)) and vk  ~ N(0,σv(2)) are mutually uncorrelated and IID.

■ For the prediction step, we numerically integrate to approximate

■ For the time-update step, we directly compute

■ Note that the process model gives us

f (xk | xk-1) ~ N(αxk-1, σ2w)

and the measurement model gives us

f (zk | xk) ~ N (0.1xk 3 , σv 2 ).

■ For this example, let σw        0.2, σv       0.1, α     0.99, and x0        N(0,σw(2)).

■ Note that f (x0     Z0) needs to be specified, where

■ Since Z  1  is unknown, we assume f (x0     Z  1) f (x0)    N(0,σw(2)).

■ Simulation run for 200 samples, range of integration from    3 to 3 with Nr 500 regions.

■ If we are able to find/approximate f (xk Zk ), we can compute mean, median, mode, whatever.

Can plot these point estimates as functions of time.

Alternately, can plot the sequence of distributions as a pseudo- color image (black is low probability; white is high probability).

% Code originally based on Angle TrackingExample.m by James McNames

% of Portland State University

clear; close all;

% Define User-Specified Parameters

nSamples                 = 201;

xMin                      = -3;

xMax                      =  3;

measurementNoiseSigma = 0 .1;  processNoiseSigma     = 0 .2;  nRegions                 = 500;  alpha                  = 0 .99;

% Create Sequence of Observations from Model

x = zeros (nSamples+1,1); % time [0 . .nSamples]

z = zeros (nSamples  ,1); % time [0 . .nSamples-1]

x (1) = randn (1)*processNoiseSigma; % initialize x{0} ~ N(0,sigmaw^2)

for k=1:nSamples

x (k+1) = alpha*x (k) + randn (1)*processNoiseSigma;

z (k  ) = 0 .1*x (k)^3 + randn (1)*measurementNoiseSigma;

end

x = x (1:nSamples); % constrain to [0 . .nSamples-1]

% Recursively Estimate the Marginal Posterior

xIntegration = linspace (xMin,xMax,nRegions) . ';

width = mean (diff (xIntegration));

% initialize pdf for f(x{0})

prior = normpdf (xIntegration,0,processNoiseSigma);

% compute f(z{0} |x{0}) *f(x{0} |Z{-1}) = f(z{0} |x{0}) *f(x{0})

posteriorFiltered(:,1) = normpdf (z (1),0 .1*xIntegration .^3, . . .

measurementNoiseSigma) .*prior;

% compute f(x{0} |Z{0})=f(z{0} |x{0}) *f(x{0} |Z{-1})/[normalizing cst]

posteriorFiltered(:,1) = posteriorFiltered(:,1)/trapz (xIntegration, . . .

posteriorFiltered(:,1));

for k=2:nSamples

for cRegion=1:nRegions % find f(x{k}=x{i} |x{k-1}) for each x{i}

% compute f(x{k} |x{k-1})

prior = normpdf (xIntegration (cRegion),alpha*xIntegration, . . .

processNoiseSigma);

% compute f(x{k} |Z{k-1})=integral[ f(x{k} |x{k-1}) *f(x{k-1} |Z{k-1}) ]

posteriorPredicted(cRegion) = trapz (xIntegration, . . .

prior .*posteriorFiltered(:,k-1));

end

% compute f(z{k} |x{k})

likelihood = normpdf (z (k),0 .1*xIntegration .^3,measurementNoiseSigma);

% compute f(x{k} |Z{k-1}) *f(z{k} |x{k})

posteriorFiltered(:,k) = likelihood .*posteriorPredicted;

% normalize to get f(x{k} |Z{k})

posteriorFiltered(:,k) = posteriorFiltered(:,k)/trapz (xIntegration, . . .

posteriorFiltered(:,k));

% Find median, 95% confidence interval

percentiles = cumtrapz (xIntegration,posteriorFiltered(:,k));

iMedian = find(percentiles <= 0 .5,1,'last' );

iLower = find(percentiles <= 0 .025,1,'last' );

if isempty (iLower), iLower = 1; end;

iUpper = find(percentiles >= 0 .975,1,'first' );

if isempty (iUpper), iUpper = length (xIntegration); end

xHatMedian (k) = xIntegration (iMedian);

xLower (k) = xIntegration (iLower);

xUpper (k) = xIntegration (iUpper);

end

[~,iMax] = max (posteriorFiltered);

xHatMode = xIntegration (iMax);

xHatMean = xIntegration' *posteriorFiltered.*width;

% Plot true state and observed sequence

figure (10); clf;

ax = plotyy (0:nSamples-1,x,0:nSamples-1,z);

legend ( 'True state x_k' ,'Measured z_k' );

ylabel (ax (1),'x_k' ); ylabel (ax (2),'z_k' );

title ( 'Signals for tracking example' );

xlabel ( 'Iteration k' );