feature: Weighting matrices from the Elhesasy paper achieve improveme…

…nt in steady state accuracy of MPC. It is close to PID

Some random samples:
x +00.10, y +00.12, z +00.80 ——— velocity -00.02, +00.00, +00.00

x +00.10, y +00.11, z +00.80 ——— velocity +00.01, -00.04, -00.00

x +00.10, y +00.11, z +00.80 ——— velocity +00.00, -00.04, +00.00

gym_pybullet_drones/control/MPCCasADiControl.py

@@ -169,10 +169,13 @@ def __init__(self,
         J = 0  # Objective function
         g = []  # constraints vector
 
-        # Q=diag(MX([100, 100, 100, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
+        '''# Q=diag(MX([100, 100, 100, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
         Q = diagcat(100, 100, 100, 1, 1, 1, 1, 1, 1, 1, 1, 1)
         # R=diag(MX([10.0, 10.0, 10.0, 10.0]))
-        R = diagcat(10.0, 10.0, 10.0, 10.0)
+        R = diagcat(10.0, 10.0, 10.0, 10.0)'''
+
+        Q = diagcat(1, 1, 1, 0.6, 0.6, 1, 0, 0, 0, 0, 0, 0)
+        R = diagcat(0.3, 0.3, 0.3, 0.8)
 
         x_init = P[0:Nx]
         g = X[:, 0] - P[0:Nx]  # initial condition constraints