modelnonlinear.m

KALMAN FILTERING FOR FUZZY DYNAMIC SYSTEMS

function [x, y, xhat] = ModelNonlinear(x, u, xhat, sigmaX, sigmaY)

% function [x, y, xhat] = ModelNonlinear(x, u, xhat, sigmaX, sigmaY)
%
% Simulate the nonlinear model of the truck-trailer system.
% INPUTS
%   x is the 3-dimensional state.
%   u is the scalar control.
%   xhat is the state estimate.
%   sigmaX is the standard deviation of the process noise.
%   sigmaY is the standard deviation of the measurement noise.
% OUTPUTS
%   x is the state at the next time step.
%   y is the measurement at the next time step.
%   xhat is the state estimate at the next time step, before the measurement is processed.

ell = 2.8;   % truck length
L = 5.5;     % trailer length
v = -1;      % speed
tprime = .5; % sample time

% Compute the next state.
x(3) = x(3) + v * tprime * sin(x(2) + v * tprime / 2 / L * x(1));
x(2) = x(2) + v * tprime / L * x(1);
x(1) = (1 - v * tprime / L) * x(1) + v * tprime / ell * u;

% Compute the system matrices of the T-S model.
[A1, A2, B1, B2, h1, h2] = FuzzyModel(x);
A = h1 * A1 + h2 * A2;
B = h1 * B1 + h2 * B2;

% Add process noise to the state.
noise(1,1) = sigmaX(1)^2 * randn;
noise(2,1) = sigmaX(2)^2 * randn;
noise(3,1) = sigmaX(3)^2 * randn;
x = x + noise;

% Compute the noisy 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;

% Extrapolate the state estimate.
xhat = A * xhat + B * u;