Skip to content
The Chat Archive
gpt-4.1-mini-2025-04-14· OpenAI LMArena Human Preference 140K

write a python code that uses MPC control, and three observers (to observe x1 and x2),…

  1. User

    write a python code that uses MPC control, and three observers (to observe x1 and x2), to be compared in plots, 1- Sliding Mode Observer 2- Kalman 3- luenberger observer the system defined by the differential equations : dx1 = (u2 - u1) * x3 / C1; dx2 = (u3 - u2) * x3 / C2; dx3 = ((u1 - u2)x1 + (u2 - u3)x2 + Eu3 - Rx3) / L; with the parameters : E = 60; C1 = 33e-6; C2 = 33e-6; L = 50e-3; R = 33; constraints: u1 u2 u3 take only discrete values 0 or 1, and have maximum switching frequency of 20kHz, only one input control can change at a time either u1 or u2 or u3, so at each state there will be 4 actions that can be taken : none, flip u1, flip u2, flip u3. x1_ref = 20.0; x2_ref = 40.0; x3_ref = 0.5; with some allowable margin of error ex1=2, ex2=2, ex3=0.05 plot the results x1 x2 x3 and the KPIs of the algorithm, and plot the 3d phase space

  2. Assistant

    Certainly! Below is a Python script that implements:

    • The system dynamics you provided,
    • An MPC controller with discrete inputs and switching constraints,
    • Three observers to estimate (x_1) and (x_2):
      1. Sliding Mode Observer (SMO),
      2. Kalman Filter,
      3. Luenberger Observer,
    • Comparison plots of the actual states and the three observers,
    • KPIs including final errors and control switch counts,
    • 3D phase space plot of the actual states.

    Notes on implementation:

    • The system is nonlinear due to multiplication of states and inputs.
    • For the Kalman filter and Luenberger observer, we linearize the system around the current operating point at each step (Extended Kalman Filter style).
    • The MPC controller enumerates all possible control moves over a short horizon (3 steps) respecting the "only one input flips at a time" constraint.
    • The observers estimate (x_1) and (x_2) only; (x_3) is assumed measured.
    • The observers use the measured (x_3) and control inputs (u).
    • The Kalman filter assumes process and measurement noise covariances (tunable).
    • The Luenberger observer uses a fixed gain matrix (L) (tuned manually).
    • The Sliding Mode Observer uses a high gain sliding term on (x_3) error.

    import numpy as np
    import matplotlib.pyplot as plt
    from mpl_toolkits.mplot3d import Axes3D
    
    # -------------------------------
    # Parameters and simulation setup
    # -------------------------------
    
    E  = 60.0
    C1 = 33e-6
    C2 = 33e-6
    L  = 50e-3
    R  = 33.0
    
    x1_ref = 20.0
    x2_ref = 40.0
    x3_ref = 0.5
    
    ex1 = 2.0
    ex2 = 2.0
    ex3 = 0.05
    
    dt = 1.0/20000.0  # 50 us timestep (20 kHz)
    T_end = 0.02
    N = int(T_end/dt)
    
    # MPC horizon
    horizon = 3
    
    # Sliding Mode Observer gain
    k_slide = 1e3
    
    # Kalman filter noise covariances (tune as needed)
    Q_kf = np.diag([1e-4, 1e-4])  # process noise covariance
    R_kf = np.diag([1e-3, 1e-3])  # measurement noise covariance
    
    # Luenberger observer gain (tuned manually)
    # 2 states, 2 outputs (x1, x2)
    L_luen = np.array([[5000, 0],
                       [0, 5000]])
    
    # -------------------------------
    # Helper functions
    # -------------------------------
    
    def plant_dynamics(x, u):
        x1, x2, x3 = x
        u1, u2, u3 = u
        dx1 = (u2 - u1) * x3 / C1
        dx2 = (u3 - u2) * x3 / C2
        dx3 = (((u1 - u2)*x1) + ((u2 - u3)*x2) + E*u3 - R*x3) / L
        return np.array([dx1, dx2, dx3])
    
    def euler_step(x, u, dt):
        return x + plant_dynamics(x, u) * dt
    
    def allowed_actions(u):
        u = np.array(u)
        actions = [tuple(u)]  # no change
        for i in range(3):
            u_new = u.copy()
            u_new[i] = 1 - u_new[i]
            actions.append(tuple(u_new))
        return actions
    
    def generate_candidate_sequences(u_current, horizon):
        if horizon == 0:
            return [[]]
        sequences = []
        possible_next = allowed_actions(u_current)
        for u_next in possible_next:
            for subseq in generate_candidate_sequences(u_next, horizon-1):
                sequences.append([u_next] + subseq)
        return sequences
    
    def mpc_cost(x0, seq):
        cost = 0.0
        x = np.array(x0)
        for u in seq:
            x = euler_step(x, u, dt)
            e1 = x[0] - x1_ref
            e2 = x[1] - x2_ref
            e3 = x[2] - x3_ref
            cost += e1**2 + e2**2 + e3**2
        return cost
    
    def linearize_system(x, u):
        """
        Linearize the system around current state x and input u.
        Returns A (2x2), B (2x3) matrices for states x1,x2 only.
        We consider x3 measured and known.
        """
        x1, x2, x3 = x
        u1, u2, u3 = u
    
        # Partial derivatives for dx1 and dx2 w.r.t x1 and x2
        # dx1 = (u2 - u1)*x3 / C1  -> no x1 or x2 dependence
        # dx2 = (u3 - u2)*x3 / C2  -> no x1 or x2 dependence
        # So A = zeros(2,2)
        A = np.zeros((2,2))
    
        # Partial derivatives of dx1 and dx2 w.r.t u1,u2,u3
        # But for observer design, B is input matrix for states
        # Here inputs are control inputs, but for observer design, we consider u as known input
        # So B is zero matrix (no direct input effect on states x1,x2)
        B = np.zeros((2,3))
    
        return A, B
    
    # -------------------------------
    # Initialization
    # -------------------------------
    
    # Actual states
    x = np.array([0.0, 0.0, 0.0])
    
    # Initial control inputs
    control = (0, 0, 0)
    
    # Observers initial states
    hat_smo = np.array([0.0, 0.0, 0.0])  # Sliding Mode Observer estimates x1,x2,x3
    hat_kf = np.array([0.0, 0.0])         # Kalman filter estimates x1,x2
    hat_luen = np.array([0.0, 0.0])       # Luenberger observer estimates x1,x2
    
    # Kalman filter initialization
    P_kf = np.eye(2) * 1.0
    
    # For KPIs
    switch_count = [0, 0, 0]
    
    # Data storage
    time_arr = np.zeros(N)
    x_arr = np.zeros((N,3))
    hat_smo_arr = np.zeros((N,3))
    hat_kf_arr = np.zeros((N,2))
    hat_luen_arr = np.zeros((N,2))
    u_arr = np.zeros((N,3))
    
    # -------------------------------
    # Simulation loop
    # -------------------------------
    
    for i in range(N):
        t = i*dt
        time_arr[i] = t
    
        # --- MPC Controller ---
        x0_pred = np.array([hat_smo[0], hat_smo[1], x[2]])  # use SMO estimates for x1,x2, measured x3
        candidate_seqs = generate_candidate_sequences(control, horizon)
    
        best_cost = np.inf
        best_seq = None
        for seq in candidate_seqs:
            cost = mpc_cost(x0_pred, seq)
            if cost < best_cost:
                best_cost = cost
                best_seq = seq
    
        chosen_control = best_seq[0] if best_seq is not None else control
    
        # Count switches
        for idx, (prev, new) in enumerate(zip(control, chosen_control)):
            if prev != new:
                switch_count[idx] += 1
    
        control = chosen_control
        u1, u2, u3 = control
        u_arr[i] = control
    
        # --- Plant update ---
        x = euler_step(x, control, dt)
        x_arr[i] = x
    
        # --- Sliding Mode Observer ---
        # States: x1,x2,x3 estimated
        # Use measured x3 and control inputs
        dx1_hat = (u2 - u1) * x[2] / C1
        dx2_hat = (u3 - u2) * x[2] / C2
        dx3_hat = (((u1 - u2)*hat_smo[0]) + ((u2 - u3)*hat_smo[1]) + E*u3 - R*hat_smo[2]) / L \
                  + k_slide * np.sign(x[2] - hat_smo[2])
    
        hat_smo += np.array([dx1_hat, dx2_hat, dx3_hat]) * dt
        hat_smo_arr[i] = hat_smo
    
        # --- Kalman Filter ---
        # State: x1,x2
        # Measurement: x1,x2 (assumed measured with noise)
        # Here we simulate measurement as actual x1,x2 (no noise added for simplicity)
        z = x[:2]
    
        # Linearize system for KF
        A, B = linearize_system(hat_kf, control)
        # Discretize A for dt (simple Euler)
        Ad = np.eye(2) + A*dt
        Bd = B*dt
    
        # Prediction
        hat_kf = Ad @ hat_kf  # no control input effect assumed
        P_kf = Ad @ P_kf @ Ad.T + Q_kf
    
        # Measurement update
        H = np.eye(2)
        y = z - H @ hat_kf
        S = H @ P_kf @ H.T + R_kf
        K = P_kf @ H.T @ np.linalg.inv(S)
        hat_kf = hat_kf + K @ y
        P_kf = (np.eye(2) - K @ H) @ P_kf
    
        hat_kf_arr[i] = hat_kf
    
        # --- Luenberger Observer ---
        # Linearized system same as KF
        # Output y = x1,x2 measured
        y_luen = x[:2]
        # Observer update: dx_hat/dt = A x_hat + L(y - x_hat)
        dx_luen = A @ hat_luen + L_luen @ (y_luen - hat_luen)
        hat_luen += dx_luen * dt
        hat_luen_arr[i] = hat_luen
    
    # -------------------------------
    # KPIs
    # -------------------------------
    
    final_errors = {
        'x1': abs(x1_ref - x_arr[-1,0]),
        'x2': abs(x2_ref - x_arr[-1,1]),
        'x3': abs(x3_ref - x_arr[-1,2]),
    }
    
    print("Final absolute errors:")
    for k,v in final_errors.items():
        print(f"  |{k}_ref - {k}| = {v:.3f}")
    
    print("\nControl switches:")
    print(f"  u1: {switch_count[0]}")
    print(f"  u2: {switch_count[1]}")
    print(f"  u3: {switch_count[2]}")
    
    # -------------------------------
    # Plotting
    # -------------------------------
    
    plt.figure(figsize=(14,10))
    
    # States x1,x2,x3
    plt.subplot(3,2,1)
    plt.plot(time_arr, x_arr[:,0], label='x1 actual')
    plt.plot(time_arr, hat_smo_arr[:,0], '--', label='SMO x1')
    plt.plot(time_arr, hat_kf_arr[:,0], ':', label='KF x1')
    plt.plot(time_arr, hat_luen_arr[:,0], '-.', label='Luenberger x1')
    plt.axhline(x1_ref, color='k', linestyle='--', alpha=0.3)
    plt.title('State x1')
    plt.xlabel('Time [s]')
    plt.ylabel('x1')
    plt.legend()
    plt.grid()
    
    plt.subplot(3,2,3)
    plt.plot(time_arr, x_arr[:,1], label='x2 actual')
    plt.plot(time_arr, hat_smo_arr[:,1], '--', label='SMO x2')
    plt.plot(time_arr, hat_kf_arr[:,1], ':', label='KF x2')
    plt.plot(time_arr, hat_luen_arr[:,1], '-.', label='Luenberger x2')
    plt.axhline(x2_ref, color='k', linestyle='--', alpha=0.3)
    plt.title('State x2')
    plt.xlabel('Time [s]')
    plt.ylabel('x2')
    plt.legend()
    plt.grid()
    
    plt.subplot(3,2,5)
    plt.plot(time_arr, x_arr[:,2], label='x3 actual')
    plt.axhline(x3_ref, color='k', linestyle='--', alpha=0.3)
    plt.title('State x3 (measured)')
    plt.xlabel('Time [s]')
    plt.ylabel('x3')
    plt.legend()
    plt.grid()
    
    # Control inputs
    plt.subplot(3,2,2)
    plt.step(time_arr, u_arr[:,0], label='u1', where='post')
    plt.step(time_arr, u_arr[:,1], label='u2', where='post')
    plt.step(time_arr, u_arr[:,2], label='u3', where='post')
    plt.title('Control Inputs')
    plt.xlabel('Time [s]')
    plt.ylabel('Control (0 or 1)')
    plt.legend()
    plt.grid()
    
    # 3D phase space plot
    ax = plt.subplot(3,2,4, projection='3d')
    ax.plot(x_arr[:,0], x_arr[:,1], x_arr[:,2], lw=1)
    ax.set_xlabel('x1')
    ax.set_ylabel('x2')
    ax.set_zlabel('x3')
    ax.set_title('3D Phase Space')
    
    # Observer errors (x1 and x2)
    plt.subplot(3,2,6)
    plt.plot(time_arr, np.abs(x_arr[:,0] - hat_smo_arr[:,0]), '--', label='SMO x1 error')
    plt.plot(time_arr, np.abs(x_arr[:,1] - hat_smo_arr[:,1]), '--', label='SMO x2 error')
    plt.plot(time_arr, np.abs(x_arr[:,0] - hat_kf_arr[:,0]), ':', label='KF x1 error')
    plt.plot(time_arr, np.abs(x_arr[:,1] - hat_kf_arr[:,1]), ':', label='KF x2 error')
    plt.plot(time_arr, np.abs(x_arr[:,0] - hat_luen_arr[:,0]), '-.', label='Luenberger x1 error')
    plt.plot(time_arr, np.abs(x_arr[:,1] - hat_luen_arr[:,1]), '-.', label='Luenberger x2 error')
    plt.title('Observer Absolute Errors')
    plt.xlabel('Time [s]')
    plt.ylabel('Absolute error')
    plt.legend()
    plt.grid()
    
    plt.tight_layout()
    plt.show()
    

    How to run

    • Requires numpy and matplotlib.
    • Run in any Python 3 environment.
    • The simulation runs for 20 ms with 50 µs steps.
    • The MPC controller tries to minimize the quadratic error over a 3-step horizon.
    • The observers estimate (x_1) and (x_2) and are compared in plots.

    What you will see

    • Actual states (x_1, x_2, x_3) over time.
    • Observer estimates for (x_1) and (x_2) from SMO, Kalman, and Luenberger.
    • Control inputs (u_1, u_2, u_3) over time.
    • 3D phase space of the actual states.
    • Absolute errors of each observer for (x_1) and (x_2).
    • Final errors and control switch counts printed in the console.

    This code provides a solid base for comparing the three observers under the same MPC control scheme on your nonlinear system with discrete inputs and switching constraints. You can tune observer gains, noise covariances, and MPC horizon to improve performance. Let me know if you want help with any extensions!

  3. User

    Traceback (most recent call last): File "c:\Users\Manifold\Documents\Python_Library\obs_cont.py", line 194, in hat_x, P = observer_func([hat_x1, hat_x2, hat_x3], y_measured, np.array([u1, u2, u3])) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ File "c:\Users\Manifold\Documents\Python_Library\obs_cont.py", line 125, in kalman_filter P_pred = A_disc @ P @ A_disc.T + Q_kalman # P is the covariance matrix ^

  4. Assistant

    The error you encountered:

    P_pred = A_disc @ P @ A_disc.T + Q_kalman  # P is the covariance matrix
    

    suggests a dimension mismatch in matrix multiplication. This usually happens when the dimensions of A_disc, P, or Q_kalman do not align.


    Common causes in your context:

    • A_disc is likely a 2x2 matrix (since Kalman filter estimates only (x_1, x_2)),
    • P should be 2x2 covariance matrix,
    • Q_kalman should also be 2x2.

    If any of these are not 2x2, the multiplication will fail.


    How to fix:

    1. Check dimensions of A_disc, P, and Q_kalman:

    Make sure:

    • A_disc.shape == (2, 2)
    • P.shape == (2, 2)
    • Q_kalman.shape == (2, 2)
    1. Ensure P is initialized as a 2x2 matrix:
    P = np.eye(2) * 1.0  # covariance matrix for x1,x2
    
    1. Ensure Q_kalman and R_kalman are 2x2:
    Q_kalman = np.diag([1e-4, 1e-4])
    R_kalman = np.diag([1e-3, 1e-3])
    
    1. Make sure A_disc is computed correctly:

    If you discretize A with Euler:

    A_disc = np.eye(2) + A * dt
    

    where A is 2x2.


    Example snippet for Kalman filter update (simplified):

    # Linearize system around current estimate
    A, B = linearize_system(hat_kf, control)  # A is 2x2
    
    # Discretize A
    A_disc = np.eye(2) + A * dt
    
    # Prediction
    hat_kf = A_disc @ hat_kf  # hat_kf shape (2,)
    P_pred = A_disc @ P @ A_disc.T + Q_kalman  # P shape (2,2)
    
    # Measurement update
    H = np.eye(2)  # measurement matrix
    z = measured_states  # shape (2,)
    y = z - H @ hat_kf
    S = H @ P_pred @ H.T + R_kalman
    K = P_pred @ H.T @ np.linalg.inv(S)
    hat_kf = hat_kf + K @ y
    P = (np.eye(2) - K @ H) @ P_pred
    

    If you want me to help debug your exact code, please share the relevant Kalman filter function and how you define A, P, and Q_kalman. Otherwise, check the above points carefully.


    Summary:

    • The error is a matrix dimension mismatch.
    • Ensure all matrices involved in P_pred = A_disc @ P @ A_disc.T + Q_kalman are 2x2.
    • Initialize and update P properly.
    • Discretize A correctly.

    If you want, I can help you rewrite the Kalman filter part to avoid this error. Just let me know!

Responses were produced in different, uncontrolled contexts (system prompts, settings, prior turns). This is not a benchmark.