《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波

《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波

  • 前言
  • 1. MATLAB仿真:示例15.1
  • 2. MATLAB仿真:示例15.2
  • 3. MATLAB仿真:示例15.3
  • 4. MATLAB仿真:示例15.4
  • 5. MATLAB仿真:示例15.5
  • 6. 小结

前言

《最优状态估计-卡尔曼,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)]);

《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第1张图片
《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第2张图片
《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第3张图片

>> 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');

《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第4张图片
《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第5张图片
《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第6张图片
《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第7张图片
《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第8张图片

>> ParticleEx2
............................................................已锁定最新绘图
已锁定最新绘图
已锁定最新绘图

ans =

   1.0e+05 *

  1113.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

  12221.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

  23330.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

  34440.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

  45550.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

  56610.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)]);

《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第9张图片
《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第10张图片
《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第11张图片

>> 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;

《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第12张图片
《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第13张图片
《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第14张图片
《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第15张图片
《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第16张图片

>> 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;

《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第17张图片
《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第18张图片
《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第19张图片

《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第20张图片
《最优状态估计-卡尔曼,H∞及非线性滤波》:第15章 粒子滤波_第21张图片

>> 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

你可能感兴趣的:(SC3:,最优状态估计,粒子滤波,最优状态估计,运动控制,MATLAB仿真,Particle,filter)