untitled2.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];


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
T=size(X2321,1);
x = zeros(n_x,T);
z = ones(1,T);
y = zeros(n_y,T);
u = zeros(n_u,T);           % Control signals.

u(1,:)=X2321(:,16)'/10; % 左轮逻辑速度（mm/s ）
u(2,:)=X2321(:,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(:,12)'/10;%左轮实速度（mm/s ）
y(2,:)=X2321(:,14)'/10;%right轮实速度（mm/s ）
y(3,:)=X2321(:,22)'/1000;