trucktrailer.asv

KALMAN FILTERING FOR FUZZY DYNAMIC SYSTEMS

%function [ErrEstimate, ErrMeasurement] = TruckTrailer(sigmaX, sigmaY, fOptimal, fPlot)

% function [ErrEstimate, ErrMeasurement] = TruckTrailer(sigmaX, sigmaY, fOptimal, fPlot)
%
% Simulate the truck-trailer system.
% INPUTS
%   sigmaX = 3-element vector containing the standard deviation of 
%            the state process noise (rad, rad, velocity)
%   sigmaY = 3-element vector containing the standard deviation of
%            the measurement noise (rad, rad, velocity)
%   fOptimal = flag indicating if we should use the optimal uncoupled
%              Kalman filter.  If 1, then we use the optimal 
%              Kalman filter.  If 0, then we use the suboptimal decoupled
%              Kalman filter.
%   fPlot = flag indicating if we should plot the results.
%           If 1, then plot.  If 0, then do not plot.
% OUTPUTS
%   ErrEstimate = state estimation error (RMS)
%   ErrMeasurement = measurement error (RMS)
clear;clc;     %060602乐加
sigmaX=[1;2;3];sigmaY=[1;2;3];fOptimal=0;      %060602乐加
tf = 120; % duration of simulation (seconds)

% Set the initial state vector.
x = [-pi/4; -pi/4; -5];
x = [-pi/4; pi/4; -5];
x = [pi/4; -pi/4; -5];
x = [pi/4; pi/4; -5];
xArray = [x];

% Set the noise covariance matrices.
Sw = diag(sigmaX)^2;
Sv = diag(sigmaY)^2;

% Compute the initial state measurement.
noise(1,1) = sigmaY(1)^2 * randn;
noise(2,1) = sigmaY(2)^2 * randn;
noise(3,1) = sigmaY(3)^2 * randn;
y = x + noise;
yArray = [y];

% Compute the initial global and local state estimates.
xhat = x;
xhatArray = [];
[A1, A2, B1, B2, h1, h2] = FuzzyModel(x);
xhat1 = h1 * x;
xhat2 = h2 * x;

% Compute the Algebraic Ricatti Equation solutions for the 
% optimal controllers and the Kalman filters for the local systems.
[pi1, pi2, P1, P2] = ARESolutions(Sw, Sv);

% Initialize the state estimate covariance.
P = diag(sigmaY)^2;

Flop = 0;
for t = 0 : tf
   % Compute the state estimate at the present time.
   if (fOptimal == 1)
      [xhat, P] = Kalman(xhat, y, sigmaX, sigmaY, P);
  else 
   	y1 = h1 * y;
	   xhat1 = xhat1 + P1 * inv(P1 + Sv) * (y1 - xhat1);
	   y2 = h2 * y;
	   xhat2 = xhat2 + P2 * inv(P2 + Sv) * (y2 - xhat2);
      xhat = xhat1 + xhat2;
  end   
   xhatArray = [xhatArray xhat];
   % Compute the optimal control
   u = Controller(xhat, pi1, pi2);
   % Extrapolate the state and state estimate to the next time.
  	[x, y, xhat] = ModelNonlinear(x, u, xhat, sigmaX, sigmaY);
   if (fOptimal == 0)
	   xhat = xhat1 + xhat2;
		[A1, A2, B1, B2, h1, h2] = FuzzyModel(xhat);
		xhat1 = A1 * xhat1 + h1 * B1 * u;
	   xhat2 = A2 * xhat2 + h2 * B2 * u;
	   xhat = xhat1 + xhat2;
      [A1, A2, B1, B2, h1, h2] = FuzzyModel(xhat);
  end
   if (t == tf), break, end;
	xArray = [xArray x];
	yArray = [yArray y];
end
Flop = 1;

tArray = 0 : tf;

xArray(1:2,:) = 180 / pi * xArray(1:2,:);
xhatArray(1:2,:) = 180 / pi * xhatArray(1:2,:);
yArray(1:2,:) = 180 / pi * yArray(1:2,:);

ErrEstimate = xArray - xhatArray ;
ErrEstimate = std(ErrEstimate');

ErrMeasurement = xArray - yArray;
ErrMeasurement = std(ErrMeasurement');

% if (fPlot == 0), return, end;                     %注释掉以后有图

close all; % close all open figures

% figure;
% plot(tArray, xArray(1,:), 'k');
% xlabel('time (seconds)');
% ylabel('truck angle (degrees)');

% figure;
% plot(tArray, xArray(2,:), 'k');
% xlabel('time (seconds)');
% ylabel('trailer angle (degrees)');

% xArray3 = xArray(3,:);
%   fid = fopen('xArray3.txt','w');
%   fprintf(fid,'%7.3f\r',xArray3);
%   fclose(fid);
% figure;
% plot(tArray, xArray3, 'k');
% xlabel('时间 (秒)');grid on;
%ylabel('trailer position (meters)');

  xhat1 = xArray(1,:)-xhatArray(1,:);
  xhat2 = xArray(1,:)-yArray(1,:);
  fid = fopen('xhat1.txt','w');
  fprintf(fid,'%7.3f\r',xhat1);
  fclose(fid);
  fid = fopen('xhat2.txt','w');
  fprintf(fid,'%7.3f\r',xhat2);
  fclose(fid);
figure;
plot(tArray, xArray(1,:)-xhatArray(1,:), 'k-', tArray, xArray(1,:)-yArray(1,:), 'k:');
xlabel('时间 (秒)');ylabel()grid on;
% ylabel('truck angle error (degrees)');

figure;
plot(tArray, xArray(2,:)-xhatArray(2,:), 'k-', tArray, xArray(2,:)-yArray(2,:), 'k:');
xlabel('时间 (秒)');grid on;
% ylabel('trailer angle error (degrees)');

figure;
plot(tArray, xArray(3,:)-xhatArray(3,:), 'k-', tArray, xArray(3,:)-yArray(3,:), 'k:');
xlabel('时间 (秒)');grid on;
% ylabel('trailer position error (meters)');

% figure;
% plot(tArray, xArray(1,:)-xhatArray(1,:), 'k-');
% xlabel('time (seconds)');
% ylabel('truck angle error (degrees)');

% figure;
% plot(tArray, xArray(2,:)-xhatArray(2,:), 'k-');
% xlabel('time (seconds)');
% ylabel('trailer angle error (degrees)');

% xhat3 = xArray(3,:)-xhatArray(3,:);
%   fid = fopen('xhat.txt','w');
%   fprintf(fid,'%7.3f\r',xhat3);
%   fclose(fid);
% figure;
% plot(tArray, xArray(3,:)-xhatArray(3,:), 'k-');
% xlabel('时间 (秒)');grid on;
%ylabel('trailer position error (meters)');

% figure;
% plot(tArray, xArray(1,:)-yArray(1,:), 'k-');
% xlabel('time (seconds)');
% ylabel('truck angle error (degrees)');

% figure;
% plot(tArray, xArray(2,:)-yArray(2,:), 'k-');
% xlabel('time (seconds)');
% ylabel('trailer angle error (degrees)');

% yArray3 = xArray(3,:)-yArray(3,:);
%   fid = fopen('yArray3.txt','w');
%   fprintf(fid,'%7.3f\r',yArray3);
%   fclose(fid);
% figure;
% plot(tArray, xArray(3,:)-yArray(3,:), 'k-');
% xlabel('时间 (秒)');grid on;
%ylabel('trailer position error (meters)');

% figure;
% subplot(3,1,1),plot(tArray, xArray3, 'k');xlabel('时间 (秒)');grid on;
% subplot(3,1,2),plot(tArray, xArray(3,:)-xhatArray(3,:), 'k-');xlabel('时间 (秒)');grid on;
% subplot(3,1,3),plot(tArray, xArray(3,:)-yArray(3,:), 'k-');xlabel('时间 (秒)');grid on;