Add files via upload

AileronActuatorFailure.m

@@ -0,0 +1,20 @@
+function XDOT = AileronActuatorFailure(X, U, faultCondition, faultValue)
+    % Extract control input vector
+    d_A = U(1); % Aileron
+
+    % Simulate fault condition
+    switch faultCondition
+        case 'stuck'
+            d_A = faultValue; % Stuck at faultValue radians
+        case 'limitedRange'
+            d_A = max(min(d_A, faultValue), -faultValue); % Limit range to +/- faultValue
+        case 'unresponsive'
+            d_A = 0; % Aileron does not respond
+    end
+
+    % Update control input vector
+    U(1) = d_A;
+
+    % Calculate dynamics with modified control input
+    XDOT = RCAM_model(X, U);
+end

AirspeedSensorFailure.m

@@ -0,0 +1,217 @@
+function XDOT  = AirspeedSensorFailure(X, U, faultCondition)
+
+%-------- STATE AND CONTROL VECTORS --------%
+
+% extract state vector
+x1 = X(1); %u
+x2 = X(2); %v
+x3 = X(3); %w
+x4 = X(4); %p
+x5 = X(5); %q
+x6 = X(6); %r
+x7 = X(7); %phi
+x8 = X(8); %theta
+x9 = X(9); %psi
+
+% extract control input vector
+u1 = U(1); %d_A (aileron)
+u2 = U(2); %d_T (stabilizer)
+u3 = U(3); %d_R (rudder)
+u4 = U(4); %d_th1 (throttle 1)
+u5 = U(5); %d_th2 (throttle 2)
+
+%-------- CONSTANTS --------%
+% Nominal vehicle constants
+m = 120000; %kg
+
+cbar = 6.6;     %Mean Aerodynamic Chord (m)
+It = 24.8;      %Distance by AC of tail and body (m)
+S = 260;        %Wing planform area (m^2)
+St = 64;        %Tail planform area (m^2)
+
+Xcg = 0.23*cbar;        %x position of CoG in Fm (m)
+Ycg = 0;                %y position of CoG in Fm (m)
+Zcg = 0.10*cbar;        %z position of CoG in Fm (m)
+
+Xac = 0.12*cbar;        %x position of aerodynamic center in Fm (m)
+Yac = 0;                %y position of aerodynamic center in Fm (m)
+Zac = 0;                %z position of aerodynamic center in Fm (m)
+
+% Engine Constants
+
+Xapt1 = 0;              %x position of engine 1 force in Fm (m)
+Yapt1 = -7.94;          %y position of engine 1 force in Fm (m)
+Zapt1 = -1.9;           %z position of engine 1 force in Fm (m)
+
+Xapt2 = 0;              %x position of engine 2 force in Fm (m)
+Yapt2 = 7.94;           %y position of engine 2 force in Fm (m)
+Zapt2 = -1.9;           %z position of engine 2 force in Fm (m)
+
+% Other Constants
+rho = 1.225;                %Air density (kg/m^3)
+g = 9.81;                   %Gravitational accleration (m/s^2)
+depsda = 0.25;              %Change in downwash w.r.t alpha
+alpha_L0 = -11.5*pi/180;  %Zero lift angle of attach (rad)
+n = 5.5;                    %Slope of linear region of lift slope
+a3 = -768.5;                %Coeff of alpha^3
+a2 = 609.2;                %Coeff of alpha^2
+a1 = -155.2;                %Coeff of alpha^1
+a0 = 15.212;                %Coeff of alpha^0
+alpha_switch = 14.5*(pi/180);% alpha where lift slope goes from linear to non-linear
+
+
+%-------- 2. INTERMEDIATE VARIABLES --------%
+
+
+ % True airspeed calculation
+    Va_true = sqrt(x1^2 + x2^2 + x3^2);
+
+    % Simulate airspeed sensor failure based on faultCondition
+    switch faultCondition
+        case 'no_fault'
+            Va = Va_true;
+        case 'complete_failure'
+            Va = 0; % Complete failure
+        case 'partial_blockage'
+            Va = Va_true - 50; % Partial failure with constant offset
+        otherwise
+            Va = Va_true; % Default case handles unexpected faultCondition values
+    end
+
+alpha = atan2(x3, x1);         % Calculate alpha as Eq (5)
+beta = asin(x2/Va);            % Calculate beta as Eq (6)
+
+Q = 0.5*rho*Va^2;              % Calculate dynamic pressure as Eq (7)
+
+wbe_b = [x4;x5;x6];            % Define wbe_b as Eq (8)
+V_b = [x1;x2;x3];              % Define V_b as Eq (9)
+
+%-------- 3. AERODYNAMIC FORCE COEFFICIENTS --------%
+
+%calculate the CL_wb as Eq (10)
+if alpha<=alpha_switch 
+    CL_wb = n*(alpha - alpha_L0);
+else
+    CL_wb = a3*alpha^3 + a2*alpha^2 + a1*alpha + a0;
+end
+
+%calculate the CL_t
+epsilon = depsda * (alpha - alpha_L0);
+alpha_t = alpha - epsilon + u2 + 1.3*x5*It/Va;
+CL_t = 3.1* (St/S)*alpha_t; % Eq (12)
+
+%Total force
+CL = CL_wb + CL_t; %Eq (13)
+
+% Total drag force 
+CD = 0.13 + 0.07*(5.5*alpha + 0.654)^2; %Eq (15)
+
+% Calculate sideforce
+CY = -1.6*beta + 0.24*u3; %Eq (14)
+
+%-------- 4. DIMENSIONAL AERODYNAMIC FORCES --------%
+% Calculating actual dimensional forces in F_s 
+FA_s = [-CD*Q*S; CY*Q*S; -CL*Q*S]; %Eq (16)
+
+%Rotation matrix to F_b 
+C_bs = [cos(alpha) 0 -sin(alpha);
+    0 1 0;
+    sin(alpha) 0 cos(alpha)];
+FA_b = C_bs*FA_s; %Eq (17)
+
+%-------- 5. AERODYNAMIC MOMENT COEFF ABOUT AC --------%
+% Calculate moments in F_b.
+eta11 = -1.4*beta;
+eta21 = -0.59 - (3.1*(St*It)/(S*cbar))*(alpha - epsilon);
+eta31 = (1 - alpha* (180/(15*pi)))*beta;
+
+eta = [eta11;
+       eta21;
+       eta31];
+
+dCMdx = (cbar/Va) * [-11 0 5;   
+                    0 (-4.03*(St*It^2)/(S*cbar^2)) 0;
+                    1.7 0 -11.5];
+dCMdu = [-0.6 0 0.22;
+        0 (-3.1*(St*It)/(S*cbar)) 0;
+        0 0 -0.63];
+
+% Calculating CM = [Cl;CmlCn] about Aerodynamic center in F_b
+CMac_b = eta + dCMdx*wbe_b + dCMdu*[u1;u2;u3];
+
+%-------- 6. AERODYNAMIC MOMENT ABOUT AC --------%
+% Normalize to an aerodynamic moment
+MAac_b = CMac_b*Q*S*cbar;
+
+%-------- 7. AERODYNAMIC MOMENT ABOUT CG --------%
+% Transfer moment to CG
+rcg_b = [Xcg; Ycg; Zcg];
+rac_b = [Xac; Yac; Zac];
+MAcg_b = MAac_b + cross(FA_b, rcg_b - rac_b); %Eq (20)
+
+
+
+%-------- 8. ENGINE FORCE AND MOMENT --------%
+% thrust of each engine
+F1 = u4*m*g; % Eq(24)
+F2 = u5*m*g; % Eq(25)
+
+% engine thrust aligned with F_b:
+FE1_b = [F1; 0 ;0];
+FE2_b = [F2; 0 ;0];
+
+FE_b = FE1_b + FE2_b;
+
+% engine moment due to offset of engine thrust from CoG
+mew1 = [Xcg - Xapt1;
+        Yapt1 - Ycg;
+        Zcg - Zapt1];
+mew2 = [Xcg - Xapt2;
+        Yapt2 - Ycg;
+        Zcg - Zapt2];
+% Eq (26)
+MEcg1_b = cross(mew1, FE1_b);
+MEcg2_b = cross(mew2, FE2_b);
+
+MEcg_b = MEcg1_b + MEcg2_b;
+
+%-------- 9. GRAVITY EFEFCTS --------%
+% calculate gravitational forces in the body frame. this causes no moment
+% about CoG
+g_b = [-g*sin(x8);
+        g*cos(x8)*sin(x7);
+        g*cos(x8)*cos(x7)];
+Fg_b = m*g_b; % Eq(29)
+
+
+
+%-------- 10. STATE DERIVATION --------%
+%Inertia matrix
+Ib = m*[40.07 0 -2.0923;
+        0 64 0;
+        -2.0923 0 99.92];
+
+% Inverse of Inertia matrix
+InvIb = (1/m)* [0.0249836 0 0.000523151;
+        0 0.015625 0;
+        0.000523151 0 0.010019];
+
+% Form F_b and calculate udot, vdot, wdot
+F_b = Fg_b + FE_b + FA_b;
+x1to3dot = (1/m)*F_b - cross(wbe_b, V_b);
+
+% Form Mcg_b and calculate pdot, qdot, rdot.
+Mcg_b = MAcg_b + MEcg_b;
+x4to6dot = InvIb * (Mcg_b - cross(wbe_b, Ib*wbe_b));
+
+% Calculate phidot, thetadot, psidot
+H_phi = [1 sin(x7)*tan(x8) cos(x7)*tan(x8);
+        0 cos(x7) -sin(x7);
+        0 sin(x7)/cos(x8) cos(x7)/cos(x8)];
+
+x7to9dot = H_phi*wbe_b;
+
+% Place in first order form
+XDOT = [x1to3dot
+        x4to6dot
+        x7to9dot];