pf_demo4.m

自己编写的粒子滤波的源码,用于故障诊断,是MATLAB语言写的,

% 正常-〉右轮故障  clear;clc;load NEW_dataset.mat;
clear;
echo off;

% =======================================================================
%              INITIALISATION AND PARAMETERS
% =======================================================================

N = 100;                     % Number of particles.
T = 301;                     % Number of time steps. steps: from 400 to 700

% Here, we give you the choice to try three different types of
% resampling algorithms: multinomial (select 3), residual (1) and 
% deterministic (2). Note that the code for these O(N) algorithms is generic.

resamplingScheme = 2;    

n_x = 3;                    % Continuous state dimension.
n_z = 3;                    % Number of discrete states.
n_y = 3;                    % Dimension of observations.
n_u = 2; %Dimension of input


par.A = zeros(n_x,n_x,n_z);
par.B = zeros(n_x,n_x,n_z);
par.C = zeros(n_y,n_x,n_z);
par.D = zeros(n_y,n_y,n_z);
%par.E = zeros(n_x,n_x,n_z);
par.F = zeros(n_x,n_u,n_z);
par.G = zeros(n_y,n_u,n_z);

a=600;% distance between wheels mm
factor=1.24;%factor computed from experience


for i=1:n_z,
%  par.A(:,:,i) = i*randn(n_x,n_x);
%  par.C(:,:,i) = i*randn(n_y,n_x);
%  par.B(:,:,i) = 0.01*eye(n_x,n_x);    
  par.D(:,:,i) = [0.1,0,0;0,0.1,0;0,0,0.0223]; 
  par.F(:,:,i) = [1,0;0,1;-180/(a*factor*pi),180/(a*factor*pi)];
%  par.G(:,:,i) = (1/n_y)*zeros(n_y,n_u);   
end;
par.C(:,:,1)=eye(3,3);% s0
par.C(:,:,2)=[0,0,0;0,1,0;0,0,1];%s1
par.C(:,:,3)=[1,0,0;0,0,0;0,0,1];%s2
%par.C(:,:,4)=[1,0,0;0,1,0;0,0,0];%s3



%par.T = unidrnd(10,n_z,n_z);           % Transition matrix.
%for i=1:n_z,
%  par.T(i,:) = par.T(i,:)./sum(par.T(i,:)); 
%end;
par.T = zeros(n_z,n_z);           % Transition matrix.
par.T=[0.34,0.33,0.33;0.33,0.34,0.33;0.33,0.33,0.34];

% which unidrnd
par.pz0 = unidrnd(10,n_z,1);            % Initial discrete distribution. 
par.pz0 = par.pz0./sum(par.pz0); 
par.mu0 = zeros(n_x,1);                 % Initial Gaussian mean.
par.S0  = [0.01,0,0;0,0.01,0;0,0,0.0005];             % Initial Gaussian covariance.  


% =======================================================================
%                          GENERATE THE DATA
% =======================================================================
% get the data from mat file;
load 2321.txt; % load raw data from data file
x = zeros(n_x,T);
z = ones(1,T);
y = zeros(n_y,T);
u = zeros(n_u,T);           % Control signals.

u(1,:)=X2321(400:700,16)'/10; % 左轮逻辑速度（mm/s ）
u(2,:)=X2321(400:700,18)'/10;%右轮逻辑速度（mm/s ）


x(1,:)=u(1,:);
x(2,:)=u(2,:);
x(3,:)=(u(2,:)-u(1,:))*180/(a*factor*pi);
y(1,:)=X2321(400:700,12)'/10;%左轮实速度（mm/s ）
y(2,:)=X2321(400:700,14)'/10;%right轮实速度（mm/s ）
y(3,:)=X2321(400:700,22)'/1000;

%print the raw data u1(x1) u2(x2) x3 y1 y2 y3
figure(1)
clf
subplot(321)
plot(1:T,x(1,:),'r','linewidth',2);
ylabel('left control input','fontsize',10);
grid on;
subplot(322)
plot(1:T,x(2,:),'r','linewidth',2);
ylabel('right control input','fontsize',10);
grid on;
subplot(323)
plot(1:T,y(1,:),'r','linewidth',2);
ylabel('left encoder','fontsize',10);
grid on;
subplot(324)
plot(1:T,y(2,:),'r','linewidth',2);
ylabel('right encoder','fontsize',10);
grid on;
subplot(325)
plot(1:T,y(3,:),'r','linewidth',2);
ylabel('gyroscope','fontsize',10);
grid on;
subplot(326)
%discreate state 
plot(1:T,y(3,:),'r','linewidth',2);
ylabel('y3','fontsize',10);
grid on;


fprintf('\n')
fprintf('\n')
fprintf('Estimation has started')
fprintf('\n')


% =======================================================================
%                              PF ESTIMATION
% =======================================================================


% INITIALISATION:
% ==============
z_pf = ones(1,T,N);            % These are the particles for the estimate
                               % of z. 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.
z_pf_pred = ones(1,T,N);       % One-step-ahead predicted values of z.
x_pf = 10*randn(n_x,T,N);      % These are the particles for the estimate x.
x_pf_pred = x_pf;  
y_pred = 10*randn(n_y,T,N);    % One-step-ahead predicted values of y.
w = ones(T,N);                 % Importance weights.
%initz = 1/n_z*ones(1,n_z); 
initz = [0.5,0.25,0.25];    
for i=1:N,
  z_pf(:,1,i) = length(find(cumsum(initz')<rand))+1; 
end;

disp(' ');
tic;                  % Initialize timer for benchmarking

for t=2:T,    
  fprintf('PF :  t = %i / %i  \r',t,T); fprintf('\n');  

  % SEQUENTIAL IMPORTANCE SAMPLING STEP:
  % =================================== 
  for i=1:N,   % 预测 x,z 的 估计值
    % sample z(t)~p(z(t)|z(t-1)) 
    z_pf_pred(1,t,i) = length(find(cumsum(par.T(z_pf(1,t-1,i),:)')<rand))+1;
    % sample x(t)~p(x(t)|z(t|t-1),x(t-1))
    %fprintf('z_pf_pred(1,t,i)=%d  \r',z_pf_pred(1,t,i)); fprintf('\n');  
    x_pf_pred(:,t,i) = par.A(:,:,z_pf_pred(1,t,i)) * x_pf(:,t-1,i) + ...
                       par.B(:,:,z_pf_pred(1,t,i))*randn(n_x,1) + ...
                       par.F(:,:,z_pf_pred(1,t,i))*u(:,t); % 引用控制输入 u
  end;
  % Evaluate importance weights.
  for i=1:N,   % 更新(纠正)偏差,并选择性取样
    y_pred(:,t,i) =  par.C(:,:,z_pf_pred(1,t,i)) * x_pf_pred(:,t,i) + ...
                     par.G(:,:,z_pf_pred(1,t,i))*u(:,t);  % 引用控制输入 u
    Cov = par.D(:,:,z_pf_pred(1,t,i))*par.D(:,:,z_pf_pred(1,t,i))'; 
    w(t,i) =  (det(Cov)^(-0.5))*exp(-0.5*(y(:,t)-y_pred(:,t,i))'* ...% 引用观测数据 y
				    pinv(Cov)*(y(:,t)-y_pred(:,t,i))) + 1e-99;% 引用观测数据 y
  end;  % which pinv
  w(t,:) = w(t,:)./sum(w(t,:));       % Normalise the weights.

  
  % SELECTION STEP: 选择性取样
  % ===============
  if resamplingScheme == 1
    outIndex = residualR(1:N,w(t,:)');        % Higuchi and Liu.
  elseif resamplingScheme == 2
    outIndex = deterministicR(1:N,w(t,:)');   % Kitagawa.
  else  
    outIndex = multinomialR(1:N,w(t,:)');     % Ripley, Gordon, etc.  
  end;
  z_pf(1,t,:) = z_pf_pred(1,t,outIndex);
  x_pf(:,t,:) = x_pf_pred(:,t,outIndex);

end;   % End of t loop.

time_pf = toc;     % How long did this take?




z_plot_pf = zeros(T,N);

for t=1:T,
  z_plot_pf(t,:) = z_pf(1,t,:);
end;

z_num_pf = zeros(T,n_z);
z_max_pf = zeros(T,1);

for t=1:T,
  for i=1:n_z,
    z_num_pf(t,i)= length(find(z_plot_pf(t,:)==i));
  end;
  [arb,z_max_pf(t)] = max(z_num_pf(t,:));  
end;


figure(1)
subplot(326)
%discreate state 
plot(1:T,z_max_pf,'b--','linewidth',2);
ylabel('state estimate','fontsize',10);
axis([0 T+1 0 4])
legend('1:normal 3:right encoder fault');
grid on;

figure(2) 
clf
%plot(1:T,z,'k',1:T,z,'ko',1:T,z_max_rbpf,'r+',1:T,z_max_pf,'bv','linewidth',1);
%legend('','True state','RBPF MAP estimate','PF MAP estimate');
plot(1:T,z_max_pf,'b-','linewidth',2);
legend('PF MAP estimate');
axis([0 T+1 0.5 n_z+0.5])