Skip to content

Commit cfc1c34

Browse files
mattia-gramugliamattia-gramuglia
authored
Added Funnel Two-Layer MRAC
Added Funnel Two-Layer MRAC. Funnel implemented to both inner and outer loop.
1 parent 612039f commit cfc1c34

File tree

3 files changed

+392
-1
lines changed

3 files changed

+392
-1
lines changed

‎GainsController.py‎

Lines changed: 143 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1189,6 +1189,149 @@ def FunnelMRACwithBASELINE(mass_total_estimated,air_density_estimated,surface_ar
11891189
# end of FUNNEL MRAC WITH BASELINE
11901190
# ================================================================================================================================================================
11911191

1192+
# ================================================================================================================================================================
1193+
# FUNNEL TWO-LAYER MRAC WITH BASELINE
1194+
# ================================================================================================================================================================
1195+
def TwoLayerMRACwithBASELINE(mass_total_estimated,air_density_estimated,surface_area_estimated,drag_coefficient_matrix_estimated):
1196+
1197+
# Number of states to be integrated by RK4
1198+
number_of_states = 138
1199+
# Length of the array vector that will be exported
1200+
size_DATA = 74
1201+
# size_DATA = 94 # OLD data format
1202+
1203+
# ----------------------------------------------------------------
1204+
# Baseline Parameters
1205+
# ----------------------------------------------------------------
1206+
1207+
# **Translational** baseline parameters to let the reference model follow the user-defined model (mu_baseline_tran)
1208+
KP_tran = np.matrix(1 * np.diag([5,5,6]))
1209+
KD_tran = np.matrix(1 * np.diag([8,8,3]))
1210+
KI_tran = np.matrix(1 * np.diag([1,1,0.1]))
1211+
1212+
# **Translational** parameters for the PD baseline controller (mu_PD_baseline_tran)
1213+
KP_tran_PD_baseline = np.matrix(1 * np.diag([5,5,6]))
1214+
KD_tran_PD_baseline = np.matrix(1 * np.diag([8,8,3]))
1215+
1216+
# **Rotational** baseline parameters
1217+
KP_rot = np.matrix(1 * np.diag([100,100,50]))
1218+
1219+
# **Rotational** parameters for the PID baseline controller (Moment_baseline_PI)
1220+
KP_rot_PI_baseline = np.matrix(2 * np.diag([1,1,0.5]))
1221+
KD_rot_PI_baseline = np.matrix(0.3 * np.diag([1,1,0.5]))
1222+
KI_rot_PI_baseline = np.matrix(1 * np.diag([1,1,0.5]))
1223+
1224+
K_P_omega_ref = np.matrix(1.5 * np.diag([5,5,2]))
1225+
1226+
# ----------------------------------------------------------------
1227+
# Translational Parameters MRAC
1228+
# ----------------------------------------------------------------
1229+
1230+
# Plant parameters **Translational** dynamics
1231+
A_tran = np.block([[np.zeros((3, 3)), np.identity(3)],
1232+
[np.zeros((3, 3)), np.zeros((3, 3))]])
1233+
1234+
B_tran = np.matrix(np.block([[np.zeros((3, 3))],
1235+
[np.identity(3)]]))
1236+
1237+
A_tran_bar = A_tran # Estimated A_tran matrix
1238+
Lambda_bar = (1/mass_total_estimated) * np.identity(3) # Estimate of \Lambda
1239+
Theta_tran_adaptive_bar = air_density_estimated * surface_area_estimated * drag_coefficient_matrix_estimated
1240+
1241+
# **Translational** reference model parameters and estimates
1242+
A_ref_tran = np.block([[np.zeros((3, 3)), np.identity(3)],
1243+
[ -KP_tran, -KD_tran]])
1244+
1245+
B_ref_tran = np.matrix(np.block([[np.zeros((3, 3))],
1246+
[(1/mass_total_estimated)*np.identity(3)]]))
1247+
1248+
# **Translational** adaptive parameters
1249+
Gamma_x_tran = np.matrix(5e2 * np.diag([1,1,10,1,1,10])) # Adaptive rates
1250+
Gamma_r_tran = np.matrix(1e2 * np.diag([1,1,10])) # Adaptive rates
1251+
Gamma_Theta_tran = np.matrix(1e0 * np.diag([1,1,1,1,1,1])) # Adaptive rates
1252+
1253+
# ----------------------------------------------------------------
1254+
# Rotational Parameters MRAC
1255+
# ----------------------------------------------------------------
1256+
1257+
# Plant parameters **Rotational** dynamics
1258+
A_rot = np.matrix(np.zeros((3,3)))
1259+
B_rot = np.matrix(np.eye(3))
1260+
1261+
# **Rotational** reference model parameters
1262+
A_ref_rot = -K_P_omega_ref
1263+
B_ref_rot = np.matrix(np.eye(3))
1264+
1265+
# **Rotational** adaptive parameters
1266+
Gamma_x_rot = np.matrix(2e1 * np.diag([1,10,1])) # Adaptive rates
1267+
Gamma_r_rot = np.matrix(1e-3 * np.diag([1,1,1])) # Adaptive rates
1268+
Gamma_Theta_rot = np.matrix(1e0 * np.diag([1,1,1,1,1,1])) # Adaptive rates
1269+
1270+
# ----------------------------------------------------------------
1271+
# Additional gains for 2-Layer MRAC
1272+
# ----------------------------------------------------------------
1273+
1274+
# **Translational** second layer parameters
1275+
poles_ref_tran = LA.eig(A_ref_tran)[0]
1276+
poles_transient_tran = poles_ref_tran + 2*np.min(np.real(poles_ref_tran))
1277+
K_transient_tran = scipy.signal.place_poles(A_tran, B_ref_tran, poles_transient_tran)
1278+
K_transient_tran = np.matrix(K_transient_tran.gain_matrix)
1279+
A_transient_tran = A_tran - B_ref_tran*K_transient_tran # Eigenvalues of A_transient should be further on the left in the complex plane of those of A_ref!!!!
1280+
1281+
Q_tran_2Layer = np.matrix(1e-2 * np.diag([1,1,12,1,1,2]))
1282+
P_tran_2Layer = np.matrix(linalg.solve_continuous_lyapunov(A_transient_tran.T, -Q_tran_2Layer))
1283+
1284+
Gamma_g_tran = np.matrix(1e1 * np.diag([1,1,1,1,1,1])) # Adaptive rates
1285+
1286+
# **Rotational** second layer parameters
1287+
poles_ref_rot = LA.eig(A_ref_rot)[0]
1288+
poles_transient_rot = poles_ref_rot + np.min(np.real(poles_ref_rot))
1289+
K_transient_rot = scipy.signal.place_poles(A_rot, B_ref_rot, poles_transient_rot)
1290+
K_transient_rot = np.matrix(K_transient_rot.gain_matrix)
1291+
A_transient_rot = A_rot - B_ref_rot*K_transient_rot # Eigenvalues of A_transient should be further on the left in the complex plane of those of A_ref!!!!
1292+
1293+
Q_rot_2Layer = np.matrix(8e-4 * np.diag([1,10,1]))
1294+
P_rot_2Layer = np.matrix(linalg.solve_continuous_lyapunov(A_transient_rot.T, -Q_rot_2Layer))
1295+
1296+
Gamma_g_rot = np.matrix(1e0 * np.diag([1,1,1])) # Adaptive rates
1297+
1298+
# Estimates of the matrices that verify the matching conditions for the **Translational** dynamics.
1299+
# Used for baseline controller and to center constraining ellipsoid when using projection operator
1300+
K_x_tran_bar = (LA.pinv(B_tran*Lambda_bar)*(A_ref_tran - A_tran_bar)).T * np.matrix(1*np.diag([1,1,1]))
1301+
K_r_tran_bar = (LA.pinv(B_tran*Lambda_bar)*B_ref_tran).T * np.matrix(1*np.diag([1,1,1]))
1302+
K_g_tran_bar = (LA.pinv(B_tran*Lambda_bar)*(A_transient_tran - A_ref_tran)).T * np.matrix(0.001*np.diag([1,1,1]))
1303+
1304+
# ----------------------------------------------------------------
1305+
# Funnel Parameters MRAC
1306+
# ----------------------------------------------------------------
1307+
1308+
# **Translational** Funnel parameters
1309+
eta_max_funnel_tran = 1
1310+
M_funnel_tran = np.matrix(1e0 * np.diag([1,1,1,1,1,1])) # n x n
1311+
u_max = 85
1312+
u_min = 0
1313+
Delta_u_min = 5
1314+
nu_funnel_tran = 0.01
1315+
1316+
# **Rotational** Funnel parameters
1317+
eta_max_funnel_rot = 1
1318+
M_funnel_rot = np.matrix(1e0 * np.diag([1,1,1])) # n x n
1319+
Moment_max = 10
1320+
Moment_min = 0
1321+
Delta_Moment_min = 0.01
1322+
nu_funnel_rot = 0.01
1323+
1324+
return [number_of_states,size_DATA,KP_tran,KD_tran,KI_tran,KP_tran_PD_baseline,KD_tran_PD_baseline,KP_rot,KP_rot_PI_baseline,
1325+
KD_rot_PI_baseline,KI_rot_PI_baseline,K_P_omega_ref,A_tran,B_tran,A_tran_bar,Lambda_bar,Theta_tran_adaptive_bar,
1326+
A_ref_tran,B_ref_tran,Gamma_x_tran,Gamma_r_tran,Gamma_Theta_tran,Gamma_Theta_tran,A_rot,B_rot,A_ref_rot,
1327+
B_ref_rot,Gamma_x_rot,Gamma_r_rot,Gamma_Theta_rot,A_transient_tran,Q_tran_2Layer,P_tran_2Layer,Gamma_g_tran,
1328+
A_transient_rot,Q_rot_2Layer,P_rot_2Layer,Gamma_g_rot,K_x_tran_bar,K_r_tran_bar,K_g_tran_bar,eta_max_funnel_tran,
1329+
M_funnel_tran,u_max,u_min,Delta_u_min,nu_funnel_tran,eta_max_funnel_rot,M_funnel_rot,Moment_max,Moment_min,
1330+
Delta_Moment_min,nu_funnel_rot]
1331+
# ================================================================================================================================================================
1332+
# end of FUNNEL TWO-LAYER MRAC WITH BASELINE
1333+
# ================================================================================================================================================================
1334+
11921335

11931336

11941337

0 commit comments

Comments
 (0)