Merge pull request #9 from idsc-frazzoli/ps04

v0.4.1

build/lib/cs2solutions/__init__.py

@@ -1 +1 @@
-__all__ = ["test", "intro", "aircraft", "cs2bot", "discretization", "morse", "decomp", "lqrfeedback", "duckiebot", "plot"]
+__all__ = ["cs", "test", "intro", "aircraft", "cs2bot", "discretization", "morse", "decomp", "lqrfeedback", "duckiebot", "plot"]

build/lib/cs2solutions/aircraft.py

@@ -65,7 +65,7 @@ def test_aircraft_state_space(student_sol: callable, actual_sol: callable, shoul
     sol_model = actual_sol()
 
     # Check if the student's state space model is an instance of the StateSpace class
-    assert isinstance(student_model, type(None)), f"Please make sure to return a StateSpace object. Got {type(student_model)} instead."
+    assert not isinstance(student_model, type(None)), f"Please make sure to return a StateSpace object. Got {type(student_model)} instead."
     assert isinstance(student_model, ct.StateSpace), f"Expected a StateSpace object, but got {type(student_model)}"
 
     # Check if the student's state space model is equal to the solution state space model
@@ -129,21 +129,22 @@ def test_is_system_stable(student_sol: callable, actual_sol: callable, shouldpri
 
     # Test 2: Marginally stable system
     sys_marginally_stable = ct.StateSpace(np.array([[0, 1], [-1, 0]]), np.array([[1], [0]]), np.array([[0, 1]]), np.array([[0]]))
-    result = student_sol(sys_unstable)
+    result = student_sol(sys_marginally_stable)
     print("Student's result: ", result)
     print("Expected result: ", actual_sol(sys_marginally_stable))
     passed_tests += 1 if result == actual_sol(sys_marginally_stable) else 0
 
     # Test 3: Stable system
     sys_stable = ct.StateSpace(np.array([[-0.5, 0], [0, -1]]), np.array([[1], [0]]), np.array([[0, 1]]), np.array([[0]]))
-    result = student_sol(sys_unstable)
+    result = student_sol(sys_stable)
     print("Student's result: ", result)
     print("Expected result: ", actual_sol(sys_stable))
     passed_tests += 1 if result == actual_sol(sys_stable) else 0
 
     # Test 4: Check aircraft stability
     sys_aircraft = sol_aircraft_state_space()
     result = student_sol(sys_aircraft)
+    print("Student's result: ", result)
     print("Expected result: ", actual_sol(sys_aircraft))
     passed_tests += 1 if result == actual_sol(sys_aircraft) else 0
 

build/lib/cs2solutions/duckiebot.py

@@ -54,7 +54,7 @@ def plot_track(x_coord_ref: np.array, y_coord_ref: np.array,
     plt.subplot(2, 2, 2)
     plt.plot(t, y_coord_ref)
     if y_ctr is not None:
-        plt.plot(t_curvy, y_ctr, 'r')
+        plt.plot(t, y_ctr, 'r')
         plt.legend(['reference', 'controller'])
     else:
         plt.legend(['reference'])
@@ -64,7 +64,7 @@ def plot_track(x_coord_ref: np.array, y_coord_ref: np.array,
     plt.subplot(2, 2, 4)
     plt.plot(t, w_curvy)
     if w_ctr is not None:
-        plt.plot(t_curvy, w_ctr, 'r')
+        plt.plot(t, w_ctr, 'r')
         plt.legend(['reference', 'controller'])
     else:
         plt.legend(['reference'])
@@ -124,6 +124,93 @@ def plot_track_multiple_controller(x_coord_ref: np.array, y_coord_ref: np.array,
     plt.xlabel('Time t [sec]')
     plt.tight_layout()
 
+# Updated plot_track_multiple_controller function specifically for the notebook "CS2-2024-unicycle-state-estimation.ipynb"
+def plot_track_multiple_controller2(x_coord_ref: np.array, y_coord_ref: np.array,
+               theta_ref: np.array, t: List[np.array],
+               w_curvy: np.array,
+               y_ctr: List[np.array],
+               w_ctr: Optional[np.array]) -> None:
+    # Configure matplotlib plots to be a bit bigger and optimize layout
+    number_ctr = len(t)
+    plt.figure(figsize=[14, 6])
+    # Plot the resulting trajectory (and some road boundaries)
+
+    plt.subplot(1, 4, 2)
+    plt.plot(y_coord_ref, x_coord_ref)
+    plt.legend(['reference'])
+    legend_list = ['reference']
+    for i in range(number_ctr):
+      plt.plot(y_ctr[i], x_coord_ref, linewidth=1)
+      if number_ctr == 4:
+        if i == 0:
+          legend_list.append('aggressive_controller')
+        elif i == 1:
+          legend_list.append('easy_controller')
+        elif i == 2:
+          legend_list.append('complex_1_controller')
+        elif i == 3:
+          legend_list.append('complex_2_controller')
+        else:
+          legend_list.append(f'controller_{i-4}')
+      else:
+        legend_list.append(f'controller_{i+1}')
+
+    plt.legend(legend_list, loc='upper left')
+    plt.plot(y_coord_ref - 0.9/np.cos(theta_ref), x_coord_ref, 'k-', linewidth=1)
+    plt.plot(y_coord_ref - 0.3/np.cos(theta_ref), x_coord_ref, 'k--', linewidth=1)
+    plt.plot(y_coord_ref + 0.3/np.cos(theta_ref), x_coord_ref, 'k-', linewidth=1)
+
+
+
+    plt.xlabel('y [m]')
+    plt.ylabel('x [m]');
+    plt.axis('Equal')
+
+    # Plot the lateral position
+    plt.subplot(2, 2, 2)
+    plt.plot(t[0], y_coord_ref)
+    legend_list = ['reference']
+    for i in range(number_ctr):
+      plt.plot(t[i], y_ctr[i], linewidth=1)
+      if number_ctr == 4:
+        if i == 0:
+          legend_list.append('aggressive_controller')
+        elif i == 1:
+          legend_list.append('easy_controller')
+        elif i == 2:
+          legend_list.append('complex_1_controller')
+        elif i == 3:
+          legend_list.append('complex_2_controller')
+        else:
+          legend_list.append(f'controller_{i-4}')
+      else:
+        legend_list.append(f'controller_{i+1}')
+    plt.legend(legend_list)
+    plt.ylabel('Lateral position $y$ [m]')
+
+    # Plot the control input
+    plt.subplot(2, 2, 4)
+    plt.plot(t[0], w_curvy)
+    for i in range(number_ctr):
+      plt.plot(t[i], y_ctr[i], linewidth=1)
+      if number_ctr == 4:
+        if i == 0:
+          legend_list.append('aggressive_controller')
+        elif i == 1:
+          legend_list.append('easy_controller')
+        elif i == 2:
+          legend_list.append('complex_1_controller')
+        elif i == 3:
+          legend_list.append('complex_2_controller')
+        else:
+          legend_list.append(f'controller_{i-4}')
+      else:
+        legend_list.append(f'controller_{i+1}')
+    plt.legend(legend_list)
+    plt.ylabel('$\\omega$ [rad/s]')
+    plt.xlabel('Time t [sec]')
+    plt.tight_layout()
+
 # Utility function to plot the step response
 def plot_step_response(t: np.array, y: np.array, u: np.array) -> None:
     axes_out = plt.subplot(2, 1, 1)

build/lib/cs2solutions/lqrfeedback.py

@@ -112,7 +112,6 @@ def sol_control_matrix2(A: np.ndarray, B: np.ndarray) -> np.ndarray:
 
     return R
 
-
 def sol_2x2_ackermann(A: np.ndarray, B: np.ndarray, C:np.ndarray, p_1: float, p_2: float) -> Tuple[np.ndarray, np.ndarray, float, np.ndarray, np.ndarray]:
     """
     Calculates and outputs all steps of the Ackermann formula in one go. Only works for 2x2 matrices.
@@ -168,6 +167,41 @@ def sol_2x2_ackermann(A: np.ndarray, B: np.ndarray, C:np.ndarray, p_1: float, p_
 
     return p_cl, K, k_r, A_cl, B_cl
 
+def sol_2x2_acker_estimation(A: np.ndarray, C: np.ndarray, poles: List[float]) -> np.ndarray:
+    """
+    Calculates the observer gain matrix L for a 2x2 system.
+
+    Parameters:
+    - ``A`` (np.array): The state matrix.
+    - ``C`` (np.array): The output matrix.
+    - ``poles`` (List[float]): The poles of the observer.
+
+    Returns:
+    - ``L```(np.array): The observer gain matrix.
+    """
+    # Safety
+    assert isinstance(A, np.ndarray), "A must be a NumPy array"
+    assert isinstance(C, np.ndarray), "C must be a NumPy array"
+    assert A.shape[0] == A.shape[1], "A must be a square matrix"
+    assert A.shape[0] == C.shape[1], "A and C must have compatible dimensions"
+    assert A.shape[0] == 2, "A must have dimensions larger than 0"
+    assert len(poles) == 2, "poles must be a list of length 2"
+    N = 2  # Number of states
+
+    # Calculate the observer gain matrix L
+    CA = C @ A
+    O = np.concatenate((C, CA), axis=0)
+    O_inv = np.linalg.inv(O)
+    gamma = O_inv @ np.array([[0, 1]]).T
+
+    p_1 = poles[0]*(-1)
+    p_2 = poles[1]*(-1)
+    ab = p_1 + p_2
+    b = p_1*p_2
+    p_cl = A @ A + ab*A + b*np.identity(2)
+
+    L = p_cl @ gamma
+    return L
 
 def sol_check_pos_def_sym(A: np.ndarray) -> bool:
     """

pyproject.toml

@@ -7,7 +7,7 @@ packages = ["src/solutions"]
 
 [project]
 name = "cs2solutions"
-version = "0.4.0"
+version = "0.4.1"
 dependencies =[
     "control",
     "matplotlib",

src/cs2solutions.egg-info/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: cs2solutions
-Version: 0.4.0
+Version: 0.4.1
 Summary: A package containing solutions for the CS2 lecture at ETH Zürich
 Author-email: Kalle Laitinen <[email protected]>, Dejan Milojevic <[email protected]>, Niclas Scheuer <[email protected]>
 Maintainer-email: Kalle Laitinen <[email protected]>, Niclas Scheuer <[email protected]>

src/cs2solutions.egg-info/SOURCES.txt

@@ -3,6 +3,7 @@ README.md
 pyproject.toml
 src/cs2solutions/__init__.py
 src/cs2solutions/aircraft.py
+src/cs2solutions/cs.py
 src/cs2solutions/cs2bot.py
 src/cs2solutions/decomp.py
 src/cs2solutions/discretization.py

src/cs2solutions/__init__.py

@@ -1 +1 @@
-__all__ = ["test", "intro", "aircraft", "cs2bot", "discretization", "morse", "decomp", "lqrfeedback", "duckiebot", "plot"]
+__all__ = ["cs", "test", "intro", "aircraft", "cs2bot", "discretization", "morse", "decomp", "lqrfeedback", "duckiebot", "plot"]

src/cs2solutions/decomp.py

@@ -223,11 +223,13 @@ def test_controllable(student_sol: callable, actual_sol: callable, shouldprint:
     print("Student solution:")
     try:
         student1 = student_sol(A1, B1)
+        print(student1)
     except Exception as e:
         print("Error in controllable:", e)
         student1 = None
     print("Master solution:")
     solution1 = actual_sol(A1, B1)
+    print(solution1)
     passed_tests += 1 if student1 == solution1 else 0
 
     # Controllable system
@@ -236,11 +238,13 @@ def test_controllable(student_sol: callable, actual_sol: callable, shouldprint:
     print("Student solution:")
     try:
         student2 = student_sol(A2, B2)
+        print(student2)
     except Exception as e:
         print("Error in controllable:", e)
         student2 = None
     print("Master solution:")
     solution2 = actual_sol(A2, B2)
+    print(solution2)
     passed_tests += 1 if student2 == solution2 else 0
 
     print("Passed tests:", passed_tests, " out of 2")
@@ -267,11 +271,13 @@ def test_observable(student_sol: callable, actual_sol: callable, shouldprint: bo
     print("Student solution:")
     try:
         student1 = student_sol(A1, C1)
+        print(student1)
     except Exception as e:
         print("Error in observable:", e)
         student1 = None
     print("Master solution:")
     solution1 = actual_sol(A1, C1)
+    print(solution1)
     passed_tests += 1 if student1 == solution1 else 0
 
     # Unobservable system
@@ -280,11 +286,13 @@ def test_observable(student_sol: callable, actual_sol: callable, shouldprint: bo
     print("Student solution:")
     try:
         student2 = student_sol(A2, C2)
+        print(student2)
     except Exception as e:
         print("Error in observable:", e)
         student2 = None
     print("Master solution:")
     solution2 = actual_sol(A2, C2)
+    print(solution2)
     passed_tests += 1 if student2 == solution2 else 0
 
     print("Passed tests:", passed_tests, " out of 2")

src/cs2solutions/duckiebot.py

@@ -54,7 +54,7 @@ def plot_track(x_coord_ref: np.array, y_coord_ref: np.array,
     plt.subplot(2, 2, 2)
     plt.plot(t, y_coord_ref)
     if y_ctr is not None:
-        plt.plot(t_curvy, y_ctr, 'r')
+        plt.plot(t, y_ctr, 'r')
         plt.legend(['reference', 'controller'])
     else:
         plt.legend(['reference'])
@@ -64,7 +64,7 @@ def plot_track(x_coord_ref: np.array, y_coord_ref: np.array,
     plt.subplot(2, 2, 4)
     plt.plot(t, w_curvy)
     if w_ctr is not None:
-        plt.plot(t_curvy, w_ctr, 'r')
+        plt.plot(t, w_ctr, 'r')
         plt.legend(['reference', 'controller'])
     else:
         plt.legend(['reference'])
@@ -124,6 +124,93 @@ def plot_track_multiple_controller(x_coord_ref: np.array, y_coord_ref: np.array,
     plt.xlabel('Time t [sec]')
     plt.tight_layout()
 
+# Updated plot_track_multiple_controller function specifically for the notebook "CS2-2024-unicycle-state-estimation.ipynb"
+def plot_track_multiple_controller2(x_coord_ref: np.array, y_coord_ref: np.array,
+               theta_ref: np.array, t: List[np.array],
+               w_curvy: np.array,
+               y_ctr: List[np.array],
+               w_ctr: Optional[np.array]) -> None:
+    # Configure matplotlib plots to be a bit bigger and optimize layout
+    number_ctr = len(t)
+    plt.figure(figsize=[14, 6])
+    # Plot the resulting trajectory (and some road boundaries)
+
+    plt.subplot(1, 4, 2)
+    plt.plot(y_coord_ref, x_coord_ref)
+    plt.legend(['reference'])
+    legend_list = ['reference']
+    for i in range(number_ctr):
+      plt.plot(y_ctr[i], x_coord_ref, linewidth=1)
+      if number_ctr == 4:
+        if i == 0:
+          legend_list.append('aggressive_controller')
+        elif i == 1:
+          legend_list.append('easy_controller')
+        elif i == 2:
+          legend_list.append('complex_1_controller')
+        elif i == 3:
+          legend_list.append('complex_2_controller')
+        else:
+          legend_list.append(f'controller_{i-4}')
+      else:
+        legend_list.append(f'controller_{i+1}')
+
+    plt.legend(legend_list, loc='upper left')
+    plt.plot(y_coord_ref - 0.9/np.cos(theta_ref), x_coord_ref, 'k-', linewidth=1)
+    plt.plot(y_coord_ref - 0.3/np.cos(theta_ref), x_coord_ref, 'k--', linewidth=1)
+    plt.plot(y_coord_ref + 0.3/np.cos(theta_ref), x_coord_ref, 'k-', linewidth=1)
+
+
+
+    plt.xlabel('y [m]')
+    plt.ylabel('x [m]');
+    plt.axis('Equal')
+
+    # Plot the lateral position
+    plt.subplot(2, 2, 2)
+    plt.plot(t[0], y_coord_ref)
+    legend_list = ['reference']
+    for i in range(number_ctr):
+      plt.plot(t[i], y_ctr[i], linewidth=1)
+      if number_ctr == 4:
+        if i == 0:
+          legend_list.append('aggressive_controller')
+        elif i == 1:
+          legend_list.append('easy_controller')
+        elif i == 2:
+          legend_list.append('complex_1_controller')
+        elif i == 3:
+          legend_list.append('complex_2_controller')
+        else:
+          legend_list.append(f'controller_{i-4}')
+      else:
+        legend_list.append(f'controller_{i+1}')
+    plt.legend(legend_list)
+    plt.ylabel('Lateral position $y$ [m]')
+
+    # Plot the control input
+    plt.subplot(2, 2, 4)
+    plt.plot(t[0], w_curvy)
+    for i in range(number_ctr):
+      plt.plot(t[i], y_ctr[i], linewidth=1)
+      if number_ctr == 4:
+        if i == 0:
+          legend_list.append('aggressive_controller')
+        elif i == 1:
+          legend_list.append('easy_controller')
+        elif i == 2:
+          legend_list.append('complex_1_controller')
+        elif i == 3:
+          legend_list.append('complex_2_controller')
+        else:
+          legend_list.append(f'controller_{i-4}')
+      else:
+        legend_list.append(f'controller_{i+1}')
+    plt.legend(legend_list)
+    plt.ylabel('$\\omega$ [rad/s]')
+    plt.xlabel('Time t [sec]')
+    plt.tight_layout()
+
 # Utility function to plot the step response
 def plot_step_response(t: np.array, y: np.array, u: np.array) -> None:
     axes_out = plt.subplot(2, 1, 1)