Adjusted table call and other fixes

design/controller/lqr_algorithm.m → design/controller/control_algorithm.m

@@ -1,8 +1,14 @@
-function [u] = lqr_algorithm(x, r)
+function [u] = control_algorithm(x, r)
     % Computes control output. Uses gain schedule table and simplified roll model
     % Inputs: full state x, reference signal r
     % Outputs: control input u
 
+    persistent table; 
+
+    if isempty(table)
+        table = load("controller\gains.mat", "Ks", "P_mesh", "C_mesh");
+    end
+
     %% State
     % decompose state vector: [q(4); w(3); v(3); alt; Cl; delta]
     q = x(1:4); w = x(5:7); v = x(8:10); alt = x(11); Cl = x(12); delta = x(13);
@@ -16,9 +22,9 @@
     % cat roll state
     x_roll = [phi; w(1); delta];
 
-    %% Schedule
+    %% Gain scheduling
     % get gain from schedule
-    Ks = lqr_schedule(x);
+    Ks = control_scheduler(table, x);
     K_pre = Ks(4);
     K = Ks(1:3);
 

design/controller/control_scheduler.m

@@ -0,0 +1,37 @@
+function [K] = control_scheduler(table, x, dynamicpressure, canardcoeff)
+    % determines feedback gain from states. Interpolates from look-up table
+    % input: full state x
+    % output: K = [phi, p, delta, pre]
+
+    % check if state is provided
+    if isempty(x) == 0
+        % decompose state vector: [q(4); w(3); v(3); alt; Cl; delta]
+        v = x(8:10); alt = x(11); Cl = x(12);
+
+        % calculate air data
+        [~, ~, rho, ~] = model_airdata(alt);
+        airspeed = norm(v);
+        p_dyn = rho/2*airspeed^2;
+    end 
+
+    % check for optinonal inputs
+    if nargin > 2 && isempty(dynamicpressure) == 0
+        p_dyn = dynamicpressure;
+    end
+    if nargin > 2 && isempty(canardcoeff) == 0
+       Cl = canardcoeff; 
+    end
+
+    %% Load table
+    Ks = table.Ks;
+    P_mesh = table.P_mesh;
+    C_mesh = table.C_mesh;
+
+    %% Interpolate table
+    K = zeros(4,1);
+    for i=1:4
+        %%% interpolate linearly between design points, output 0 if state outside of table
+        K(i) = interp2(P_mesh, C_mesh, Ks(:,:,i), Cl, p_dyn, 'linear', 0); 
+    end
+end
+

design/controller/lqr_tune_table.m

@@ -5,7 +5,7 @@
 
 % amount of design point for each dimension
 P_amount = 200; % dynamic pressure
-C_amount = 10; % coefficient of lift
+C_amount = 30; % coefficient of lift
 
 %% tuning parameters
 Q = diag([10, 0, 10]);
@@ -55,7 +55,7 @@
 
 %% Plot
 if 0
-    samplep = 1e5; samplec = 0;
+    samplep = 1e5; samplec = 1.5;
     for i=1:4
         K(i) = interp2(P_mesh, C_mesh, Ks(:,:,i), samplec, samplep, 'linear');
     end
@@ -70,6 +70,7 @@
     xlabel("Coefficient")
     ylabel("Dynamic pressure")
     zlabel("K_\phi")
+    zlim([-1,1])
 
     % figure(2)
     subplot(2,2,2)
@@ -80,7 +81,8 @@
     hold off
     xlabel("Coefficient")
     ylabel("Dynamic pressure")
-    zlabel("K_p")
+    zlabel("K_{\omega_x}")
+    zlim([-3,3])
 
     % figure(3)
     subplot(2,2,3)
@@ -92,6 +94,7 @@
     xlabel("Coefficient")
     ylabel("Dynamic pressure")
     zlabel("K_\delta")
+    zlim([-4,0])
 
     % figure(4)
     subplot(2,2,4)
@@ -103,4 +106,5 @@
     xlabel("Coefficient")
     ylabel("Dynamic pressure")
     zlabel("K_{pre}")
+    zlim([-1,1])
 end

design/model/model_params.m

@@ -5,9 +5,9 @@
 J = diag([Jx, Jy, Jy]);
 
 length_cg = 0; % center of gravity
-length_cp = -1; % center of pressure
+length_cp = -0.5; % center of pressure
 area_reference = pi*(8*0.0254/2)^2; % cross section of body tube
-Cn_alpha = 1; % pitch coefficent 
+Cn_alpha = 5; % pitch coefficent 
 c_aero = Cn_alpha*area_reference*length_cp; % moment coefficient of body
 
 %% Sensors
@@ -20,7 +20,7 @@
 tau = 1/60; % time constant of first order actuator dynamics
 Cl_alpha = 1.5; % estimated coefficient of lift, const with Ma
 tau_cl_alpha = 0.01; % time constant to converge Cl back to 1.5 in filter
-area_canard = 0.005; % total canard area 
+area_canard = 0.002; % total canard area 
 length_canard = 8*0.0254+0.05; % lever arm of canard to x-axis 
 c_canard = area_canard*length_canard; % moment arm * area of canard