% PURPOSE : Demonstrate the differences between the following filters on the same problem:
%
% 1) Extended Kalman Filter (EKF)
% 2) Unscented Kalman Filter (UKF)
% 3) Particle Filter (PF)
% 4) PF with EKF proposal (PFEKF)
% 5) PF with UKF proposal (PFUKF)
% For more details refer to:
% AUTHORS : Nando de Freitas (jfgf@cs.berkeley.edu)
% Rudolph van der Merwe (rvdmerwe@ece.ogi.edu)
% DATE : 17 August 2000
clear all;
clc;
echo off;
path('./ukf',path);
% INITIALISATION AND PARAMETERS:
% ==============================
no_of_runs = 100; % number of experiments to generate statistical
% averages
doPlot = 0; % 1 plot online. 0 = only plot at the end.
sigma = 1e-5; % Variance of the Gaussian measurement noise.
g1 = 3; % Paramater of Gamma transition prior.
g2 = 2; % Parameter of Gamman transition prior.
% Thus mean = 3/2 and var = 3/4.
T = 60; % Number of time steps.
R = 1e-5; % EKF's measurement noise variance.
Q = 3/4; % EKF's process noise variance.
P0 = 3/4; % EKF's initial variance of the states.
N = 5; % Number of particles.
resamplingScheme = 1; % The possible choices are
% systematic sampling (2),
% residual (1)
% and multinomial (3).
% They're all O(N) algorithms.
Q_pfekf = 10*3/4;
R_pfekf = 1e-1;
Q_pfukf = 2*3/4;
R_pfukf = 1e-1;
alpha = 1; % UKF : point scaling parameter
beta = 0; % UKF : scaling parameter for higher order terms of Taylor series expansion
kappa = 2; % UKF : sigma point selection scaling parameter (best to leave this = 0)
%**************************************************************************************
% SETUP BUFFERS TO STORE PERFORMANCE RESULTS
% ==========================================
rmsError_ekf = zeros(1,no_of_runs);
rmsError_ukf = zeros(1,no_of_runs);
rmsError_pf = zeros(1,no_of_runs);
rmsError_pfMC = zeros(1,no_of_runs);
rmsError_pfekf = zeros(1,no_of_runs);
rmsError_pfekfMC = zeros(1,no_of_runs);
rmsError_pfukf = zeros(1,no_of_runs);
rmsError_pfukfMC = zeros(1,no_of_runs);
time_pf = zeros(1,no_of_runs);
time_pfMC = zeros(1,no_of_runs);
time_pfekf = zeros(1,no_of_runs);
time_pfekfMC = zeros(1,no_of_runs);
time_pfukf = zeros(1,no_of_runs);
time_pfukfMC = zeros(1,no_of_runs);
%**************************************************************************************
% MAIN LOOP
for j=1:no_of_runs,
rand('state',sum(100*clock)); % Shuffle the pack!
randn('state',sum(100*clock)); % Shuffle the pack!
% GENERATE THE DATA:
% ==================
x = zeros(T,1);
y = zeros(T,1);
processNoise = zeros(T,1);
measureNoise = zeros(T,1);
x(1) = 1; % Initial state.
for t=2:T
processNoise(t) = gengamma(g1,g2);
measureNoise(t) = sqrt(sigma)*randn(1,1);
x(t) = feval('ffun',x(t-1),t) +processNoise(t); % Gamma transition prior.
y(t) = feval('hfun',x(t),t) + measureNoise(t); % Gaussian likelihood.
end;
% PLOT THE GENERATED DATA:
% ========================
figure(1)
clf;
plot(1:T,x,'r',1:T,y,'b');
ylabel('Data','fontsize',15);
xlabel('Time','fontsize',15);
legend('States (x)','Observations(y)');
%%%%%%%%%%%%%%% PERFORM EKF and UKF ESTIMATION %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% ============================== %%%%%%%%%%%%%%%%%%%%%
% INITIALISATION:
% ==============
mu_ekf = ones(T,1); % EKF estimate of the mean of the states.
P_ekf = P0*ones(T,1); % EKF estimate of the variance of the states.
mu_ukf = mu_ekf; % UKF estimate of the mean of the states.
P_ukf = P_ekf; % UKF estimate of the variance of the states.
yPred = ones(T,1); % One-step-ahead predicted values of y.
mu_ekfPred = ones(T,1); % EKF O-s-a estimate of the mean of the states.
PPred = ones(T,1); % EKF O-s-a estimate of the variance of the states.
disp(' ');
for t=2:T,
fprintf('run = %i / %i : EKF & UKF : t = %i / %i \r',j,no_of_runs,t,T);
fprintf('\n')
% PREDICTION STEP:
% ================
mu_ekfPred(t) = feval('ffun',mu_ekf(t-1),t);
Jx = 0.5; % Jacobian for ffun.
PPred(t) = Q + Jx*P_ekf(t-1)*Jx';
% CORRECTION STEP:
% ================
yPred(t) = feval('hfun',mu_ekfPred(t),t);
if t<=30,
Jy = 2*0.2*mu_ekfPred(t); % Jacobian for hfun.
else
Jy = 0.5;
% Jy = cos(mu_ekfPred(t))/2;
% Jy = 2*mu_ekfPred(t)/4; % Jacobian for hfun.
end;
M = R + Jy*PPred(t)*Jy'; % Innovations covariance.
K = PPred(t)*Jy'*inv(M); % Kalman gain.
mu_ekf(t) = mu_ekfPred(t) + K*(y(t)-yPred(t));
P_ekf(t) = PPred(t) - K*Jy*PPred(t);
% Full Unscented Kalman Filter step
% =================================
[mu_ukf(t),P_ukf(t)]=ukf(mu_ukf(t-1),P_ukf(t-1),[],Q,'ukf_ffun',y(t),R,'ukf_hfun',t,alpha,beta,kappa);
end; % End of t loop.
%%%%%%%%%%%%%%% PERFORM SEQUENTIAL MONTE CARLO %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% ============================== %%%%%%%%%%%%%%%%%%%%%
% INITIALISATION:
% ==============
xparticle_pf = ones(T,N); % These are the particles for the estimate
% of x. Note that there's no need to store
% them for all t. We're only doing this to
% show you all the nice plots at the end.
xparticlePred_pf = ones(T,N); % One-step-ahead predicted values of the states.
yPred_pf = ones(T,N); % One-step-ahead predicted values of y.
w = ones(T,N); % Importance weights.
disp(' ');
tic; % Initialize timer for benchmarking
for t=2:T,
fprintf('run = %i / %i : PF : t = %i / %i \r',j,no_of_runs,t,T);
fprintf('\n')
% PREDICTION STEP:
% ================
% We use the transition prior as proposal.
for i=1:N,
xparticlePred_pf(t,i) = feval('ffun',xparticle_pf(t-1,i),t) + gengamma(g1,g2);
end;
% EVALUATE IMPORTANCE WEIGHTS:
% ============================
% For our choice of proposal, the importance weights are give by:
for i=1:N,
yPred_pf(t,i) = feval('hfun',xparticlePred_pf(t,i),t);
lik = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(t)-yPred_pf(t,i))^(2))) ...
+ 1e-99; % Deal with ill-conditioning.
w(t,i) = lik;
end;
w(t,:) = w(t,:)./sum(w(t,:)); % Normalise the weights.
% SELECTION STEP:
% ===============
% Here, we give you the choice to try three different types of
% resampling algorithms. Note that the code for these algorithms
% applies to any problem!
if resamplingScheme == 1
outIndex = residualR(1:N,w(t,:)'); % Residual resampling.
elseif resamplingScheme == 2
outIndex = systematicR(1:N,w(t,:)'); % Systematic resampling.
else
outIndex = multinomialR(1:N,w(t,:)'); % Multinomial resampling.
end;
xparticle_pf(t,:) = xparticlePred_pf(t,outIndex); % Keep particles with
% resampled indices.
end; % End of t loop.
time_pf(j) = toc; % How long did this take?
%%%%%%%%%%%%%% PERFORM SEQUENTIAL MONTE CARLO WITH MCMC %%%%%%%%%%%%%%%%
%%%%%%%%%%%%%% ======================================== %%%%%%%%%%%%%%%%
% INITIALISATION:
% ==============
xparticle_pfMC = ones(T,N); % These are the particles for the estimate
% of x. Note that there's no need to store
% them for all t. We're only doing this to
% show you all the nice pl
概率数据互联(pdaf).zip
需积分: 9 47 浏览量
2021-12-13
20:31:40
上传
评论
收藏 228KB ZIP 举报
yangjing1003004612
- 粉丝: 5
- 资源: 7
最新资源
- ASP.NET公文管理系统的设计与实现(源码)
- 操作系统原理与设计Chapter 2: OS Structure
- torch-2.3.1-cp312-cp312-manylinux2014-aarch64.whl
- CSR8675蓝牙芯片 CSR内部培训资料教材资料.zip
- 43-2-每日英语听力 10.9.2会员版_鹿蜀 【20240530更新】.apk
- 期末大作业基于EasyX和C语言的可视化学生成绩管理系统(95分以上)
- 数字电路芯片74系列芯片datasheet技术手册资料总汇合集(241个).zip
- CSDNApp_226.apk
- CCNA实训2022.pka
- 金融分析期末作业.ipynb
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
评论0