modellinear.m

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

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

% function [x, y, xhat] = ModelLinear(x, u, xhat, sigmaX, sigmaY)
%
% Simulate the linear model of the truck-trailer system.
% INPUTS
%   x = the present 3-dimensional state vector
%   u = the present scalar control
%   xhat = the present state estimate
%   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)
% OUTPUTS
%   x = the state at the next time step
%   y = the measurement at the next time step
%   xhat = the state estimate at the next time step, before the measurement is processed

% Compute the system matrices and membership function values of the T-S models.
[A1, A2, B1, B2, h1, h2] = FuzzyModel(x);

% Compute the global system matrices.
A = h1 * A1 + h2 * A2;
B = h1 * B1 + h2 * B2;

% Compute the next state.
noise(1,1) = sigmaX(1)^2 * randn;
noise(2,1) = sigmaX(2)^2 * randn;
noise(3,1) = sigmaX(3)^2 * randn;
x = A * x + B * u + noise;

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

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