Merge branch 'nav'

Author: tristan-yk

gnc-support/control/controller_gains_test.m

@@ -0,0 +1,38 @@
+A = [0,1;0,0];
+B  = [0; 1];
+
+L = 100:100:1000;
+
+Qp_t = 1;
+Qw_t = 2;
+
+Qp_a = 1;
+Qw_a = 2;
+
+for i = 1:length(L)
+    [Kt, Ktr] = controller_gains_torque(L(i), Qp_t, Qw_t)
+    [Ka, Kar] = controller_gains_angle(L(i), Qp_a/L(i)^2, Qw_a/L(i)^2)
+
+    sys_t = ss(A, B*L(i), Kt, 0);
+    sys_a = ss(A, B*L(i), Ka, 0);
+    sys_array_t(:,:,1,i) = sys_t;
+    sys_array_a(:,:,1,i) = sys_a;
+
+    cl_t = Ktr*ss(A+B*L(i)*Kt, B*L(i), eye(2), 0);
+    cl_a = Kar*ss(A+B*L(i)*Ka, B*L(i), eye(2), 0);
+    cl_array_t(:,:,1,i) = cl_t;
+    cl_array_a(:,:,1,i) = cl_a;
+
+end
+
+figure(1)
+bode(sys_array_a, "r")
+hold on
+bode(sys_array_t, "b")
+hold on
+
+figure(2)
+step(cl_array_a, "r")
+hold on
+step(cl_array_t, "b")
+hold on

gnc-support/control/controller_test.slx

@@ -0,0 +1,41 @@
+function [params, K] = param_estimator_gradient(time, w, d, c)
+    % w : angular rate measurement
+    % d : canard angle measurement / command
+    % p : parameters
+    % r : regressors
+    % Q : weights
+    % K : gain
+
+    
+    %%% initialize persistent variables
+    persistent t p w_old
+    if isempty(t)
+        t = -0.01;
+    end
+    if isempty(p)
+        p = [2; 0]; % parameters, initial guess of CL_delta and CL_0
+    end
+    if isempty(w_old)
+        w_old = w;
+    end 
+    
+    %%% set up 
+    dt = time - t;
+    dw = (w - w_old) / dt; % actual roll acceleration  
+    r = [d; 0.1] ; % regressors, delta (cmd or encoder) and constant disturbance
+    r = r * c;
+
+    %%% measurement prediction
+    dw_hat = r' * p;
+
+    %%% correction
+    Q = diag([1, 0.1]); % weights
+    Q = Q / (norm(r)^2); % normalized gradient
+    K = Q * r; % gain
+    p = p + K * (dw - dw_hat); % correct parameters
+    
+    %%% outputs and update persistent
+    t = time;
+    w_old = w;
+    params = p;
+end

gnc-support/control/param_estimator_gradient.m

@@ -0,0 +1,59 @@
+function [C_l_delta, C_l_0] = controller_estimator(time, w, delta, pdyn_params)
+    % estimates the canard aerodynamic coefficients from canard angle, roll rates, air data
+    % coeffs : canard coefficients C_l_delta and C_l_0
+    % time : current time stamp (s)
+    % w : angular rate measurement (rad/s)
+    % d : canard angle measurement or command (rad)
+    % pdyn_params : dynamic pressure * constant parameters (pressure * area * arm / inertia)
+ 
+    % dw/dt = c * Cl * d + C0 * c = phi_k' * p
+    % C_l_delta = canard lift coeff
+    % C_l_0 = rocket induced angular acceleration / (rho * area * arm)
+
+    %% tuning parameters
+    Q = diag([1e-5, 1e-9]);
+
+    %% initialize
+    persistent t c P_minus w_old d_old w_dot_old
+    if isempty(t)
+        t = -0.01; % for /(time - t)
+    end
+    if isempty(c)
+        c = [2; 0]; % initial coefficient guess
+    end
+    if isempty(P_minus)
+        P_minus = Q; 
+    end
+    if isempty(w_old)
+        w_old = w;
+    end
+    if isempty(d_old)
+        d_old = 0;
+    end
+    if isempty(w_dot_old)
+       w_dot_old = 0;
+    end
+
+    %% lowpass command and measurement
+    tau = 0.25;
+    delta = (1 - tau) * d_old + tau * delta;
+    w_dot = (1 - tau) * w_dot_old + tau * (w - w_old) / (time - t);
+
+    %% Kalman filter
+    r = pdyn_params * [delta; 1]; % regression 
+    P = P_minus + Q; % covariance prediction
+    K = P * r / (r' * P * r + 1); % correction gain. the stuff inside in brackets is just a scalar so you can just divide
+    coeffs = c + K * (w_dot - r' * c); % coefficient correction
+    P_plus = (eye(2) - K * r') * P * (eye(2) - K * r')' + K * 1* K'; % covariance correction. Joseph form for numerical stability
+    
+    %% update for next cycle
+    t = time;
+    c = coeffs;
+    P_minus = P_plus;
+    w_old = w;
+    d_old = delta;
+    w_dot_old = w_dot; 
+    C_l_delta = coeffs(1);
+    C_l_0 = coeffs(2);
+
+end

gnc-support/control/param_ident_online.slx

@@ -0,0 +1,24 @@
+function [u, K, Kr] = controller_law(xR, r, L_delta)
+    % computes the optimal control signal for a flight condition 
+    % u : control signal, desired canard angle (rad)
+    % xR : roll state [roll angle (rad); roll rate (rad/s)]
+    % r : reference signal, desired roll angle (rad)
+    % L_delta : roll acceleration control derivative (rad/s^2 / rad)
+
+    %% tuning parameters
+    Q_phi = 10; % weight of angle error
+    Q_omega = 10; % weight of rate error
+
+    %% feedback gains
+    K = - 1/L_delta * [ sqrt(Q_phi), sqrt( 2*sqrt(Q_phi) + Q_omega ) ];
+
+    %% feedforward gain
+    % A = [0, 1; 0, 0];
+    % B = [0; L_delta];
+    % C = [1, 0]; % tracking channel
+    % Kr = -1 / ( C * inv(A+B*K) * B );
+    Kr = - K(1); % simplifies to this
+
+    %% control command
+    u = K * xR + Kr * r;
+end

gnc/control/controller_estimator.m

@@ -0,0 +1,60 @@
+function [u, r] = controller_module(time, xR, pdyn)
+    % Top-level controller module.
+    % u : control command, desired canard angle (rad)
+    % r : roll angle target (rad)
+    % time : current time stamp (s)    
+    % xR : roll state [roll angle (rad), roll rate (rad/s)]
+    % pdyn : dynamic pressure (Pa)
+
+    %% settings
+    time_launch = 10; % pad delay time
+    time_coast = 10; % time from launch to burnout
+    time_program = 10; % time from launch to start of roll program
+    u_max = deg2rad(10); % limit output to this angle
+    L_min = 0.1; % limit roll control derivative for low authority conditions
+
+    %% parameters
+    persistent param
+    if isempty(param)
+        param = coder.load("model_params.mat");
+    end
+
+    %% Reference signal
+    % Generates reference signal r (rad) for roll program
+    % includes multiple roll angle steps
+    
+    t = time - time_launch;
+    r = 0;
+    if t > (time_program + 7)
+        if t < (time_program + 12)
+            r = 0.5;
+        elseif t < (time_program + 17)
+            r = -0.5;
+        elseif t < (time_program + 24)
+            r = 0.5;
+        elseif t > (time_program + 31)
+            r = 0;
+        end
+    end
+
+    %% controller algorithm
+    % Computes control signal of the adaptive LQR controller.
+
+    %%% Coefficient Estimation
+    pdyn_params = pdyn * params.c_canard;
+    C_l_delta = controller_estimator(time, w, delta, pdyn_params);
+       
+    L_delta = C_l_delta * pdyn_params;
+    L_delta = 1 / (max(min(1/L_delta, 1/L_min), -1/L_min)); % lower bounds
+
+    %%% Feedback law
+    u = controller_law(xR, r, L_delta);
+
+    %%% limit output to allowable angle
+    u = min(max(u, -u_max), u_max); % upper bounds
+
+    if t < time_coast % disable during boost
+        u = 0;
+    end
+end
+

gnc/control/controller_law.m

@@ -0,0 +1,46 @@
+clear
+
+%% Launch site
+elevation = 420; % m above sea level
+
+%% Rocket body
+m = 54.9; %mass in kg
+Jx = 0.46; % inertia roll
+Jy = 49.5; % inertia pitch, yaw
+J = diag([Jx, Jy, Jy]);
+Jinv = inv(J);
+
+length_cg = 0; % center of gravity
+length_cp = -0.5; % center of pressure
+area_reference = pi*(0.203/2)^2; % cross section of body tube
+c_aero = area_reference * (length_cp-length_cg);
+Cn_alpha = 10; % pitch forcing coefficent 
+Cn_omega = 0; % pitch damping coefficent 
+
+%% Canards
+area_canard = 2 * 0.102 * 0.0508 / 2; % total canard area 
+length_canard = 0.203/2 + 0.0508/3; % lever arm of canard to x-axis 
+c_canard = area_canard*length_canard; % moment arm * area of canard
+
+%% Environment
+g = [-9.81; 0; 0]; % gravitational acceleration in the geographic inertial frame
+
+
+%% Sensors
+S1 = [0, 0, 1;
+     1, 0, 0;
+     0, 1, 0]; % IMU 1, rotation transform from sensor frame to body frame
+S2 = [0, 0, -1;
+     -1, 0, 0;
+      0, 1, 0]; % IMU 2, rotation transform from sensor frame to body frame
+
+d1 = [1.2; 0.074; -0.027]; % center of sensor frame
+d2 = [1.2; 0.065; 0.047]; % center of sensor frame
+
+B1 = eye(3); % Soft iron compensation
+B2 = eye(3); % Soft iron compensation
+b1 = [0;0;0]; % Hard iron compensation
+b2 = [0;0;0]; % Hard iron compensation
+
+%% save and export
+save("design/model/model_params.mat");

gnc/control/controller_module.m

@@ -0,0 +1,51 @@
+function [airdata] = airdata_atmos(altitude)
+    % computes atmospheric stanadard data from altitude, according to US standard atmosphere 
+    % atmosphere data: static pressure, temperature, density, local speed of sound
+    % calculations found in Stengel 2004, pp. 30
+
+    % parameters
+    air_gamma = 1.4; % adiabatic index
+    air_R = 287.0579; % specific gas constant for air
+    air_atmosphere = [0, 101325, 288.15, 0.0065; % troposphere
+                      11000, 22632.1, 216.65, 0; % tropopause
+                      20000, 5474.9, 216.65, -0.001; % stratosphere
+                      32000, 868.02, 228.65, -0.0028]; % stratosphere 2
+                      % base height, P_base, T_base, lapse rate;
+    earth_r0 = 6356766; % mean earth radius
+    earth_g0 = 9.81; % zero height gravity
+
+    % geopotential altitude
+    altitude = earth_r0*altitude / (earth_r0 -altitude);
+    
+    % select atmosphere behaviour from table
+    layer = air_atmosphere(1,:);
+    if altitude > air_atmosphere(2,1)
+        if altitude < air_atmosphere(3,1)
+            layer = air_atmosphere(2,:);
+        elseif altitude < air_atmosphere(4,1)
+            layer = air_atmosphere(3,:);
+        elseif altitude >= air_atmosphere(4,1)
+            layer = air_atmosphere(4,:);
+        end
+    end
+
+    b = layer(1); % base height
+    P_B = layer(2); % base pressure
+    T_B = layer(3); % base temperature
+    k = layer(4); % temperature lapse rate
+    
+    if k == 0
+        pressure = P_B * exp(-earth_g0*(altitude-b)/(air_R*T_B));
+    else
+        pressure = P_B * (1 - k/T_B*(altitude-b))^(earth_g0/(air_R*k));
+    end
+    temperature = T_B - k*(altitude-b);
+    density = pressure / (air_R*temperature);
+    sonic_speed = sqrt(air_gamma*air_R*temperature);
+
+    % return values
+    airdata.pressure = pressure;
+    airdata.temperature = temperature;
+    airdata.density = density;
+    airdata.sonic_speed = sonic_speed;
+end

gnc/model_params.m

@@ -0,0 +1,59 @@
+function [airdata_altitude, airdata] = airdata_atmos_jacobian(altitude)
+    % computes atmospheric data and gradient w.r.t. altitude from altitude, according to US standard atmosphere 
+    % atmospheric data: static pressure, temperature, density, local speed of sound
+    % calculations found in Stengel 2004, pp. 30
+
+    % parameters
+    air_gamma = 1.4; % adiabatic index
+    air_R = 287.0579; % specific gas constant for air
+    air_atmosphere = [0, 101325, 288.15, 0.0065; % troposphere
+                      11000, 22632.1, 216.65, 0; % tropopause
+                      20000, 5474.9, 216.65, -0.001; % stratosphere
+                      32000, 868.02, 228.65, -0.0028]; % stratosphere 2
+                      % base height, P_base, T_base, lapse rate;
+    earth_r0 = 6356766; % mean earth radius
+    earth_g0 = 9.81; % zero height gravity
+
+    % geopotential altitude
+    altitude_ratio = earth_r0 / (earth_r0 - altitude);
+    altitude = altitude_ratio * altitude;
+    
+    % select atmosphere behaviour from table
+    layer = air_atmosphere(1,:);
+    if altitude > air_atmosphere(2,1)
+        if altitude < air_atmosphere(3,1)
+            layer = air_atmosphere(2,:);
+        elseif altitude < air_atmosphere(4,1)
+            layer = air_atmosphere(3,:);
+        elseif altitude >= air_atmosphere(4,1)
+            layer = air_atmosphere(4,:);
+        end
+    end
+
+    b = layer(1); % base height
+    P_B = layer(2); % base pressure
+    T_B = layer(3); % base temperature
+    k = layer(4); % temperature lapse rate
+    
+    if k == 0
+        pressure = P_B * exp(-earth_g0*(altitude-b)/(air_R*T_B));
+        pressure_altitude = - P_B*earth_g0 / (T_B*air_R) * (altitude_ratio^2) * exp( - earth_g0*(altitude - b) / (T_B*air_R) );
+    else
+        pressure = P_B * (1 - k/T_B*(altitude-b))^(earth_g0/(air_R*k));
+        pressure_altitude = - P_B*earth_g0 / (T_B*air_R) * (altitude_ratio^2) * ( 1 - k/T_B*(altitude - b) )^(earth_g0/(air_R*k) - 1);
+    end
+    temperature = T_B - k * (altitude - b);
+    temperature_altitude = - k * altitude_ratio^2;
+    density = pressure / (air_R*temperature);
+    density_altitude = 1/air_R * (pressure_altitude * temperature - pressure * temperature_altitude) / temperature^2;
+    sonic_speed = sqrt(air_gamma*air_R*temperature);
+    sonic_speed_altitude = 1/2 * temperature_altitude * sqrt(air_gamma*air_R / temperature);
+
+    % return values
+    airdata.pressure = pressure;
+    airdata_altitude.pressure = pressure_altitude;
+    airdata.density = density;
+    airdata_altitude.density = density_altitude;
+    airdata.sonic_speed = sonic_speed;
+    airdata_altitude.sonic_speed = sonic_speed_altitude;
+end