你们用算法算遍了一切，就是没算到我不要命

LQR gain K (u = -K x_hat): [[ -89.15315449 2466.23833163]]
Observer gain L: [[-18.68235294  -5.21176471]
 [  6.17805348  -0.15482353]]

import numpy as np
from dataclasses import dataclass
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt

# -----------------------------
# 1) Vehicle model parameters
# -----------------------------
@dataclass
class VehicleParams:
    m: float = 1400.0      # kg
    Iz: float = 2500.0     # kg*m^2
    a: float = 1.1         # m  CG -> front axle
    b: float = 1.5         # m  CG -> rear axle
    Cf: float = 80000.0    # N/rad (front cornering stiffness)
    Cr: float = 90000.0    # N/rad (rear cornering stiffness)
    vx: float = 20.0       # m/s (constant longitudinal speed)

    # "low mu" effect: reduce cornering stiffness
    mu_scale: float = 0.45 # 1.0=dry, smaller=slippery

    @property
    def L(self):
        return self.a + self.b

def state_space_matrices(p: VehicleParams):
    """
    Linear bicycle model with states x=[vy, r]^T
    Inputs: delta (steering), Mz (additional yaw moment from ESP)
    """
    m, Iz, a, b, vx = p.m, p.Iz, p.a, p.b, p.vx
    Cf = p.Cf * p.mu_scale
    Cr = p.Cr * p.mu_scale

    # A matrix components (standard linear bicycle model)
    A11 = -(Cf + Cr) / (m * vx)
    A12 = -vx - (a*Cf - b*Cr) / (m * vx)
    A21 = -(a*Cf - b*Cr) / (Iz * vx)
    A22 = -(a*a*Cf + b*b*Cr) / (Iz * vx)

    A = np.array([[A11, A12],
                  [A21, A22]])

    # steering input
    B_delta = np.array([[Cf / m],
                        [a*Cf / Iz]])

    # yaw moment input
    B_M = np.array([[0.0],
                    [1.0 / Iz]])

    return A, B_delta, B_M

# -----------------------------
# 2) Driver steering profile
# -----------------------------
def steering_delta(t, delta_step=0.05, t0=1.0):
    """
    Step steering input (rad). 0.05 rad ~ 2.86 deg
    """
    return delta_step if t >= t0 else 0.0

# -----------------------------
# 3) ESP controller (yaw-rate feedback)
# -----------------------------
@dataclass
class ESPGains:
    Kp: float = 3500.0     # N*m per (rad/s)
    Kd: float = 500.0      # N*m per (rad/s)
    Mz_max: float = 2500.0 # N*m saturation (represents brake capability)

def esp_control(t, x, p: VehicleParams, g: ESPGains):
    """
    Compute additional yaw moment Mz.
    x = [vy, r]
    """
    vy, r = x
    delta = steering_delta(t)

    # simple reference yaw rate
    r_ref = (p.vx / p.L) * delta

    e = r_ref - r
    Mz = g.Kp * e - g.Kd * r

    # saturation
    Mz = np.clip(Mz, -g.Mz_max, g.Mz_max)
    return Mz, r_ref

# -----------------------------
# 4) ODE definitions
# -----------------------------
def ode_no_esp(t, x, p: VehicleParams):
    A, B_delta, B_M = state_space_matrices(p)
    delta = steering_delta(t)
    dx = (A @ x.reshape(-1, 1) + B_delta * delta).flatten()
    return dx

def ode_with_esp(t, x, p: VehicleParams, g: ESPGains, log):
    A, B_delta, B_M = state_space_matrices(p)
    delta = steering_delta(t)
    Mz, r_ref = esp_control(t, x, p, g)

    dx = (A @ x.reshape(-1, 1) + B_delta * delta + B_M * Mz).flatten()

    # logging
    log["t"].append(t)
    log["delta"].append(delta)
    log["Mz"].append(Mz)
    log["r_ref"].append(r_ref)
    log["vy"].append(x[0])
    log["r"].append(x[1])

    return dx

# -----------------------------
# 5) Run simulation
# -----------------------------
def run():
    # try a "slippery road"
    p = VehicleParams(vx=22.0, mu_scale=0.35)  # lower mu => harder to control
    g = ESPGains(Kp=4500.0, Kd=700.0, Mz_max=2800.0)

    t_span = (0.0, 6.0)
    t_eval = np.linspace(t_span[0], t_span[1], 1200)

    x0 = np.array([0.0, 0.0]) # start straight

    # Case 1: no ESP
    sol1 = solve_ivp(lambda t, x: ode_no_esp(t, x, p),
                     t_span, x0, t_eval=t_eval, rtol=1e-7, atol=1e-9)

    # Case 2: with ESP (log everything)
    log = {"t": [], "delta": [], "Mz": [], "r_ref": [], "vy": [], "r": []}
    sol2 = solve_ivp(lambda t, x: ode_with_esp(t, x, p, g, log),
                     t_span, x0, t_eval=t_eval, rtol=1e-7, atol=1e-9)

    # postprocess
    vy1, r1 = sol1.y[0], sol1.y[1]
    vy2, r2 = sol2.y[0], sol2.y[1]
    beta1 = vy1 / p.vx
    beta2 = vy2 / p.vx

    # For controller logs, resample to t_eval roughly (simple interpolation)
    tlog = np.array(log["t"])
    Mzlog = np.array(log["Mz"])
    rreflog = np.array(log["r_ref"])
    # interpolation to t_eval
    Mz_i = np.interp(t_eval, tlog, Mzlog)
    rref_i = np.interp(t_eval, tlog, rreflog)
    delta_i = np.array([steering_delta(t) for t in t_eval])

    # -----------------------------
    # 6) Plot
    # -----------------------------
    plt.figure()
    plt.plot(t_eval, r1, label="r (no ESP)")
    plt.plot(t_eval, r2, label="r (with ESP)")
    plt.plot(t_eval, rref_i, label="r_ref")
    plt.xlabel("Time (s)")
    plt.ylabel("Yaw rate r (rad/s)")
    plt.legend()
    plt.grid(True)

    plt.figure()
    plt.plot(t_eval, beta1, label="beta ~ vy/vx (no ESP)")
    plt.plot(t_eval, beta2, label="beta ~ vy/vx (with ESP)")
    plt.xlabel("Time (s)")
    plt.ylabel("Side-slip beta (rad)")
    plt.legend()
    plt.grid(True)

    plt.figure()
    plt.plot(t_eval, delta_i, label="steering delta (rad)")
    plt.xlabel("Time (s)")
    plt.ylabel("delta (rad)")
    plt.legend()
    plt.grid(True)

    plt.figure()
    plt.plot(t_eval, Mz_i, label="ESP yaw moment Mz (N*m)")
    plt.xlabel("Time (s)")
    plt.ylabel("Mz (N*m)")
    plt.legend()
    plt.grid(True)

    plt.show()

if __name__ == "__main__":
    run()

import numpy as np
from dataclasses import dataclass
from scipy.integrate import solve_ivp
from scipy.linalg import solve_continuous_are
from scipy.signal import cont2discrete
import matplotlib.pyplot as plt

# =========================
# 1) Parameters
# =========================
@dataclass
class VehicleParams:
    m: float = 1400.0         # kg
    Iz: float = 2500.0        # kg*m^2
    a: float = 1.1            # m  CG->front axle
    b: float = 1.5            # m  CG->rear axle
    track: float = 1.55       # m  track width
    h: float = 0.55           # m  CG height (for load transfer)
    vx: float = 22.0          # m/s constant longitudinal speed
    Cf: float = 80000.0       # N/rad (linear cornering stiffness)
    Cr: float = 90000.0       # N/rad
    mu: float = 0.35          # friction coefficient (slippery)
    g: float = 9.81

    @property
    def L(self) -> float:
        return self.a + self.b

@dataclass
class ControlParams:
    # LQR on estimated state xhat=[vy, r] -> u = Mz_des
    q_vy: float = 2.0
    q_r: float = 60.0
    r_u: float = 3e-6
    Mz_max: float = 2800.0        # N*m "controller desire" saturation

    # brake allocation
    front_bias: float = 0.6       # portion of side brake force on front axle
    max_brake_per_wheel: float = 6000.0  # N (cap by brake system)

@dataclass
class SensorParams:
    # measurement noise (1-sigma)
    sigma_r: float = 0.01         # rad/s
    sigma_ay: float = 0.2         # m/s^2

@dataclass
class EKFParams:
    dt: float = 0.005
    # process noise covariance (tune)
    q_vy: float = 0.5
    q_r: float = 0.5
    # initial covariance
    p0_vy: float = 1.0
    p0_r: float = 0.2

# =========================
# 2) Driver steering profile
# =========================
def steering_delta(t, step=0.06, t0=1.0):
    # 0.06 rad ~ 3.4 deg
    return step if t >= t0 else 0.0

# =========================
# 3) Linear bicycle matrices (for LQR + EKF linearization)
# =========================
def bicycle_mats(p: VehicleParams):
    m, Iz, a, b, vx = p.m, p.Iz, p.a, p.b, p.vx
    Cf, Cr = p.Cf, p.Cr

    A11 = -(Cf + Cr) / (m * vx)
    A12 = -vx - (a*Cf - b*Cr) / (m * vx)
    A21 = -(a*Cf - b*Cr) / (Iz * vx)
    A22 = -(a*a*Cf + b*b*Cr) / (Iz * vx)

    A = np.array([[A11, A12],
                  [A21, A22]])

    B_delta = np.array([[Cf / m],
                        [a*Cf / Iz]])

    # "ideal" yaw moment input
    B_M = np.array([[0.0],
                    [1.0 / Iz]])

    # measurement y = [r, ay]
    # ay = d(vy)/dt + vx*r
    C_ay = A[0, :] + np.array([0.0, vx])
    D_ay_delta = B_delta[0, 0]

    C = np.array([
        [0.0, 1.0],
        C_ay
    ])
    D = np.array([
        [0.0],
        [D_ay_delta]
    ])
    return A, B_delta, B_M, C, D

def lqr_gain(A, B, Q, R):
    P = solve_continuous_are(A, B, Q, R)
    K = np.linalg.solve(R, B.T @ P)
    return K

# =========================
# 4) Nonlinear-ish tire: friction circle saturation
# =========================
def normal_loads(p: VehicleParams, ax: float):
    """
    Simple longitudinal load transfer only.
    ax: longitudinal acceleration (negative for braking)
    """
    m, g, h, L = p.m, p.g, p.h, p.L
    a, b = p.a, p.b

    Fz_front_static = m*g*(b/L)
    Fz_rear_static  = m*g*(a/L)

    dFz = m*ax*h/L  # braking ax<0 -> dFz<0 -> front load increases by -dFz
    Fz_front = Fz_front_static - dFz
    Fz_rear  = Fz_rear_static + dFz

    # per wheel (left/right split, no lateral transfer in this simple model)
    return (Fz_front/2, Fz_front/2, Fz_rear/2, Fz_rear/2)  # FL, FR, RL, RR

def lateral_forces_linear(p: VehicleParams, vy: float, r: float, delta: float):
    """
    Axle lateral forces from linear bicycle model:
    returns Fy_front_total, Fy_rear_total
    """
    vx = p.vx
    a, b = p.a, p.b
    Cf, Cr = p.Cf, p.Cr

    alpha_f = delta - (vy + a*r)/vx
    alpha_r = -(vy - b*r)/vx

    Fy_f = Cf * alpha_f
    Fy_r = Cr * alpha_r
    return Fy_f, Fy_r

def apply_friction_circle(Fx, Fy, muFz):
    """
    Saturate (Fx, Fy) so that sqrt(Fx^2 + Fy^2) <= muFz
    Keep direction as much as possible (scale if needed).
    """
    mag = np.sqrt(Fx*Fx + Fy*Fy)
    if mag <= muFz or muFz <= 0:
        return Fx, Fy
    scale = muFz / (mag + 1e-12)
    return Fx*scale, Fy*scale

# =========================
# 5) Brake allocation: Mz_des -> four wheel brake forces Fx_i (negative)
# =========================
def allocate_brakes_from_Mz(p: VehicleParams, c: ControlParams, Mz_des: float):
    """
    Use side braking to generate yaw moment:
    Mz = sum(y_i * Fx_i), y_left=+t/2, y_right=-t/2, Fx_i negative for braking.
    To get positive Mz, brake RIGHT side (y=-t/2, Fx<0 => Mz positive).

    Allocation:
      - choose one side based on sign(Mz_des)
      - split to front/rear by front_bias
      - per-wheel cap
    Return Fx_FL, Fx_FR, Fx_RL, Fx_RR (negative for braking)
    """
    t = p.track
    if abs(Mz_des) < 1e-6:
        return 0.0, 0.0, 0.0, 0.0

    # choose side
    brake_right = (Mz_des > 0.0)
    y = -t/2 if brake_right else +t/2  # y of that side wheels

    # need sum Fx_side such that Mz_des = y * sumFx_side
    sumFx_side = Mz_des / (y + 1e-12)  # should be negative (braking)
    # enforce braking direction (negative)
    sumFx_side = min(sumFx_side, 0.0)

    Fx_front = c.front_bias * sumFx_side
    Fx_rear  = (1.0 - c.front_bias) * sumFx_side

    # per-wheel
    Fx_front_each = np.clip(Fx_front, -2*c.max_brake_per_wheel, 0.0) / 2.0
    Fx_rear_each  = np.clip(Fx_rear,  -2*c.max_brake_per_wheel, 0.0) / 2.0

    Fx_FL = Fx_FR = Fx_RL = Fx_RR = 0.0
    if brake_right:
        Fx_FR = Fx_front_each
        Fx_RR = Fx_rear_each
    else:
        Fx_FL = Fx_front_each
        Fx_RL = Fx_rear_each

    return Fx_FL, Fx_FR, Fx_RL, Fx_RR

# =========================
# 6) Plant dynamics (with friction circle + brake allocation)
# =========================
def plant_step(p: VehicleParams, vy: float, r: float, delta: float, Fx_wheels):
    """
    One-step continuous dynamics:
      - compute linear axle Fy
      - split axle Fy to left/right equally
      - apply friction circle per wheel with Fx_i and mu*Fz_i
      - sum wheel forces -> dynamics
    Return (dvy, dr), and also ay, Fx_total, Fy_total, Mz_from_brakes
    """
    m, Iz, vx, a, b = p.m, p.Iz, p.vx, p.a, p.b

    Fx_FL, Fx_FR, Fx_RL, Fx_RR = Fx_wheels
    Fx_total = Fx_FL + Fx_FR + Fx_RL + Fx_RR

    # approximate ax from total Fx
    ax = Fx_total / m

    # normal loads with longitudinal transfer
    Fz_FL, Fz_FR, Fz_RL, Fz_RR = normal_loads(p, ax)
    mu = p.mu

    # linear axle lateral forces
    Fy_f, Fy_r = lateral_forces_linear(p, vy, r, delta)

    # split to wheels
    Fy_FL = Fy_f / 2.0
    Fy_FR = Fy_f / 2.0
    Fy_RL = Fy_r / 2.0
    Fy_RR = Fy_r / 2.0

    # friction circle per wheel
    Fx_FL_s, Fy_FL_s = apply_friction_circle(Fx_FL, Fy_FL, mu*Fz_FL)
    Fx_FR_s, Fy_FR_s = apply_friction_circle(Fx_FR, Fy_FR, mu*Fz_FR)
    Fx_RL_s, Fy_RL_s = apply_friction_circle(Fx_RL, Fy_RL, mu*Fz_RL)
    Fx_RR_s, Fy_RR_s = apply_friction_circle(Fx_RR, Fy_RR, mu*Fz_RR)

    Fx_total_s = Fx_FL_s + Fx_FR_s + Fx_RL_s + Fx_RR_s
    Fy_total_s = Fy_FL_s + Fy_FR_s + Fy_RL_s + Fy_RR_s

    # yaw moment from lateral forces (bicycle approximation) + from longitudinal brake asymmetry
    # Mz_lat = a*Fy_f - b*Fy_r (use saturated sums per axle)
    Fy_f_s = Fy_FL_s + Fy_FR_s
    Fy_r_s = Fy_RL_s + Fy_RR_s
    Mz_lat = a*Fy_f_s - b*Fy_r_s

    # Mz_brake = sum(y_i * Fx_i)
    t = p.track
    yL, yR = +t/2.0, -t/2.0
    Mz_brake = yL*(Fx_FL_s + Fx_RL_s) + yR*(Fx_FR_s + Fx_RR_s)

    # dynamics
    dvy = (Fy_total_s / m) - vx*r
    dr  = (Mz_lat + Mz_brake) / Iz

    # ay = dvy + vx*r
    ay = dvy + vx*r

    return dvy, dr, ay, Fx_total_s, Fy_total_s, Mz_brake

# =========================
# 7) EKF (discrete-time) using linearized model for prediction
# =========================
class SimpleEKF:
    """
    State: x=[vy, r]
    Measure: y=[r, ay]
    Prediction uses discretized linear bicycle model:
      x_{k+1} = Ad x_k + Bd_delta delta_k + Bd_M * Mz_ideal_k  (for ideal case)
    In our "realistic" case, we still use same linear model as a filter model.
    """
    def __init__(self, p: VehicleParams, s: SensorParams, ekf: EKFParams):
        self.p = p
        self.s = s
        self.ekf = ekf

        A, B_delta, B_M, C, D = bicycle_mats(p)
        self.A = A
        self.Bd = B_delta
        self.Bm = B_M
        self.C = C
        self.D = D

        # discretize (ZOH) with dt
        dt = ekf.dt
        Ad, Bd, _, _, _ = cont2discrete((A, np.hstack([B_delta, B_M]), C, np.zeros((2,2))), dt)

        self.Ad = Ad
        self.Bd2 = Bd  # columns: [delta, Mz]

        # covariances
        self.Q = np.diag([ekf.q_vy, ekf.q_r]) * dt
        self.R = np.diag([s.sigma_r**2, s.sigma_ay**2])

        self.x = np.array([0.0, 0.0])
        self.P = np.diag([ekf.p0_vy, ekf.p0_r])

    def predict(self, delta, Mz_model):
        u = np.array([delta, Mz_model])
        self.x = (self.Ad @ self.x) + (self.Bd2 @ u)
        self.P = self.Ad @ self.P @ self.Ad.T + self.Q

    def update(self, y_meas, delta, Mz_model):
        """
        y = C x + D*delta + noise, but note ay depends also on dynamics.
        We'll keep measurement model:
          y_hat = C x + D*delta
        (This is approximate but works as a minimal EKF-like filter.)
        """
        y_hat = (self.C @ self.x) + (self.D.flatten() * delta)
        # innovation
        innov = y_meas - y_hat

        S = self.C @ self.P @ self.C.T + self.R
        K = self.P @ self.C.T @ np.linalg.inv(S)
        self.x = self.x + K @ innov
        self.P = (np.eye(2) - K @ self.C) @ self.P

# =========================
# 8) Simulation harness
# =========================
def run_sim():
    p = VehicleParams(vx=22.0, mu=0.35)
    c = ControlParams()
    s = SensorParams()
    ekf_cfg = EKFParams(dt=0.005)

    # LQR (ideal yaw-moment controller): u = Mz_des = -K xhat
    A, B_delta, B_M, C, D = bicycle_mats(p)
    Q = np.diag([c.q_vy, c.q_r])
    R = np.array([[c.r_u]])
    K = lqr_gain(A, B_M, Q, R)  # 1x2

    # EKF
    filt = SimpleEKF(p, s, ekf_cfg)

    # time grid
    T = 6.0
    dt = ekf_cfg.dt
    N = int(T/dt) + 1
    ts = np.linspace(0.0, T, N)

    # ---------
    # three modes:
    # (1) no ESP (Fx_wheels=0)
    # (2) ideal ESP: apply "Mz_des" directly as external yaw moment (not realistic)
    # (3) realistic ESP: allocate to brakes + friction circle
    # ---------
    def simulate_mode(mode: int):
        vy, r = 0.0, 0.0
        filt.x[:] = np.array([0.2, -0.05])   # initial mismatch to show convergence
        filt.P[:] = np.diag([ekf_cfg.p0_vy, ekf_cfg.p0_r])

        logs = {
            "t": [], "delta": [],
            "vy": [], "r": [], "beta": [],
            "vy_hat": [], "r_hat": [], "beta_hat": [],
            "ay_meas": [], "ay_true": [],
            "Mz_des": [], "Mz_brake": [],
            "Fx_total": [], "Fy_total": [],
            "Fx_FL": [], "Fx_FR": [], "Fx_RL": [], "Fx_RR": []
        }

        for t in ts:
            delta = steering_delta(t)

            # controller uses estimated state
            xhat = filt.x.copy()
            Mz_des = float(-(K @ xhat.reshape(-1,1)).item())
            Mz_des = float(np.clip(Mz_des, -c.Mz_max, c.Mz_max))

            if mode == 1:
                # no ESP
                Fx_wheels = (0.0, 0.0, 0.0, 0.0)
                dvy, dr, ay_true, Fx_total, Fy_total, Mz_br = plant_step(p, vy, r, delta, Fx_wheels)

            elif mode == 2:
                # ideal Mz directly injected: emulate by converting to equivalent asymmetric Fx on track with no saturation
                # We'll implement as: compute dr = (Mz_lat + Mz_des)/Iz, i.e., bypass brake allocation and friction limit.
                # Use linear axle Fy (no friction circle) for this mode.
                Fy_f, Fy_r = lateral_forces_linear(p, vy, r, delta)
                dvy = (Fy_f + Fy_r)/p.m - p.vx*r
                dr  = (p.a*Fy_f - p.b*Fy_r + Mz_des)/p.Iz
                ay_true = dvy + p.vx*r
                Fx_total, Fy_total, Mz_br = 0.0, (Fy_f+Fy_r), Mz_des
                Fx_wheels = (0.0, 0.0, 0.0, 0.0)

            else:
                # realistic: allocate to brakes + friction circle
                Fx_wheels = allocate_brakes_from_Mz(p, c, Mz_des)
                dvy, dr, ay_true, Fx_total, Fy_total, Mz_br = plant_step(p, vy, r, delta, Fx_wheels)

            # integrate (Euler; small dt)
            vy += dvy * dt
            r  += dr  * dt

            # measurement with noise: y=[r, ay]
            y_meas = np.array([
                r + np.random.randn()*s.sigma_r,
                ay_true + np.random.randn()*s.sigma_ay
            ])

            # filter predict/update (model uses Mz_des as "Mz_model")
            filt.predict(delta, Mz_des)
            filt.update(y_meas, delta, Mz_des)

            # logs
            logs["t"].append(t)
            logs["delta"].append(delta)
            logs["vy"].append(vy)
            logs["r"].append(r)
            logs["beta"].append(vy/p.vx)

            logs["vy_hat"].append(filt.x[0])
            logs["r_hat"].append(filt.x[1])
            logs["beta_hat"].append(filt.x[0]/p.vx)

            logs["ay_meas"].append(y_meas[1])
            logs["ay_true"].append(ay_true)

            logs["Mz_des"].append(Mz_des)
            logs["Mz_brake"].append(Mz_br)

            logs["Fx_total"].append(Fx_total)
            logs["Fy_total"].append(Fy_total)

            Fx_FL, Fx_FR, Fx_RL, Fx_RR = Fx_wheels
            logs["Fx_FL"].append(Fx_FL)
            logs["Fx_FR"].append(Fx_FR)
            logs["Fx_RL"].append(Fx_RL)
            logs["Fx_RR"].append(Fx_RR)

        for k in logs:
            logs[k] = np.array(logs[k])
        return logs

    np.random.seed(7)  # reproducible
    log_no   = simulate_mode(1)
    np.random.seed(7)
    log_ideal= simulate_mode(2)
    np.random.seed(7)
    log_real = simulate_mode(3)

    # =========================
    # 9) Plot
    # =========================
    t = log_real["t"]

    plt.figure()
    plt.plot(t, log_no["r"], label="r: no ESP")
    plt.plot(t, log_ideal["r"], label="r: ideal Mz")
    plt.plot(t, log_real["r"], label="r: brakes+mu circle")
    plt.plot(t, log_real["r_hat"], label="r_hat (EKF, realistic)")
    plt.xlabel("Time (s)")
    plt.ylabel("Yaw rate r (rad/s)")
    plt.grid(True)
    plt.legend()

    plt.figure()
    plt.plot(t, log_no["beta"], label="beta: no ESP")
    plt.plot(t, log_ideal["beta"], label="beta: ideal Mz")
    plt.plot(t, log_real["beta"], label="beta: brakes+mu circle")
    plt.plot(t, log_real["beta_hat"], label="beta_hat (EKF, realistic)")
    plt.xlabel("Time (s)")
    plt.ylabel("Side-slip beta (rad)")
    plt.grid(True)
    plt.legend()

    plt.figure()
    plt.plot(t, log_real["delta"], label="steering delta")
    plt.xlabel("Time (s)")
    plt.ylabel("delta (rad)")
    plt.grid(True)
    plt.legend()

    plt.figure()
    plt.plot(t, log_ideal["Mz_des"], label="Mz_des (controller)")
    plt.plot(t, log_real["Mz_brake"], label="Mz actually from brakes (after mu circle)")
    plt.xlabel("Time (s)")
    plt.ylabel("Yaw moment (N*m)")
    plt.grid(True)
    plt.legend()

    plt.figure()
    plt.plot(t, log_real["Fx_FR"], label="Fx_FR")
    plt.plot(t, log_real["Fx_RR"], label="Fx_RR")
    plt.plot(t, log_real["Fx_FL"], label="Fx_FL")
    plt.plot(t, log_real["Fx_RL"], label="Fx_RL")
    plt.xlabel("Time (s)")
    plt.ylabel("Wheel longitudinal force Fx (N, negative=brake)")
    plt.grid(True)
    plt.legend()

    plt.figure()
    plt.plot(t, log_real["ay_true"], label="ay true")
    plt.plot(t, log_real["ay_meas"], label="ay measured (noisy)")
    plt.xlabel("Time (s)")
    plt.ylabel("ay (m/s^2)")
    plt.grid(True)
    plt.legend()

    plt.show()

if __name__ == "__main__":
    run_sim()