Astr av2 simulation

.gitignore

@@ -1,2 +1,4 @@
 **/*.slxc
 **/.DS_Store
+**/.slprj
+slprj

Actuators/ActuatorDynamics.m

@@ -0,0 +1,39 @@
+function xdot = ActuatorDynamics(x, u, GND)
+    xdot = zeros(6, 1);
+    % Simple actuator model
+    % x(1) Theta 
+    % x(2) Rate 1
+    % x(3) Phi 2
+    % x(4) Rate 2
+    % x(5) Thrust
+    % x(6) Roll
+
+    % Assume identical servos for gimbal
+    a = 400;
+    b = 35;
+
+    k1 = 1;
+    k2 = 1;
+
+    theta_error = k1 * (1 - cos(x(1)));
+    phi_error = k2 * (1 - cos(x(3)));
+
+    x(3) = x(3) - theta_error;
+    x(1) = x(1) - phi_error;
+
+    xdot(1:4) = [x(2);
+                a*(u(1) - x(1)) - b*x(2); 
+                x(4);
+                a*(u(2) - x(3)) - b*x(4)];
+
+    % Assume first order response for thrust
+    tau = 0.15;
+    xdot(5) = (1/tau) * (u(3) - x(5));
+
+    % Since torque depends on thrust changes on two fans, response time is
+    % half that of thrust. We artificially add an error to the applied
+    % input, proportional to the squared magnitude of the thrust pct
+    roll_error = 0.05 * (u(3) / (1.5 * 9.8))^2 * (1 - GND);
+    xdot(6) = (1/(tau/2)) * (u(4) + roll_error - x(6));
+
+end

Actuators/ActuatorTest.m

@@ -0,0 +1,55 @@
+% Test code for actuator modeling responses
+clear;
+close all;
+
+% Initialize time for simulation
+res = 5000;
+tmax = 10;
+dt = tmax / res;
+tsim = linspace(0,tmax,res);
+norm = [pi/24; pi/24; 14; 15];
+
+% Target step response to max values
+start = 100;
+utrg = [zeros(4, start), norm .* ones(4, res - start)];
+usim = zeros(4,res);
+xsim = zeros(6,res);
+
+% Initialize data tables
+theta_target = zeros(1, res);
+phi_target = zeros(1, res);
+theta_actual = zeros(1, res);
+phi_actual = zeros(1, res);
+
+% Simulation loop
+for i = 2:1:res
+    theta_target(i) = norm(1); %* cos(tsim(i) * 1);
+    phi_target(i) = norm(2) * sin(tsim(i) * 1) * 0;
+    utrg(1:2, i) = [theta_target(i); phi_target(i)];
+
+    xsim(:,i) = xsim(:,i-1) + ActuatorDynamics(xsim(:,i-1), utrg(:, i), 1) * dt;
+    usim(:,i) = [xsim(1,i); xsim(3,i); xsim(5:6,i)];
+
+    theta_actual(:, i) = xsim(1, i);
+    phi_actual(:, i) = xsim(3, i);
+end
+
+% Plot results
+figure;
+plot(tsim, usim .* [180/pi; 180/pi; 1; 1]); hold on; grid on;
+plot(tsim, utrg .* [180/pi; 180/pi; 1; 1], 'r--');
+% plot(tsim, (utrg - usim) .* [180/pi; 180/pi; 1; 1]);
+legend('Angle 1', 'Angle 2', 'Thrust', 'Torque');
+
+figure;
+subplot(2, 1, 1)
+plot(theta_target, theta_actual); grid on;
+title("Theta Target vs Actual")
+
+subplot(2, 1, 2)
+plot(phi_target, phi_actual); grid on;
+title("Phi Target vs Actual")
+
+figure;
+plot(xsim(1,:), xsim(3,:), "b*");
+axis equal

Controls/ASTRAv2_Controller.m

@@ -0,0 +1,134 @@
+% Version 2 Controller formulation for ASTRAv2. Structure consists of 3
+% cascaded loops.
+%
+% First Loop is a P Loop in charge of Pos -> Vel commands.
+%
+% Second loop is a PI Loop in charge of Vel -> Acceleration commands, fitted
+% with anti-windup and soft-gatingcbased on attitude error.
+%
+% Second loop is an LQRi Loop in charge of Target Attitude -> Gimbal +
+% Torque commands. Integral action is fitted with anti-windup clamps and is
+% self soft-gated.
+% Tuning of the second loop is done by using a Genetic algorithm to
+% maximize a Crossover Frequency vs. Disk Margin tradeoff on all actuator
+% channels.
+%
+% By: Pablo Plata   -   11/27/25 (Happy Thanksgiving!)
+function [U, VEI] = ASTRAv2_Controller(PosTarget, X, constantsASTRA, t)
+
+% Time Counter
+persistent lastT VelErrorI AttErrorI lastAttError
+if isempty(lastT)
+    lastT = 0;
+    VelErrorI = zeros(3,1);
+    AttErrorI = zeros(3,1);
+    lastAttError = zeros(3,1);
+end
+dT = t - lastT;
+lastT = t;
+K_Att = constantsASTRA.K_Att;
+
+% Controller Limits
+thrustMax = 1.5 * 9.8;   %N
+gimbalMax = pi/18;
+InputBounds = [-gimbalMax       gimbalMax;
+               -gimbalMax       gimbalMax;
+               .4 * thrustMax   thrustMax;
+               -pi/6            pi/6];
+U = zeros(4,1);
+
+%% First Loop (P Loop)
+    % Position Error Vector
+    PosError = PosTarget - X(5:7);
+
+    % Velocity Command
+    K_P = [0.56; 0.56; 0.65];
+    VelTarget = K_P .* PosError;
+
+    % Velocity Saturation Step
+    MaxVel = [1 1 1.5]';
+    VelTarget = max(min(VelTarget, MaxVel), -MaxVel);
+
+%% Second Loop (PI Loop)
+    % Velocity Error Vector
+    VelError = VelTarget - X(8:10);
+
+    % Integral Accumulator
+    K_I = [2.3; 2.3; 5];
+    Leak = 0.35;
+    Clamp = [5; 5; 5];
+
+    % Normalize errors (0 to 1 scale)
+    MaxAttError = [0.05; 0.05; 0.3];
+    MaxVelError = [0.3; 0.3; 0.3];
+    NormAttErr = abs(lastAttError) ./ MaxAttError;
+    NormVelErr = abs(VelError) ./ MaxVelError;
+
+    % Combine errors (Vector magnitude)
+    TotalErrorMetric = NormAttErr + NormVelErr;
+
+    % Calculate Gate using Gaussian function
+    Gate = (1 - Leak) * exp(-2 * TotalErrorMetric.^2) + Leak;
+
+    % Integrator
+    K_I = K_I .* Gate;
+    VelErrorI = VelErrorI + K_I .* VelError .* dT;
+    VelErrorI = max(min(VelErrorI, Clamp), -Clamp);
+    K_P = [3.2; 3.2; 3.5];
+
+    % Acceleration Target
+    AccelTarget = K_P .* VelError + VelErrorI  + [0; 0; constantsASTRA.g];
+
+    % Acceleration Saturation Step
+    MaxAccelUp = [2 2 15]';
+    MaxAccelDown = [-2 -2 4]';
+    AccelTarget = max(min(AccelTarget, MaxAccelUp), MaxAccelDown);
+
+%% Kinematics Step
+    % Compute thrust target
+    TargetForce_I = constantsASTRA.m * AccelTarget;
+    U(3) = norm(TargetForce_I);
+
+    % Compute target attitude via GSP.
+    AccelTarget(3) = max(AccelTarget(3), constantsASTRA.g);
+    Z_b = AccelTarget / norm(AccelTarget);
+
+    % Heading reference (+X axis rolled to north)
+    HDGRef = [1; 0; 0];
+    Y_b = cross(Z_b, HDGRef);
+    Y_b = Y_b / norm(Y_b);
+
+    % Complete the triad
+    X_b = cross(Y_b, Z_b);
+
+    % Create DCM and convert to quaternion
+    DCM = [X_b Y_b Z_b];
+    TargetAtt = DCM_Quat_Conversion(DCM);
+
+%% Third Loop (LQRi)
+    % Attitude Error computation
+    Q_Conj = [X(1); -X(2:4)];
+    AttError = HamiltonianProd(Q_Conj) * TargetAtt;
+    if AttError(1) < 0
+        AttError = -AttError;
+    end
+    lastAttError = AttError(2:4);
+
+    % Error accumulation and clamping
+    Clamp = [3; 3; 0.4];
+    AttErrorI = AttErrorI + AttError(2:4) .* dT;
+    AttErrorI = max(min(AttErrorI, Clamp), -Clamp);
+
+    % State vector and error
+    X_Err = [-AttError(2:4); X(11:13); AttErrorI];
+
+    % LQR Controller
+    U([1 2 4]) = -K_Att * X_Err;
+
+%% Controls Saturation
+uMax = InputBounds(:, 2);
+uMin = InputBounds(:, 1);
+U = min(max(U, uMin), uMax);
+VEI = VelTarget;
+
+