trucktrailer.m

基于T-s模糊模型用于估计非线性系统的状态的kalman滤波器。

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)

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)
      flops(0);
      [xhat, P] = Kalman(xhat, y, sigmaX, sigmaY, P);
      Flop = Flop + flops;
   else 
      flops(0);
   	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;
      Flop = Flop + flops;
	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)
      flops(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);
      Flop = Flop + flops;
   end
   if (t == tf), break, end;
	xArray = [xArray x];
	yArray = [yArray y];
end
Flop

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)');

figure;
plot(tArray, xArray(3,:), 'k');
xlabel('time (seconds)');
ylabel('trailer position (meters)');

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

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

figure;
plot(tArray, xArray(3,:)-xhatArray(3,:), 'k-', tArray, xArray(3,:)-yArray(3,:), 'k:');
xlabel('time (seconds)');
ylabel('trailer position error (meters)');