pf2.m

粒子滤波的各种算法,扩展卡而满滤波,

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].  这个系统来自[Jul00] 
，基于[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  从0.5秒开始到30秒结束，步幅为0.5秒。
    fprintf('.');  用点来表示
    % Simulate the system. 仿真这个系统
    for tau = dt : dt : T  从0.04到0.5秒，步幅为0.04秒。
        % 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 
       需要注意的是，我们需要用一种不改变他们相对量级的方法来测量q(i)的所有可能值，否则所有的q(i)的值将变为0，因为指数的大值。
%  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');