《最优状态估计-卡尔曼，H∞及非线性滤波》：第15章 粒子滤波
前言
《最优状态估计-卡尔曼，H∞及非线性滤波》由国外引进的一本关于状态估计的专业书籍，2006年正式出版，作者是Dan Simon教授，来自克利夫兰州立大学，电气与计算机工程系。主要应用于运动估计与控制，学习本文的过程中需要有一定的专业基础知识打底。
本书共分为四个部分，全面介绍了最优状态估计的理论和方法。第1部分为基础知识，回顾了线性系统、概率论和随机过程相关知识，介绍了最小二乘法、维纳滤波、状态的统计特性随时间的传播过程。第2部分详细介绍了卡尔曼滤波及其等价形式，介绍了卡尔曼滤波的扩展形式，包括相关噪声和有色噪声条件下的卡尔曼滤波、稳态滤波、衰减记忆滤波和带约束的卡尔曼滤波等（掌握了卡尔曼，基本上可以说这本书掌握了一半）。第3部分详细介绍了H∞滤波，包括时域和频域的H∞滤波，混合卡尔曼/H∞滤波，带约束的H∞ 滤波。第4部分介绍非线性系统滤波方法，包括扩展卡尔曼滤波、无迹卡尔曼滤波及粒子滤波。本书适合作为最优状态估计相关课程的高年级本科生或研究生教材，或从事相关研究工作人员的参考书。
其实自己研究生期间的主研方向并不是运动控制，但自己在本科大三时参加过智能车大赛，当时是采用PID对智能车的运动进行控制，彼时凭借着自学的一知半解，侥幸拿到了奖项。到了研究生期间，实验室正好有研究平衡车的项目，虽然自己不是那个方向，但实验室经常有组内报告，所以对运动控制在实际项目中的应用也算有了基本的了解。参加工作后，有需要对运动估计与控制进行相关研究，所以接触到这本书。
这次重新捡起运动控制，是希望自己可以将这方面的知识进行巩固再学习，结合原书的习题例程进行仿真，简单记录一下这个过程。主要以各章节中习题仿真为主，这是本书的第十五章的5个仿真示例（仿真平台：32位MATLAB2015b），话不多说，开始！
1. MATLAB仿真：示例15.1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能：《最优状态估计-卡尔曼，H∞及非线性滤波》示例仿真
%示例15.1: ParticleEx1.m
%环境：Win7，Matlab2015b
%Modi: C.S
%时间：2022-05-02
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function ParticleEx1
% Particle filter example, adapted from Gordon, Salmond, and Smith paper.
x = 0.1; % initial state
Q = 1; % process noise covariance
R = 1; % measurement noise covariance
tf = 50; % simulation length
N = 100; % number of particles in the particle filter
xhat = x;
P = 2;
xhatPart = x;
% Initialize the particle filter.
for i = 1 : N
xpart(i) = x + sqrt(P) * randn;
end
xArr = [x];
yArr = [x^2 / 20 + sqrt(R) * randn];
xhatArr = [x];
PArr = [P];
xhatPartArr = [xhatPart];
close all;
for k = 1 : tf
% System simulation
x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;
y = x^2 / 20 + sqrt(R) * randn;
% Extended Kalman filter
F = 0.5 + 25 * (1 - xhat^2) / (1 + xhat^2)^2;
P = F * P * F' + Q;
H = xhat / 10;
K = P * H' * (H * P * H' + R)^(-1);
xhat = 0.5 * xhat + 25 * xhat / (1 + xhat^2) + 8 * cos(1.2*(k-1));
xhat = xhat + K * (y - xhat^2 / 20);
P = (1 - K * H) * P;
% Particle filter
for i = 1 : N
xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;
ypart = xpartminus(i)^2 / 20;
vhat = y - ypart;
q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R);
end
% Normalize the likelihood of each a priori estimate.
qsum = sum(q);
for i = 1 : N
q(i) = q(i) / qsum;
end
% Resample.
for i = 1 : N
u = rand; % uniform random number between 0 and 1
qtempsum = 0;
for j = 1 : N
qtempsum = qtempsum + q(j);
if qtempsum >= u
xpart(i) = xpartminus(j);
break;
end
end
end
% The particle filter estimate is the mean of the particles.
xhatPart = mean(xpart);
% Plot the estimated pdf's at a specific time.
if k == 20
% Particle filter pdf
pdf = zeros(81,1);
for m = -40 : 40
for i = 1 : N
if (m <= xpart(i)) && (xpart(i) < m+1)
pdf(m+41) = pdf(m+41) + 1;
end
end
end
figure;
m = -40 : 40;
plot(m, pdf / N, 'r');
hold;
title('Estimated pdf at k=20');
disp(['min, max xpart(i) at k = 20: ', num2str(min(xpart)), ', ', num2str(max(xpart))]);
% Kalman filter pdf
pdf = (1 / sqrt(P) / sqrt(2*pi)) .* exp(-(m - xhat).^2 / 2 / P);
plot(m, pdf, 'b');
legend('Particle filter', 'Kalman filter');
end
% Save data in arrays for later plotting
xArr = [xArr x];
yArr = [yArr y];
xhatArr = [xhatArr xhat];
PArr = [PArr P];
xhatPartArr = [xhatPartArr xhatPart];
end
t = 0 : tf;
%figure;
%plot(t, xArr);
%ylabel('true state');
figure;
plot(t, xArr, 'b.', t, xhatArr, 'k-', t, xhatArr-2*sqrt(PArr), 'r:', t, xhatArr+2*sqrt(PArr), 'r:');
axis([0 tf -40 40]);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time step'); ylabel('state');
legend('True state', 'EKF estimate', '95% confidence region');
figure;
plot(t, xArr, 'b.', t, xhatPartArr, 'k-');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time step'); ylabel('state');
legend('True state', 'Particle filter estimate');
xhatRMS = sqrt((norm(xArr - xhatArr))^2 / tf);
xhatPartRMS = sqrt((norm(xArr - xhatPartArr))^2 / tf);
disp(['Kalman filter RMS error = ', num2str(xhatRMS)]);
disp(['Particle filter RMS error = ', num2str(xhatPartRMS)]);
>> ParticleEx1
已锁定最新绘图
min, max xpart(i) at k = 20: -6.0588, 7.0139
Kalman filter RMS error = 20.9337
Particle filter RMS error = 3.6006
2. MATLAB仿真：示例15.2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能：《最优状态估计-卡尔曼，H∞及非线性滤波》示例仿真
%示例15.2: ParticleEx2.m
%环境：Win7，Matlab2015b
%Modi: C.S
%时间：2022-05-02
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [xArray, xhatArray] = ParticleEx2
% Particle filter example.
% Track a body falling through the atmosphere.
% This system is taken from [Jul00], which was based on [Ath68].
rho0 = 2; % lb-sec^2/ft^4
g = 32.2; % ft/sec^2
k = 2e4; % ft
R = 10^4; % measurement noise variance (ft^2)
Q = diag([0 0 0]); % process noise covariance
M = 10^5; % horizontal range of position sensor
a = 10^5; % altitude of position sensor
P = diag([1e6 4e6 10]); % initial estimation error covariance
x = [3e5; -2e4; 1e-3]; % initial state
xhat = [3e5; -2e4; 1e-3]; % initial state estimate
N = 1000; % number of particles
% Initialize the particle filter.
for i = 1 : N
xhatplus(:,i) = x + sqrt(P) * [randn; randn; randn];
end
T = 0.5; % measurement time step
randn('state',sum(100*clock)); % random number generator seed
tf = 30; % simulation length (seconds)
dt = 0.04; % time step for integration (seconds)
xArray = x;
xhatArray = xhat;
for t = T : T : tf
fprintf('.');
% Simulate the system.
for tau = dt : dt : T
% Fourth order Runge Kutta ingegration
dx1(1,1) = x(2);
dx1(2,1) = rho0 * exp(-x(1)/k) * x(2)^2 / 2 * x(3) - g;
dx1(3,1) = 0;
dx1 = dx1 * dt;
xtemp = x + dx1 / 2;
dx2(1,1) = xtemp(2);
dx2(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g;
dx2(3,1) = 0;
dx2 = dx2 * dt;
xtemp = x + dx2 / 2;
dx3(1,1) = xtemp(2);
dx3(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g;
dx3(3,1) = 0;
dx3 = dx3 * dt;
xtemp = x + dx3;
dx4(1,1) = xtemp(2);
dx4(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g;
dx4(3,1) = 0;
dx4 = dx4 * dt;
x = x + (dx1 + 2 * dx2 + 2 * dx3 + dx4) / 6;
x = x + sqrt(dt * Q) * [randn; randn; randn] * dt;
end
% Simulate the noisy measurement.
z = sqrt(M^2 + (x(1)-a)^2) + sqrt(R) * randn;
% Simulate the continuous-time part of the particle filter (time update).
xhatminus = xhatplus;
for i = 1 : N
for tau = dt : dt : T
% Fourth order Runge Kutta ingegration
xtemp = xhatminus(:,i);
dx1(1,1) = xtemp(2);
dx1(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g;
dx1(3,1) = 0;
dx1 = dx1 * dt;
xtemp = xhatminus(:,i) + dx1 / 2;
dx2(1,1) = xtemp(2);
dx2(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g;
dx2(3,1) = 0;
dx2 = dx2 * dt;
xtemp = xhatminus(:,i) + dx2 / 2;
dx3(1,1) = xtemp(2);
dx3(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g;
dx3(3,1) = 0;
dx3 = dx3 * dt;
xtemp = xhatminus(:,i) + dx3;
dx4(1,1) = xtemp(2);
dx4(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g;
dx4(3,1) = 0;
dx4 = dx4 * dt;
xhatminus(:,i) = xhatminus(:,i) + (dx1 + 2 * dx2 + 2 * dx3 + dx4) / 6;
xhatminus(:,i) = xhatminus(:,i) + sqrt(dt * Q) * [randn; randn; randn] * dt;
xhatminus(3,i) = max(0, xhatminus(3,i)); % the ballistic coefficient cannot be negative
end
zhat = sqrt(M^2 + (xhatminus(1,i)-a)^2);
vhat(i) = z - zhat;
end
% Note that we need to scale all of the q(i) probabilities in a way
% that does not change their relative magnitudes.
% Otherwise all of the q(i) elements will be zero because of the
% large value of the exponential.
vhatscale = max(abs(vhat)) / 4;
qsum = 0;
for i = 1 : N
q(i) = exp(-(vhat(i)/vhatscale)^2);
qsum = qsum + q(i);
end
% Normalize the likelihood of each a priori estimate.
for i = 1 : N
q(i) = q(i) / qsum;
end
% Resample.
for i = 1 : N
u = rand; % uniform random number between 0 and 1
qtempsum = 0;
for j = 1 : N
qtempsum = qtempsum + q(j);
if qtempsum >= u
xhatplus(:,i) = xhatminus(:,j);
% Use roughening to prevent sample impoverishment.
E = max(xhatminus')' - min(xhatminus')';
sigma = 0.2 * E * N^(-1/length(x));
xhatplus(:,i) = xhatplus(:,i) + sigma .* [randn; randn; randn];
xhatplus(3,i) = max(0,xhatplus(3,i)); % the ballistic coefficient cannot be negative
break;
end
end
end
% The particle filter estimate is the mean of the particles.
xhat = 0;
for i = 1 : N
xhat = xhat + xhatplus(:,i);
end
xhat = xhat / N;
% Save data for plotting.
xArray = [xArray x];
xhatArray = [xhatArray xhat];
end
close all;
t = 0 : T : tf;
figure;
semilogy(t, abs(xArray(1,:) - xhatArray(1,:)), 'b'); hold;
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('Altitude Estimation Error');
figure;
semilogy(t, abs(xArray(2,:) - xhatArray(2,:)), 'b'); hold;
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('Velocity Estimation Error');
figure;
semilogy(t, abs(xArray(3,:) - xhatArray(3,:)), 'b'); hold;
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('Ballistic Coefficient Estimation Error');
figure;
plot(t, xArray(1,:));
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('True Position');
figure;
plot(t, xArray(2,:));
title('Falling Body Simulation', 'FontSize', 12);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('True Velocity');
>> ParticleEx2
............................................................已锁定最新绘图
已锁定最新绘图
已锁定最新绘图
ans =
1.0e+05 *
1 至 11 列
3.0000 2.9040 2.8079 2.7117 2.6154 2.5191 2.4227 2.3262 2.2297 2.1331 2.0364
-0.2000 -0.2002 -0.2003 -0.2005 -0.2006 -0.2008 -0.2009 -0.2010 -0.2012 -0.2013 -0.2014
0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
12 至 22 列
1.9397 1.8430 1.7464 1.6498 1.5533 1.4572 1.3617 1.2671 1.1740 1.0833 0.9960
-0.2015 -0.2015 -0.2014 -0.2011 -0.2006 -0.1997 -0.1982 -0.1957 -0.1918 -0.1859 -0.1772
0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
23 至 33 列
0.9137 0.8378 0.7697 0.7101 0.6592 0.6163 0.5803 0.5502 0.5249 0.5034 0.4851
-0.1654 -0.1504 -0.1331 -0.1150 -0.0975 -0.0818 -0.0684 -0.0574 -0.0484 -0.0412 -0.0355
0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
34 至 44 列
0.4692 0.4554 0.4432 0.4324 0.4227 0.4140 0.4060 0.3988 0.3921 0.3859 0.3802
-0.0308 -0.0270 -0.0239 -0.0213 -0.0191 -0.0173 -0.0158 -0.0145 -0.0133 -0.0124 -0.0115
0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
45 至 55 列
0.3749 0.3699 0.3651 0.3607 0.3565 0.3525 0.3487 0.3450 0.3415 0.3382 0.3349
-0.0108 -0.0101 -0.0095 -0.0090 -0.0086 -0.0081 -0.0078 -0.0074 -0.0071 -0.0069 -0.0066
0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
56 至 61 列
0.3318 0.3288 0.3259 0.3231 0.3203 0.3176
-0.0064 -0.0062 -0.0060 -0.0058 -0.0056 -0.0055
0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
3. MATLAB仿真：示例15.3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能：《最优状态估计-卡尔曼，H∞及非线性滤波》示例仿真
%示例15.3: ParticleEx3.m
%环境：Win7，Matlab2015b
%Modi: C.S
%时间：2022-05-02
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [xhatRMS, xhatPartRMS, xhatPartRegRMS] = ParticleEx3
% Particle filter example, adapted from Gordon, Salmond, and Smith paper.
x = 0.1; % initial state
Q = 0.001; % process noise covariance
R = 1; % measurement noise covariance
tf = 50; % simulation length
N = 3; % number of particles in the particle filter
NReg = 5 * N; % number of probability bins in the regularized particle filter
xhat = x;
P = 2;
xhatPart = x;
xhatPartReg = x;
% Initialize the particle filter.
for i = 1 : N
xpart(i) = x + sqrt(P) * randn; % normal particle filter
xpartReg(i) = xpart(i); % regularized particle filter
end
% Initialization for the regularized particle filter.
d = length(x); % dimension of the state vector
c = 2; % volume of unit hypersphere in d-dimensional space
h = (8 * c^(-1) * (d + 4) * (2 * sqrt(pi))^d)^(1 / (d + 4)) * N^(-1 / (d + 4)); % bandwidth of regularized filter
% Initialize arrays.
xArr = [x];
yArr = [x^2 / 20 + sqrt(R) * randn];
xhatArr = [x];
PArr = [P];
xhatPartArr = [xhatPart];
xhatPartRegArr = [xhatPartReg];
close all; % close all open figures
for k = 1 : tf
% System simulation
x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;
y = x^2 / 20 + sqrt(R) * randn;
% Extended Kalman filter
F = 0.5 + 25 * (1 - xhat^2) / (1 + xhat^2)^2;
P = F * P * F' + Q;
H = xhat / 10;
K = P * H' * (H * P * H' + R)^(-1);
xhat = 0.5 * xhat + 25 * xhat / (1 + xhat^2) + 8 * cos(1.2*(k-1));
xhat = xhat + K * (y - xhat^2 / 20);
P = (1 - K * H) * P;
% Particle filter
for i = 1 : N
xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;
ypart = xpartminus(i)^2 / 20;
vhat = y - ypart;
q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R);
end
% Normalize the likelihood of each a priori estimate.
qsum = sum(q);
for i = 1 : N
q(i) = q(i) / qsum;
end
% Resample.
for i = 1 : N
u = rand; % uniform random number between 0 and 1
qtempsum = 0;
for j = 1 : N
qtempsum = qtempsum + q(j);
if qtempsum >= u
xpart(i) = xpartminus(j);
break;
end
end
end
% The particle filter estimate is the mean of the particles.
xhatPart = mean(xpart);
% Now run the regularized particle filter.
% Perform the time update to the get the a priori regularized particles.
for i = 1 : N
xpartminusReg(i) = 0.5 * xpartReg(i) + 25 * xpartReg(i) / (1 + xpartReg(i)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;
ypart = xpartminusReg(i)^2 / 20;
vhat = y - ypart;
q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R);
end
% Normalize the probabilities of the a priori particles.
q = q / sum(q);
% Compute the covariance of the a priori particles.
S = cov(xpartminusReg');
A = chol(S)';
% Define the domain from which we will choose a posteriori particles for
% the regularized particle filter.
xreg(1) = min(xpartminusReg) - std(xpartminusReg);
xreg(NReg) = max(xpartminusReg) + std(xpartminusReg);
dx = (xreg(NReg) - xreg(1)) / (NReg - 1);
for i = 2 : NReg - 1
xreg(i) = xreg(i-1) + dx;
end
% Create the pdf approximation that is required for the regularized
% particle filter.
for j = 1 : NReg
qreg(j) = 0;
for i = 1 : N
normx = norm(inv(A) * (xreg(j) - xpartminusReg(i)));
if normx < h
qreg(j) = qreg(j) + q(i) * (d + 2) * (1 - normx^2 / h^2) / 2 / c / h^d / det(A);
end
end
end
% Normalize the likelihood of each state estimate for the regularized particle filter.
qsum = sum(qreg);
for j = 1 : NReg
qreg(j) = qreg(j) / qsum;
end
% Resample for the regularized particle filter.
for i = 1 : N
u = rand; % uniform random number between 0 and 1
qtempsum = 0;
for j = 1 : NReg
qtempsum = qtempsum + qreg(j);
if qtempsum >= u
xpartReg(i) = xreg(j);
break;
end
end
end
% The regularized particle filter estimate is the mean of the regularized particles.
xhatPartReg = mean(xpartReg);
% Plot the discrete pdf and the continuous pdf at a specific time.
if k == 5
figure; hold on;
for i = 1 : N
plot([xpartminusReg(i) xpartminusReg(i)], [0 q(i)], 'k-');
plot(xpartminusReg(i), q(i), 'ko');
end
plot(xreg, qreg, 'r:');
set(gca,'FontSize',12); set(gcf,'Color','White');
set(gca,'box','on');
xlabel('state estimate'); ylabel('pdf');
end
% Save data in arrays for later plotting
xArr = [xArr x];
yArr = [yArr y];
xhatArr = [xhatArr xhat];
PArr = [PArr P];
xhatPartArr = [xhatPartArr xhatPart];
xhatPartRegArr = [xhatPartRegArr xhatPartReg];
end
t = 0 : tf;
%figure;
%plot(t, xArr);
%ylabel('true state');
figure;
plot(t, xArr, 'b.', t, xhatArr, 'k-', t, xhatArr-2*sqrt(PArr), 'r:', t, xhatArr+2*sqrt(PArr), 'r:');
axis([0 tf -40 40]);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time step'); ylabel('state');
legend('True state', 'EKF estimate', '95% confidence region');
figure;
plot(t, xArr, 'b.', t, xhatPartArr, 'k-', t, xhatPartRegArr, 'r:');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time step'); ylabel('state');
legend('True state', 'Particle filter', 'Regularized particle filter');
xhatRMS = sqrt((norm(xArr - xhatArr))^2 / tf);
xhatPartRMS = sqrt((norm(xArr - xhatPartArr))^2 / tf);
xhatPartRegRMS = sqrt((norm(xArr - xhatPartRegArr))^2 / tf);
disp(['Kalman filter RMS error = ', num2str(xhatRMS)]);
disp(['Particle filter RMS error = ', num2str(xhatPartRMS)]);
disp(['Regularized particle filter RMS error = ', num2str(xhatPartRegRMS)]);
>> ParticleEx3
Kalman filter RMS error = 0.34704
Particle filter RMS error = 1.161
Regularized particle filter RMS error = 1.1955
ans =
0.3470
4. MATLAB仿真：示例15.4
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能：《最优状态估计-卡尔曼，H∞及非线性滤波》示例仿真
%示例15.4: ParticleEx4.m
%环境：Win7，Matlab2015b
%Modi: C.S
%时间：2022-05-02
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [StdRMSErr, AuxRMSErr] = ParticleEx4
% Particle filter example.
% Track a body falling through the atmosphere.
% This system is taken from [Jul00], which was based on [Ath68].
% Compare the particle filter with the auxiliary particle filter.
global rho0 g k dt
rho0 = 2; % lb-sec^2/ft^4
g = 32.2; % ft/sec^2
k = 2e4; % ft
R = 10^4; % measurement noise variance (ft^2)
Q = diag([0 0 0]); % process noise covariance
M = 10^5; % horizontal range of position sensor
a = 10^5; % altitude of position sensor
P = diag([1e6 4e6 10]); % initial estimation error covariance
x = [3e5; -2e4; 1e-3]; % initial state
xhat = [3e5; -2e4; 1e-3]; % initial state estimate
N = 200; % number of particles
% Initialize the particle filter.
for i = 1 : N
xhatplus(:,i) = x + sqrt(P) * [randn; randn; randn]; % standard particle filter
xhatplusAux(:,i) = xhatplus(:,i); % auxiliary particle filter
end
T = 0.5; % measurement time step
randn('state',sum(100*clock)); % random number generator seed
tf = 30; % simulation length (seconds)
dt = 0.5; % time step for integration (seconds)
xArray = x;
xhatArray = xhat;
xhatAuxArray = xhat;
for t = T : T : tf
fprintf('.');
% Simulate the system.
for tau = dt : dt : T
% Fourth order Runge Kutta ingegration
[dx1, dx2, dx3, dx4] = RungeKutta(x);
x = x + (dx1 + 2 * dx2 + 2 * dx3 + dx4) / 6;
x = x + sqrt(dt * Q) * [randn; randn; randn] * dt;
end
% Simulate the noisy measurement.
z = sqrt(M^2 + (x(1)-a)^2) + sqrt(R) * randn;
% Simulate the continuous-time part of the particle filter (time update).
xhatminus = xhatplus;
xhatminusAux = xhatplusAux;
for i = 1 : N
for tau = dt : dt : T
% Fourth order Runge Kutta ingegration
% standard particle filter
[dx1, dx2, dx3, dx4] = RungeKutta(xhatminus(:,i));
xhatminus(:,i) = xhatminus(:,i) + (dx1 + 2 * dx2 + 2 * dx3 + dx4) / 6;
xhatminus(:,i) = xhatminus(:,i) + sqrt(dt * Q) * [randn; randn; randn] * dt;
xhatminus(3,i) = max(0, xhatminus(3,i)); % the ballistic coefficient cannot be negative
% auxiliary particle filter
[dx1, dx2, dx3, dx4] = RungeKutta(xhatminusAux(:,i));
xhatminusAux(:,i) = xhatminusAux(:,i) + (dx1 + 2 * dx2 + 2 * dx3 + dx4) / 6;
xhatminusAux(:,i) = xhatminusAux(:,i) + sqrt(dt * Q) * [randn; randn; randn] * dt;
xhatminusAux(3,i) = max(0, xhatminusAux(3,i)); % the ballistic coefficient cannot be negative
end
zhat = sqrt(M^2 + (xhatminus(1,i)-a)^2);
vhat(i) = z - zhat;
zhatAux = sqrt(M^2 + (xhatminusAux(1,i)-a)^2);
vhatAux(i) = z - zhatAux;
end
% Note that we need to scale all of the q(i) probabilities in a way
% that does not change their relative magnitudes.
% Otherwise all of the q(i) elements will be zero because of the
% large value of the exponential.
% standard particle filter
vhatscale = max(abs(vhat)) / 4;
qsum = 0;
for i = 1 : N
q(i) = exp(-(vhat(i)/vhatscale)^2);
qsum = qsum + q(i);
end
% Normalize the likelihood of each a priori estimate.
for i = 1 : N
q(i) = q(i) / qsum;
end
% auxiliary particle filter
vhatscaleAux = max(abs(vhatAux)) / 4;
qsumAux = 0;
for i = 1 : N
qAux(i) = exp(-(vhatAux(i)/vhatscaleAux)^2);
qsumAux = qsumAux + qAux(i);
end
% Regularize the probabilities - this is conceptually identical to the
% auxiliary particle filter - increase low probabilities and decrease
% high probabilities.
% Large k means low regularization (k = infinity is identical to the
% standard particle filter). Small k means high regularization (k = 1
% means that all probabilities are equal).
kAux = 1.1;
qAux = ((kAux - 1) * qAux + mean(qAux)) / kAux;
% Normalize the likelihood of each a priori estimate.
for i = 1 : N
qAux(i) = qAux(i) / qsumAux;
end
% Resample the standard particle filter
for i = 1 : N
u = rand; % uniform random number between 0 and 1
qtempsum = 0;
for j = 1 : N
qtempsum = qtempsum + q(j);
if qtempsum >= u
xhatplus(:,i) = xhatminus(:,j);
xhatplus(3,i) = max(0,xhatplus(3,i)); % the ballistic coefficient cannot be negative
break;
end
end
end
% The standard particle filter estimate is the mean of the particles.
xhat = mean(xhatplus')';
% Resample the auxiliary particle filter
for i = 1 : N
u = rand; % uniform random number between 0 and 1
qtempsum = 0;
for j = 1 : N
qtempsum = qtempsum + qAux(j);
if qtempsum >= u
xhatplusAux(:,i) = xhatminusAux(:,j);
xhatplusAux(3,i) = max(0,xhatplusAux(3,i)); % the ballistic coefficient cannot be negative
break;
end
end
end
% The auxiliary particle filter estimate is the mean of the particles.
xhatAux = mean(xhatplusAux')';
% Save data for plotting.
xArray = [xArray x];
xhatArray = [xhatArray xhat];
xhatAuxArray = [xhatAuxArray xhatAux];
end
close all;
t = 0 : T : tf;
figure;
semilogy(t, abs(xArray(1,:) - xhatArray(1,:)), 'b-'); hold;
semilogy(t, abs(xArray(1,:) - xhatAuxArray(1,:)), 'r:');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('Altitude Estimation Error');
legend('Standard particle filter', 'Auxiliary particle filter');
figure;
semilogy(t, abs(xArray(2,:) - xhatArray(2,:)), 'b-'); hold;
semilogy(t, abs(xArray(2,:) - xhatAuxArray(2,:)), 'r:');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('Velocity Estimation Error');
legend('Standard particle filter', 'Auxiliary particle filter');
figure;
semilogy(t, abs(xArray(3,:) - xhatArray(3,:)), 'b-'); hold;
semilogy(t, abs(xArray(3,:) - xhatAuxArray(3,:)), 'r:');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('Ballistic Coefficient Estimation Error');
legend('Standard particle filter', 'Auxiliary particle filter');
figure;
plot(t, xArray(1,:));
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('True Position');
figure;
plot(t, xArray(2,:));
title('Falling Body Simulation', 'FontSize', 12);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('True Velocity');
for i = 1 : 3
StdRMSErr(i) = sqrt((norm(xArray(i,:) - xhatArray(i,:)))^2 / tf / dt);
AuxRMSErr(i) = sqrt((norm(xArray(i,:) - xhatAuxArray(i,:)))^2 / tf / dt);
end
disp(['Standard particle filter RMS error = ', num2str(StdRMSErr)]);
disp(['Auxiliary particle filter RMS error = ', num2str(AuxRMSErr)]);
function [dx1, dx2, dx3, dx4] = RungeKutta(x)
% Fourth order Runge Kutta integration for the falling body system.
global rho0 g k dt
dx1(1,1) = x(2);
dx1(2,1) = rho0 * exp(-x(1)/k) * x(2)^2 / 2 * x(3) - g;
dx1(3,1) = 0;
dx1 = dx1 * dt;
xtemp = x + dx1 / 2;
dx2(1,1) = xtemp(2);
dx2(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g;
dx2(3,1) = 0;
dx2 = dx2 * dt;
xtemp = x + dx2 / 2;
dx3(1,1) = xtemp(2);
dx3(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g;
dx3(3,1) = 0;
dx3 = dx3 * dt;
xtemp = x + dx3;
dx4(1,1) = xtemp(2);
dx4(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g;
dx4(3,1) = 0;
dx4 = dx4 * dt;
return;
>> ParticleEx4
............................................................已锁定最新绘图
已锁定最新绘图
已锁定最新绘图
Standard particle filter RMS error = 307277.3073 29175.53723 0.6545551726
Auxiliary particle filter RMS error = 88495.5421 15077.9012 1.59109849
ans =
1.0e+05 *
3.0728 0.2918 0.0000
5. MATLAB仿真：示例15.5
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能：《最优状态估计-卡尔曼，H∞及非线性滤波》示例仿真
%示例15.5: ParticleEx5.m
%环境：Win7，Matlab2015b
%Modi: C.S
%时间：2022-05-02
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [StdErr, EKFErr] = ParticleEx5
% EKF Particle filter example.
% Track a body falling through the atmosphere.
% This system is taken from [Jul00], which was based on [Ath68].
% Compare the particle filter with the EKF particle filter.
global rho0 g k dt
rho0 = 2; % lb-sec^2/ft^4
g = 32.2; % ft/sec^2
k = 2e4; % ft
R = 10^4; % measurement noise variance (ft^2)
Q = diag([0 0 0]); % process noise covariance
M = 10^5; % horizontal range of position sensor
a = 10^5; % altitude of position sensor
P = diag([1e6 4e6 10]); % initial estimation error covariance
x = [3e5; -2e4; 1e-3]; % initial state
xhat = [3e5; -2e4; 1e-3]; % initial state estimate
N = 200; % number of particles
% Initialize the particle filter.
for i = 1 : N
xhatplus(:,i) = x + sqrt(P) * [randn; randn; randn]; % standard particle filter
xhatplusEKF(:,i) = xhatplus(:,i); % EKF particle filter
Pplus(:,:,i) = P; % initial EKF particle filter estimation error covariance
end
T = 0.5; % measurement time step
randn('state',sum(100*clock)); % random number generator seed
tf = 30; % simulation length (seconds)
dt = 0.5; % time step for integration (seconds)
xArray = x;
xhatArray = xhat;
xhatEKFArray = xhat;
for t = T : T : tf
fprintf('.');
% Simulate the system.
for tau = dt : dt : T
% Fourth order Runge Kutta ingegration
[dx1, dx2, dx3, dx4] = RungeKutta(x);
x = x + (dx1 + 2 * dx2 + 2 * dx3 + dx4) / 6;
x = x + sqrt(dt * Q) * [randn; randn; randn] * dt;
end
% Simulate the noisy measurement.
z = sqrt(M^2 + (x(1)-a)^2) + sqrt(R) * randn;
% Simulate the continuous-time part of the particle filters (time update).
xhatminus = xhatplus;
xhatminusEKF = xhatplusEKF;
for i = 1 : N
for tau = dt : dt : T
% Fourth order Runge Kutta ingegration
% standard particle filter
[dx1, dx2, dx3, dx4] = RungeKutta(xhatminus(:,i));
xhatminus(:,i) = xhatminus(:,i) + (dx1 + 2 * dx2 + 2 * dx3 + dx4) / 6;
xhatminus(:,i) = xhatminus(:,i) + sqrt(dt * Q) * [randn; randn; randn] * dt;
xhatminus(3,i) = max(0, xhatminus(3,i)); % the ballistic coefficient cannot be negative
% EKF particle filter
[dx1, dx2, dx3, dx4] = RungeKutta(xhatminusEKF(:,i));
xhatminusEKF(:,i) = xhatminusEKF(:,i) + (dx1 + 2 * dx2 + 2 * dx3 + dx4) / 6;
xhatminusEKF(:,i) = xhatminusEKF(:,i) + sqrt(dt * Q) * [randn; randn; randn] * dt;
xhatminusEKF(3,i) = max(0, xhatminusEKF(3,i)); % the ballistic coefficient cannot be negative
end
% standard particle filter
zhat = sqrt(M^2 + (xhatminus(1,i)-a)^2);
vhat(i) = z - zhat;
% EKF particle filter
zhatEKF = sqrt(M^2 + (xhatminusEKF(1,i)-a)^2);
F = [0 1 0; -rho0 * exp(-xhatminusEKF(1,i)/k) * xhatminusEKF(2,i)^2 / 2 / k * xhatminusEKF(3,i) ...
rho0 * exp(-xhatminusEKF(1,i)/k) * xhatminusEKF(2,i) * xhatminusEKF(3,i) ...
rho0 * exp(-xhatminusEKF(1,i)/k) * xhatminusEKF(2,i)^2 / 2; ...
0 0 0];
H = [(xhatminusEKF(1,i) - a) / sqrt(M^2 + (xhatminusEKF(1,i)-a)^2) 0 0];
Pminus(:,:,i) = F * Pplus(:,:,i) * F' + Q;
K = Pminus(:,:,i) * H' * inv(H * Pminus(:,:,i) * H' + R);
xhatminusEKF(:,i) = xhatminusEKF(:,i) + K * (z - zhatEKF);
zhatEKF = sqrt(M^2 + (xhatminusEKF(1,i)-a)^2);
vhatEKF(i) = z - zhatEKF;
end
% Note that we need to scale all of the q(i) probabilities in a way
% that does not change their relative magnitudes.
% Otherwise all of the q(i) elements will be zero because of the
% large value of the exponential.
% standard particle filter
vhatscale = max(abs(vhat)) / 4;
qsum = 0;
for i = 1 : N
q(i) = exp(-(vhat(i)/vhatscale)^2);
qsum = qsum + q(i);
end
% Normalize the likelihood of each a priori estimate.
for i = 1 : N
q(i) = q(i) / qsum;
end
% EKF particle filter
vhatscaleEKF = max(abs(vhatEKF)) / 4;
qsumEKF = 0;
for i = 1 : N
qEKF(i) = exp(-(vhatEKF(i)/vhatscaleEKF)^2);
qsumEKF = qsumEKF + qEKF(i);
end
% Normalize the likelihood of each a priori estimate.
for i = 1 : N
qEKF(i) = qEKF(i) / qsumEKF;
end
% Resample the standard particle filter
for i = 1 : N
u = rand; % uniform random number between 0 and 1
qtempsum = 0;
for j = 1 : N
qtempsum = qtempsum + q(j);
if qtempsum >= u
xhatplus(:,i) = xhatminus(:,j);
xhatplus(3,i) = max(0,xhatplus(3,i)); % the ballistic coefficient cannot be negative
break;
end
end
end
% The standard particle filter estimate is the mean of the particles.
xhat = mean(xhatplus')';
% Resample the EKF particle filter
Ptemp = Pplus;
for i = 1 : N
u = rand; % uniform random number between 0 and 1
qtempsum = 0;
for j = 1 : N
qtempsum = qtempsum + qEKF(j);
if qtempsum >= u
xhatplusEKF(:,i) = xhatminusEKF(:,j);
xhatplusEKF(3,i) = max(0,xhatplusEKF(3,i)); % the ballistic coefficient cannot be negative
Pplus(:,:,i) = Ptemp(:,:,j);
break;
end
end
end
% The EKF particle filter estimate is the mean of the particles.
xhatEKF = mean(xhatplusEKF')';
% Save data for plotting.
xArray = [xArray x];
xhatArray = [xhatArray xhat];
xhatEKFArray = [xhatEKFArray xhatEKF];
end
close all;
t = 0 : T : tf;
figure;
semilogy(t, abs(xArray(1,:) - xhatArray(1,:)), 'b-'); hold;
semilogy(t, abs(xArray(1,:) - xhatEKFArray(1,:)), 'r:');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('Altitude Estimation Error');
legend('Standard particle filter', 'EKF particle filter');
figure;
semilogy(t, abs(xArray(2,:) - xhatArray(2,:)), 'b-'); hold;
semilogy(t, abs(xArray(2,:) - xhatEKFArray(2,:)), 'r:');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('Velocity Estimation Error');
legend('Standard particle filter', 'EKF particle filter');
figure;
semilogy(t, abs(xArray(3,:) - xhatArray(3,:)), 'b-'); hold;
semilogy(t, abs(xArray(3,:) - xhatEKFArray(3,:)), 'r:');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('Ballistic Coefficient Estimation Error');
legend('Standard particle filter', 'EKF particle filter');
figure;
plot(t, xArray(1,:));
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('True Position');
figure;
plot(t, xArray(2,:));
title('Falling Body Simulation', 'FontSize', 12);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds');
ylabel('True Velocity');
for i = 1 : 3
StdErr(i) = sqrt((norm(xArray(i,:) - xhatArray(i,:)))^2 / tf / dt);
EKFErr(i) = sqrt((norm(xArray(i,:) - xhatEKFArray(i,:)))^2 / tf / dt);
end
disp(['Standard particle filter RMS error = ', num2str(StdErr)]);
disp(['EKF particle filter RMS error = ', num2str(EKFErr)]);
function [dx1, dx2, dx3, dx4] = RungeKutta(x)
% Fourth order Runge Kutta integration for the falling body system.
global rho0 g k dt
dx1(1,1) = x(2);
dx1(2,1) = rho0 * exp(-x(1)/k) * x(2)^2 / 2 * x(3) - g;
dx1(3,1) = 0;
dx1 = dx1 * dt;
xtemp = x + dx1 / 2;
dx2(1,1) = xtemp(2);
dx2(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g;
dx2(3,1) = 0;
dx2 = dx2 * dt;
xtemp = x + dx2 / 2;
dx3(1,1) = xtemp(2);
dx3(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g;
dx3(3,1) = 0;
dx3 = dx3 * dt;
xtemp = x + dx3;
dx4(1,1) = xtemp(2);
dx4(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g;
dx4(3,1) = 0;
dx4 = dx4 * dt;
return;
>> ParticleEx5
............................................................已锁定最新绘图
已锁定最新绘图
已锁定最新绘图
Standard particle filter RMS error = 306499.5143 29128.98582 0.6480721314
EKF particle filter RMS error = 1061.23451 29485.8123 0.408172791
ans =
1.0e+05 *
3.0650 0.2913 0.0000
6. 小结
运动控制在现代生活中的实际应用非常广泛，除了智能工厂中各种智能设备的自动运转控制，近几年最火的自动驾驶技术，以及航空航天领域，都缺少不了它的身影，所以熟练掌握状态估计理论，对未来就业也是非常有帮助的。切记矩阵理论与概率论等知识的基础一定要打好。对本章内容感兴趣或者想充分学习了解的，建议去研习书中第十五章节的内容，有条件的可以通过习题的联系进一步巩固充实。后期会对其中一些知识点在自己理解的基础上进行讨论补充，欢迎大家一起学习交流。
原书链接：Optimal State Estimation:Kalman, H-infinity, and Nonlinear Approaches
699
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
