dongliangjufc.m

LQR源程序

function [sys,x0,str,ts] = dongliangjufc(t,x,u,flag)
% CSFUNC An example M-file S-function for defining a system of
% continuous state equations:

% Dispatch the flag.
% 

switch flag,

  case 0
    [sys,x0,str,ts]=mdlInitializeSizes;% Initialization

  case 1
    sys = mdlDerivatives(t,x,u); % Calculate derivatives

  case 3
    sys = mdlOutputs(t,x,u); % Calculate outputs
  
  case { 2, 4, 9 } % Unused flags
    sys = [];
otherwise
    error(['Unhandled flag = ',num2str(flag)]); % Error handling
end
% End of csfunc.
%==============================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the 
% S-function.
%==============================================================
%
function [sys,x0,str,ts] = mdlInitializeSizes
% Call simsizes for a sizes structure, fill it in and convert it 
% to a sizes array.
%
sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 3;
sizes.NumInputs      = 24;
sizes.DirFeedthrough = 1;     % Matrix D is nonempty. 
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
%
% Initialize the initial conditions.
%
x0 = [];
%
% str is an empty matrix.
%
str = [];
%
% Initialize the array of sample times; in this example the sample 
% time is continuous, so set ts to 0 and its offset to 0.
%
ts = [0 0];
A=zeros(3,3);
B=zeros(3,3);
% End of mdlInitializeSizes.
%
%==============================================================
% mdlDerivatives
% Return the derivatives for the continuous states.
%==============================================================
function sys = mdlDerivatives(t,x,u)


% End of mdlDerivatives.
%
%==============================================================
% mdlOutputs
% Return the block outputs.
%==============================================================
%
function sys = mdlOutputs(t,x,u)
u1=u(1);
v1=u(2);
w1=u(3);
o=u(4);
v=u(5);
w=u(6);
p1=u(7);
q1=u(8);
r1=u(9);
p=u(10);
q=u(11);
r=u(12);
Fx=u(13);
Fy=u(14);
Fz=u(15);
m= u(16);
Dm=u(17);
Mx=u(18);
My=u(19);
Mz=u(20);
I1=u(21);
I2=u(22);
I3=u(23);
P1=-((I3-I2)*q*r)+Mx;
Q1=-((I1-I3)*p*r)+My;
R1=-((I2-I1)*q*p)+Mz;
B=[I1 0 0;0 I2 0;0 0 I3];
A=inv(B);
sys(1)=A(1,1)*P1+A(1,2)*Q1+A(1,3)*R1;
sys(2)=A(2,1)*P1+A(2,2)*Q1+A(2,3)*R1;
sys(3)=A(3,1)*P1+A(3,2)*Q1+A(3,3)*R1;