www.pudn.com > PF_Code.rar > Untitled2.m

 
% 正常-〉右轮故障  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;