Funnel MRAC to outer loop

Added Funnel MRAC to the outer loop

Author: mattia-gramuglia

GainsController.py

@@ -1069,6 +1069,117 @@ def HybridRobustTwoLayerMRACwithBASELINE(mass_total_estimated,air_density_estima
     # end of HYBRID ROBUST TWO-LAYER MRAC WITH BASELINE
     # ================================================================================================================================================================
 
-    
-    
-    
+    # ================================================================================================================================================================
+    # FUNNEL MRAC WITH BASELINE
+    # ================================================================================================================================================================
+    def FunnelMRACwithBASELINE(mass_total_estimated,air_density_estimated,surface_area_estimated,drag_coefficient_matrix_estimated):
+         
+        # Number of states to be integrated by RK4
+        number_of_states = 101
+        # Length of the array vector that will be exported 
+        size_DATA = 74 
+        # size_DATA = 94 # OLD data format
+        
+        # ----------------------------------------------------------------
+        #                     Baseline Parameters
+        # ----------------------------------------------------------------
+
+        # **Translational** baseline parameters to let the reference model follow the user-defined model (mu_baseline_tran)
+        KP_tran = np.matrix(1 * np.diag([5,5,6]))
+        KD_tran = np.matrix(1 * np.diag([8,8,3]))
+        KI_tran = np.matrix(1 * np.diag([1,1,0.1]))
+
+        # **Translational** parameters for the PD baseline controller (mu_PD_baseline_tran)
+        KP_tran_PD_baseline = np.matrix(1 * np.diag([5,5,6]))
+        KD_tran_PD_baseline = np.matrix(1 * np.diag([8,8,3]))
+
+        # **Rotational** baseline parameters
+        KP_rot = np.matrix(1 * np.diag([100,100,50]))
+
+        # **Rotational** parameters for the PID baseline controller (Moment_baseline_PI)
+        KP_rot_PI_baseline = np.matrix(2 * np.diag([1,1,0.5]))
+        KD_rot_PI_baseline = np.matrix(0.3 * np.diag([1,1,0.5]))
+        KI_rot_PI_baseline = np.matrix(1 * np.diag([1,1,0.5]))
+
+        K_P_omega_ref = np.matrix(1.5 * np.diag([5,5,2]))
+
+        # ----------------------------------------------------------------
+        #                   Translational Parameters MRAC
+        # ----------------------------------------------------------------
+
+        # Plant parameters **Translational** dynamics
+        A_tran = np.block([[np.zeros((3, 3)),   np.identity(3)],
+                           [np.zeros((3, 3)), np.zeros((3, 3))]])
+
+        B_tran = np.matrix(np.block([[np.zeros((3, 3))],
+                                         [np.identity(3)]]))
+
+        A_tran_bar = A_tran # Estimated A_tran matrix
+        Lambda_bar = (1/mass_total_estimated) * np.identity(3) # Estimate of \Lambda
+        Theta_tran_adaptive_bar = air_density_estimated * surface_area_estimated * drag_coefficient_matrix_estimated
+
+        # **Translational** reference model parameters and estimates
+        A_ref_tran = np.block([[np.zeros((3, 3)),  np.identity(3)],
+                               [        -KP_tran,        -KD_tran]])
+
+        B_ref_tran = np.matrix(np.block([[np.zeros((3, 3))],
+                                         [(1/mass_total_estimated)*np.identity(3)]]))
+
+        # **Translational** adaptive parameters
+        Gamma_x_tran = np.matrix(5e2 * np.diag([1,1,10,1,1,10])) # Adaptive rates
+        Gamma_r_tran = np.matrix(1e0 * np.diag([1,1,1])) # Adaptive rates
+        Gamma_Theta_tran = np.matrix(1e0 * np.diag([1,1,1,1,1,1])) # Adaptive rates
+
+        # **Translational** parameters Lyapunov equation
+        Q_tran = np.matrix(4e-3 * np.diag([1,1,20,1,1,20]))
+        P_tran = np.matrix(linalg.solve_continuous_lyapunov(A_ref_tran.T, -Q_tran))
+        
+        # Estimates of the matrices that verify the matching conditions for the **Translational** dynamics.
+        # Used for baseline controller and to center constraining ellipsoid when using projection operator
+        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]))
+        K_r_tran_bar = (LA.pinv(B_tran*Lambda_bar)*B_ref_tran).T * np.matrix(1*np.diag([1,1,1]))
+
+        # ----------------------------------------------------------------
+        #                   Rotational Parameters MRAC
+        # ----------------------------------------------------------------
+
+        # Plant parameters **Rotational** dynamics
+        A_rot = np.matrix(np.zeros((3,3)))
+        B_rot = np.matrix(np.eye(3))
+
+        # **Rotational** reference model parameters
+        A_ref_rot = -K_P_omega_ref
+        B_ref_rot = np.matrix(np.eye(3))
+
+        # **Rotational** parameters Lyapunov equation
+        Q_rot = np.matrix(7e-3 * np.diag([1,1,1]))
+        P_rot = np.matrix(linalg.solve_continuous_lyapunov(A_ref_rot.T, -Q_rot))
+
+        # **Rotational** adaptive parameters
+        Gamma_x_rot = np.matrix(1e1 * np.diag([1,1,1])) # Adaptive rates
+        Gamma_r_rot = np.matrix(1e-4 * np.diag([1,1,1])) # Adaptive rates
+        Gamma_Theta_rot = np.matrix(1e0 * np.diag([1,1,1,1,1,1])) # Adaptive rates
+        
+        # ----------------------------------------------------------------
+        #                   Funnel Parameters MRAC
+        # ----------------------------------------------------------------
+        
+        eta_bar_funnel = 1
+        M_funnel = np.matrix(1e0 * np.diag([1,1,1,1,1,1])) # n x n
+        u_max = 85
+        u_min = 0
+        Delta_u_min = 5
+        nu_funnel = 0.01
+        
+        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,
+                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,
+                A_ref_tran,B_ref_tran,Gamma_x_tran,Gamma_r_tran,Gamma_Theta_tran,Gamma_Theta_tran,Q_tran,P_tran,K_x_tran_bar,K_r_tran_bar,A_rot,
+                B_rot,A_ref_rot,B_ref_rot,Q_rot,P_rot,Gamma_x_rot,Gamma_r_rot,Gamma_Theta_rot,eta_bar_funnel,M_funnel,u_max,u_min,
+                Delta_u_min,nu_funnel]
+    # ================================================================================================================================================================
+    # end of FUNNEL MRAC WITH BASELINE
+    # ================================================================================================================================================================
+
+   
+   
+