弃疗取快递233

SimTests/GetState.m

@@ -0,0 +1,25 @@
+function out = GetState(x,P)
+    nx      = x(1)
+    nz      = x(2);
+    ny      = x(3);
+    alpha   = x(4);
+    beta    = x(5);
+
+    du       = x(6);
+    w       = x(7);
+    q       = x(8);
+    theta   = x(9);
+    v       = x(10);
+    p       = x(11);
+    r       = x(12);
+    phi     = x(13);
+    psi     = x(14);
+
+    pn      = x(15);
+    pe      = x(16);
+    pd      = x(17);
+
+    Va=norm([u+P.u0,v,w]);
+
+    out=[nx;ny;nz;p;q;r;phi;theta;psi;du;v;w;u;Va;pn;pe;pd];
+end

SimTests/gps.m

@@ -0,0 +1,59 @@
+function [sys,X0,str,ts,simStateCompliance] = gps(t,x,u,flag,P)
+    switch flag
+        case 0 %initialize
+            [sys,X0,str,ts,simStateCompliance]=mdlInitializeSizes(P);
+        case 1 %derivatives
+            sys=mdlDerivatives(t,x,u,P.mass,P.Jx,P.Jy,P.Jz,P.Jxz);
+        case 2 %update
+            sys=mdlUpdate(t,x,u);
+        case 3 %output
+            sys=mdlOutputs(t,x,u);
+        case 4 %get time of next var hit
+            sys=mdlGetTimeOfNextVarHit(t,x,u);
+        case 9 %terminate
+            sys=mdlTerminate(t,x,u);
+        otherwise
+            DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));     
+    end
+end
+
+function [sys,X0,str,ts,simStateCompliance]=mdlInitializeSizes(P)
+    sizes = simsizes;
+
+    sizes.NumContStates  = 3;
+    sizes.NumDiscStates  = 0;
+    sizes.NumOutputs     = 3;
+    sizes.NumInputs      = 9;
+    sizes.DirFeedthrough = 0;
+    sizes.NumSampleTimes = 1;   % at least one sample time is needed
+
+    sys = simsizes(sizes);    
+    X0  = P.x0;
+    str = [];
+
+    ts  = [0 0];
+
+    simStateCompliance = 'UnknownSimState';
+end
+
+function sys=mdlDerivatives(t,x,uu)
+
+    sys = [pndot; pedot; pddot];
+end
+
+function sys=mdlUpdate(t,x,u)
+    sys=[];
+end
+
+function sys=mdlOutputs(t,x,u)
+    sys=x;
+end
+
+function sys=mdlGetTimeOfNextVarHit(t,x,u)
+    sampleTime=1;
+    sys=t+sampleTime;
+end
+
+function sys=mdlTerminate(t,x,u)
+    sys=[];
+end

param.m

@@ -7,15 +7,6 @@
     0,0,0,0,0.0863470,0.0124572,-0.735554,0,0;...
     0,0,0,0,0,1,0.0386064,0,0;...
     0,0,0,0,0,0,1.00074,0,0];
-% A=[0.0153803,0.0439192,-7.83906,-9.80253,0,0,0,0,0;...
-%     -0.0438131,0.52042,203.051,-0.378441,0,0,0,0,0;...
-%     0.00123923,0.000621593,-0.003074,0,0,0,0,0,0;...
-%     0,0,1.0,0,0,0,0,0,0;...
-%     0,0,0,0,-0.456379,7.83906,-203.051,9.80253,0;...
-%     0,0,0,0,-0.137907,-2.99727,2.13345,0,0;...
-%     0,0,0,0,0.0863470,0.0124572,-0.735554,0,0;...
-%     0,0,0,0,0,1,0.0386064,0,0;...
-%     0,0,0,0,0,0,1.00074,0,0];
 
 B=[2.07005,0,0,9.57054;...
     -88.0560,0,0,0;...