Astr av2 simulation

.gitignore

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

Actuators/ActuatorDynamics.m

@@ -0,0 +1,37 @@
+function xdot = ActuatorDynamics(x, u)
+    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 = 700;
+    b = 40;
+
+    k1 = 0.5;
+    k2 = 0.5;
+
+    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
+    xdot(6) = (1/(tau/2)) * (u(4) - 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)) * 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/JacobianU.m

@@ -0,0 +1,45 @@
+function out1 = JacobianU(in1,in2)
+%JacobianU
+%    OUT1 = JacobianU(IN1,IN2)
+
+%    This function was generated by the Symbolic Math Toolbox version 25.2.
+%    05-Nov-2025 19:06:56
+
+phi = in2(2,:);
+q1 = in1(1,:);
+q2 = in1(2,:);
+q3 = in1(3,:);
+tau = in2(4,:);
+theta = in2(1,:);
+thrust = in2(3,:);
+t2 = cos(phi);
+t3 = cos(theta);
+t4 = sin(phi);
+t5 = sin(theta);
+t6 = q1.^2;
+t7 = q2.^2;
+t8 = q3.^2;
+t9 = q1.*q2.*2.0;
+t10 = q1.*q3.*2.0;
+t11 = q2.*q3.*2.0;
+t12 = t6.*2.0;
+t13 = t7.*2.0;
+t14 = t8.*2.0;
+t18 = -t6;
+t19 = -t7;
+t20 = -t8;
+t21 = t12+t13-1.0;
+t22 = t12+t14-1.0;
+t23 = t13+t14-1.0;
+t24 = t18+t19+t20+1.0;
+t25 = sqrt(t24);
+t26 = q1.*t25.*2.0;
+t27 = q2.*t25.*2.0;
+t28 = q3.*t25.*2.0;
+t29 = t11+t26;
+t30 = t10+t27;
+t31 = t9+t28;
+mt1 = [0.0,0.0,0.0,0.0,0.0,0.0,-t3.*thrust.*(t9-t28)+t4.*t5.*t23.*thrust-t2.*t5.*t30.*thrust,t3.*t22.*thrust-t4.*t5.*t31.*thrust-t2.*t5.*thrust.*(t11-t26),-t3.*t29.*thrust+t2.*t5.*t21.*thrust-t4.*t5.*thrust.*(t10-t27),t3.*thrust.*(1.2e+1./5.0)-t4.*t5.*tau.*2.0e+1,t3.*tau.*(-1.0e+3./5.3e+1)-t4.*t5.*thrust.*(1.2e+2./5.3e+1),t2.*t5.*tau.*(-1.0e+3./5.3e+1),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-t2.*t3.*t23.*thrust-t3.*t4.*t30.*thrust,t2.*t3.*t31.*thrust-t3.*t4.*thrust.*(t11-t26),t3.*t4.*t21.*thrust+t2.*t3.*thrust.*(t10-t27),t2.*t3.*tau.*2.0e+1];
+mt2 = [t2.*t3.*thrust.*(1.2e+2./5.3e+1),t3.*t4.*tau.*(-1.0e+3./5.3e+1),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-t5.*(t9-t28)-t3.*t4.*t23+t2.*t3.*t30,t5.*t22+t3.*t4.*t31+t2.*t3.*(t11-t26),-t5.*t29-t2.*t3.*t21+t3.*t4.*(t10-t27),t5.*(1.2e+1./5.0),t3.*t4.*(1.2e+2./5.3e+1),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t3.*t4.*2.0e+1,t5.*(-1.0e+3./5.3e+1),t2.*t3.*(1.0e+3./5.3e+1),0.0,0.0,0.0];
+out1 = reshape([mt1,mt2],15,4);
+end

Controls/JacobianX.m

@@ -0,0 +1,89 @@
+function out1 = JacobianX(in1,in2)
+%JacobianX
+%    OUT1 = JacobianX(IN1,IN2)
+
+%    This function was generated by the Symbolic Math Toolbox version 25.2.
+%    05-Nov-2025 19:06:55
+
+b4 = in1(13,:);
+b5 = in1(14,:);
+b6 = in1(15,:);
+omega1 = in1(10,:);
+omega2 = in1(11,:);
+omega3 = in1(12,:);
+phi = in2(2,:);
+q1 = in1(1,:);
+q2 = in1(2,:);
+q3 = in1(3,:);
+theta = in2(1,:);
+thrust = in2(3,:);
+t2 = cos(phi);
+t3 = cos(theta);
+t4 = sin(phi);
+t5 = sin(theta);
+t6 = q1.*2.0;
+t7 = q2.*2.0;
+t8 = q3.*2.0;
+t9 = q1.^2;
+t10 = q2.^2;
+t11 = q3.^2;
+t12 = -omega1;
+t13 = -omega2;
+t14 = -omega3;
+t15 = b4./2.0;
+t16 = b5./2.0;
+t17 = b6./2.0;
+t18 = omega1./2.0;
+t19 = omega2./2.0;
+t20 = omega3./2.0;
+t21 = q1./2.0;
+t22 = q2./2.0;
+t23 = q3./2.0;
+t30 = b4.*(3.0./5.3e+1);
+t31 = b5.*(3.0./5.3e+1);
+t32 = b6.*(3.0./5.3e+1);
+t33 = omega1.*(3.0./5.3e+1);
+t34 = omega2.*(3.0./5.3e+1);
+t35 = omega3.*(3.0./5.3e+1);
+t24 = -t9;
+t25 = -t10;
+t26 = -t11;
+t27 = b4+t12;
+t28 = b5+t13;
+t29 = b6+t14;
+t36 = -t21;
+t37 = -t22;
+t38 = -t23;
+t39 = -t30;
+t40 = -t33;
+t41 = t33+t39;
+t42 = t30+t40;
+t43 = t24+t25+t26+1.0;
+t44 = sqrt(t43);
+t45 = 1.0./t44;
+t46 = t44.*2.0;
+t47 = t44./2.0;
+t48 = q2.*t6.*t45;
+t49 = q3.*t6.*t45;
+t50 = q3.*t7.*t45;
+t51 = -t47;
+t55 = q1.*q2.*t45.*-2.0;
+t56 = q1.*q3.*t45.*-2.0;
+t57 = q2.*q3.*t45.*-2.0;
+t58 = t9.*t45.*-2.0;
+t59 = t10.*t45.*-2.0;
+t60 = t11.*t45.*-2.0;
+t61 = t6+t50;
+t62 = t7+t49;
+t63 = t8+t48;
+t64 = t6+t57;
+t65 = t7+t56;
+t66 = t8+t55;
+t67 = t46+t58;
+t68 = t46+t59;
+t69 = t46+t60;
+mt1 = [t21.*t27.*t45,t17-t20+t21.*t28.*t45,-t16+t19+t21.*t29.*t45,0.0,0.0,0.0,-t5.*t62.*thrust+t2.*t3.*t66.*thrust,q1.*t5.*thrust.*4.0-t2.*t3.*t67.*thrust+t3.*t4.*t65.*thrust,-t5.*t67.*thrust-q1.*t2.*t3.*thrust.*4.0+t3.*t4.*t63.*thrust,0.0,0.0,0.0,0.0,0.0,0.0,-t17+t20+t22.*t27.*t45,t22.*t28.*t45,t15-t18+t22.*t29.*t45,0.0,0.0,0.0,-t5.*t61.*thrust-q2.*t3.*t4.*thrust.*4.0+t2.*t3.*t68.*thrust,t2.*t3.*t63.*thrust+t3.*t4.*t64.*thrust,-t5.*t66.*thrust-q2.*t2.*t3.*thrust.*4.0-t3.*t4.*t68.*thrust,0.0,0.0,0.0,0.0,0.0,0.0,t16-t19+t23.*t27.*t45,-t15+t18+t23.*t28.*t45,t23.*t29.*t45,0.0,0.0,0.0];
+mt2 = [t5.*t69.*thrust-q3.*t3.*t4.*thrust.*4.0+t2.*t3.*t64.*thrust,q3.*t5.*thrust.*4.0+t2.*t3.*t62.*thrust+t3.*t4.*t69.*thrust,-t5.*t65.*thrust+t3.*t4.*t61.*thrust,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t47,t23,t37,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-t32+t35,t31-t34,0.0,0.0,0.0,t38,t47,t21,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t42,0.0,0.0,0.0,t22,t36,t47,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t41,0.0,0.0,0.0,0.0,t51,t38,t22,0.0];
+mt3 = [0.0,0.0,0.0,0.0,0.0,0.0,t32-t35,-t31+t34,0.0,0.0,0.0,t23,t51,t36,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t41,0.0,0.0,0.0,t37,t21,t51,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t42,0.0,0.0,0.0,0.0];
+out1 = reshape([mt1,mt2,mt3],15,15);
+end

Controls/SolveInput.m

@@ -0,0 +1,50 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Function computes the optimal input at the current timestep by
+% re-linearizing LQR at the current estimated state x0. We then find an input
+% u0 such that Ax0 + Bu0 = 0 and solve for our optimal Gain Matrix K. We
+% then add this u0 input as a feedforward signal. 
+%
+% By: Pablo Plata   -   10/22/25
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [K, u] = SolveInput(x0, x_ref, u0)
+
+% Calculate the J_u Jacobian [B matrices] at thrust up
+B = JacobianU([x0; zeros(3,1)], u0);
+B = B(1:12,:);
+
+% Solve for u0 such that Ax0 + Bu0 = 0
+%   invp(B) is the Moore-Penrose pseudoinverse of B
+%   check Eigen availability of this function, prob avaiable under SVD.
+xdot = nominalDynamics([x0; zeros(3,1)], zeros(4,1));
+u0 = -pinv(B) * xdot(1:12);
+
+% Calculate the J_x Jacobian [A matrices] at current state
+A = JacobianX([x0; zeros(3,1)], u0);
+A = A(1:12,1:12);
+
+% Define Q and R matrices for LQR using Bryson's Rule
+a_weights = ones(12,1);
+b_weights = ones(4,1);
+a_weights = a_weights / norm(a_weights);
+b_weights = b_weights / norm(b_weights);
+
+max_x = [2, 2, 0.08, 1000, 1000, 1000, 0.8, 0.8, 2, 2, 2, 10];
+max_u = [pi/30, pi/30, 6, 0.5];
+
+Q = eye(size(A,1)) .* a_weights ./ max_x.^2;
+R = eye(size(B,2)) .* b_weights ./ max_u.^2;
+
+% Solve the LQR problem and compute gain matrix. 
+K = SolveLQR(A, B, Q, R);
+
+% Compute optimal input
+u = -K * (x0 - x_ref) + u0;
+
+% Input saturation
+thrust_max = 1.5 * 9.8;
+maxU = [pi/24; pi/24; thrust_max; thrust_max * 10];
+minU = [-pi/24; -pi/24; thrust_max * 0.4; -thrust_max * 10];
+u = max(minU, u);
+u = min(maxU, u);

Controls/SolveLQR.m

@@ -0,0 +1,42 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+%  The following function solves an infinite time horizon LQR problem by
+% computing the unique solution P to the Continious-Time Algebraic Ricatti
+% Equation (CARE) using the Hamiltonian matrix. This solution is then used
+% to compute the optimal gain matrix for controls.
+%
+% By: Pablo Plata   -   10/22/25
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function K = SolveLQR(A, B, Q, R)
+
+% Define the Hamiltonian matrix H (size 2n x 2n)
+R_inv = inv(R);
+H = [A  -B * R_inv * B';
+    -Q  -A'];
+
+% Find eigenvalues of the Hamiltonian
+%   (eig(H) function returns a matrix of
+%   eigenvectors of H and a diagonal matrix of eigenvalues of H.)
+[EVec_H, EVal_H] = eig(H);
+EVal_H = diag(EVal_H);
+NegEigen = real(EVal_H) < 0;
+
+% Denote the V matrix, containing the eigenvectors corresponding to stable
+% eigenvalues. Then decompose into V1 (first n rows) and V2 (last n rows)
+V = EVec_H(:, NegEigen);
+V1 = V(1:size(A,1), :);
+V2 = V(size(A,1)+1:end,:);
+
+% Compute the solution P to the Algebraic Riccati Equation, and ensure real
+% coefficients and symmetry
+P = V2 / V1;
+P = real(P);
+P = (P + P') / 2;
+
+% Compute the gain matrix for the stabilizing solution
+K = R_inv * B' * P;
+
+
+