truck.m

来自「a collection of M-files to study concept」· M 代码 · 共 35 行

function [zdot] = truck(z,theta)
% TRUCK: The nonlinear model for Truck Backer-Upper Control is given. 
%	 The inputs of function are vector of states of system and 
%	 steering angle THETA. The output of function is vector of
%	 the next sampling time states Z_DOT.
%
%                      [Z_DOT] = TRUCK(Z,THETA)
%
%        Where state vector Z is definde as Z(t)=[X(t) Y(t) PHI(t)]'
%
%          X(t): X position of truck in yard 
%          Y(t): Y position of truck in yard 
%          PHI(t): Azimuth angle of truck

% FISMAT: Fuzzy Inference Systems toolbox for MATLAB
% (c) A. Lotfi, University of Queensland (Email: [e-mail])
% 13-10-93
% The program has been tested on MATLAB version 4.1, Sun workstation.

theta=theta*pi/180;
z(3)=z(3)*pi/180;

b=4;
r=2;

zdot(1)=z(1)+ r* (cos(z(3)+theta) + sin(theta)*sin(z(3)));

zdot(2)=z(2)+ r* (sin(z(3)+theta) - sin(theta)*cos(z(3)));

zdot(3)=(180/pi) * (z(3)- r*asin((2*sin(theta))/b));


zdot=zdot(:);