diff --git a/FullVehicleSim/Electrical/powertrain.py b/FullVehicleSim/Electrical/powertrain.py
index daa3cf4..53c8171 100644
--- a/FullVehicleSim/Electrical/powertrain.py
+++ b/FullVehicleSim/Electrical/powertrain.py
@@ -1,7 +1,8 @@
 import numpy as np
+from numpy import ndarray
 from paramLoader import *
 
-def calcMaxMotorTorque(worldArray:np.ndarray, step:int, resistiveForces:float, maxPower:float, maxTractionTorqueAtWheel:float):
+def calcMaxMotorTorque(worldArray:ndarray[np.float64], step:int, resistiveForces:float, maxPower:float, maxTractionTorqueAtWheel:float):
         '''
         Motor Torque at the wheel
         
@@ -35,7 +36,7 @@ def calcMaxPower(voltage:float) -> float:
         return Parameters["tractiveIMax"] * voltage
 
 
-def calcVoltage(worldArray:np.ndarray, step:int) -> float:
+def calcVoltage(worldArray:ndarray[np.float64], step:int) -> float:
 
     F = Parameters["FaradaysConstant"]
     R = Parameters["GasConstant"]
diff --git a/FullVehicleSim/Mech/aero.py b/FullVehicleSim/Mech/aero.py
index 6b5ecbc..a73295e 100644
--- a/FullVehicleSim/Mech/aero.py
+++ b/FullVehicleSim/Mech/aero.py
@@ -1,8 +1,9 @@
 import numpy as np
+from numpy import ndarray
 from paramLoader import *
 
-def calcDrag(worldArray:np.ndarray, step:int) -> float:
+def calcDrag(worldArray:ndarray[np.float64], step:int) -> float:
     return  0.5 * Parameters["airDensity"] * Parameters["dragCoeffAreaCombo"] * worldArray[step-1, varSpeed]**2
 
-def calcDownForce(worldArray:np.ndarray, step:int) -> np.ndarray:
+def calcDownForce(worldArray:ndarray[np.float64], step:int) -> np.ndarray:
     return np.asarray([0,0,0,0], dtype=float)
diff --git a/FullVehicleSim/Mech/braking.py b/FullVehicleSim/Mech/braking.py
index 4a15070..1e4f472 100644
--- a/FullVehicleSim/Mech/braking.py
+++ b/FullVehicleSim/Mech/braking.py
@@ -1,6 +1,7 @@
 from Mech import brakepadFrictionModel
 from paramLoader import *
 import numpy as np
+from numpy import ndarray
 # Docs:
 # https://docs.google.com/document/d/1oGsGDnY0DEKWpE3S6481A9yZ0F9qUEwWkSXJwTSz4E4/edit?tab=t.2rmbsj26c7w
 # The goal of these functions are to calculate the net force on the brakes, applied reverse to heading
@@ -8,7 +9,7 @@
 def brakePSI_toNewtons(psi:float) -> float:
     return psi * Parameters["brakeCaliperArea"] * 4.448222 # lb force to Newtons
 
-def calcBrakeForce(worldArray:np.ndarray, step:int) -> tuple[float,float]:
+def calcBrakeForce(worldArray:ndarray[np.float64], step:int) -> tuple[float,float]:
     """
     Calculate the brake force.
 
@@ -28,7 +29,7 @@ def calcBrakeForce(worldArray:np.ndarray, step:int) -> tuple[float,float]:
     rearBrakeForce:float = brakepadFrictionModel.calcFrictionCoeff(worldArray[step-1, varRearBrakeTemperature]) * rearBrakeForce * 2 * Parameters["brakeDiscRadius"] / Parameters["wheelRadius"]
     return frontBrakeForce, rearBrakeForce
 
-def calcBrakeCooling(worldArray:np.ndarray, step:int) -> tuple[float,float]:
+def calcBrakeCooling(worldArray:ndarray[np.float64], step:int) -> tuple[float,float]:
     """
     Calculate the cooled brake temperature.
     
@@ -42,7 +43,7 @@ def calcBrakeCooling(worldArray:np.ndarray, step:int) -> tuple[float,float]:
     #change = (q * parameters["brakepadThickness"])/(initTemperature * parameters["brakeThermalConductivity"] * parameters["brakeSurfaceArea"]
     #return initTemperature - change
 
-def calcBrakeHeating(worldArray:np.ndarray, step:int) -> tuple[float,float]:
+def calcBrakeHeating(worldArray:ndarray[np.float64], step:int) -> tuple[float,float]:
     # Calculate Brake Force
     frontBrakeForce, rearBrakeForce = calcBrakeForce(worldArray, step)
     # Guess energy increase based on kinetic energy decrease of the vehicle.
diff --git a/FullVehicleSim/Mech/general.py b/FullVehicleSim/Mech/general.py
index 5bad33b..f55e79a 100644
--- a/FullVehicleSim/Mech/general.py
+++ b/FullVehicleSim/Mech/general.py
@@ -5,15 +5,16 @@
 from Mech.steering import calcSlipAngle, calcYawRate
 from Mech.tireLoad import calcLoadTransfer
 import numpy as np
+from numpy import ndarray
 
-def calcResistiveForces(worldArray:np.ndarray, step:int):
+def calcResistiveForces(worldArray:ndarray[np.float64], step:int):
         if worldArray[step-1, varSpeed] <= 1e-5: # Floating point error
             return 0
         else:
             frontBrakeForce, rearBrakeForce = calcBrakeForce(worldArray, step)
             return -1 * (calcDrag(worldArray, step) + frontBrakeForce + rearBrakeForce)
         
-def calculateYawRate(worldArray:np.ndarray, step:int, initAcceleration:float, initYawRate:float, timeSinceLastSteer:float):
+def calculateYawRate(worldArray:ndarray[np.float64], step:int, initAcceleration:float, initYawRate:float, timeSinceLastSteer:float):
         """Calculate the yaw rate of the vehicle at the current state.
         This function computes the yaw rate by calculating tire loads, slip angles,
         cornering stiffness, and then applying the vehicle dynamics equations.
diff --git a/FullVehicleSim/Mech/steering.py b/FullVehicleSim/Mech/steering.py
index 4c06198..185f053 100644
--- a/FullVehicleSim/Mech/steering.py
+++ b/FullVehicleSim/Mech/steering.py
@@ -1,8 +1,9 @@
 # Steering model
 import numpy as np
+from numpy import ndarray
 from paramLoader import *
 
-def calcSlipAngle(worldArray:np.ndarray, step:int) -> tuple[float,float]:
+def calcSlipAngle(worldArray:ndarray[np.float64], step:int) -> tuple[float,float]:
     """
     Calculate Slip Angle Based on yawRate, Velocity, and Steering Angle.
     
diff --git a/FullVehicleSim/SimulationControlInputs/simulationControls.csv b/FullVehicleSim/SimulationControlInputs/simulationControls.csv
index c19041a..3736981 100644
--- a/FullVehicleSim/SimulationControlInputs/simulationControls.csv
+++ b/FullVehicleSim/SimulationControlInputs/simulationControls.csv
@@ -1,6 +1,6 @@
 time,throttle,brakePressureFront,brakePressureRear,steerAngle
 0.0,0.0,0.0,0.0,0.0
-10.0,1.0,0.0,0.0,0.0
-20.0,0.0,0.0,0.0,0.0
-30.0,0.0,300.0,300.0,0.0
+10.0,1.0,0.0,0.0,0.1
+20.0,0.0,0.0,0.0,0.1
+30.0,0.0,300.0,300.0,0.1
 40.0,0.0,0.0,0.0,0.0
\ No newline at end of file
diff --git a/FullVehicleSim/SimulationControlInputs/testing.csv b/FullVehicleSim/SimulationControlInputs/testing.csv
new file mode 100644
index 0000000..ea0f262
--- /dev/null
+++ b/FullVehicleSim/SimulationControlInputs/testing.csv
@@ -0,0 +1,17 @@
+time,throttle,brakePressureFront,brakePressureRear,steerAngle
+0.0,0.0,0.0,0.0,0.0
+5.0,1.0,0.0,0.0,0.0
+10.0,1.0,0.0,0.0,0.0
+15.0,0.7,0.0,0.0,0.1
+30.0,0.7,0.0,0.0,0.1
+35.0,0.7,0.0,0.0,-0.1
+45.0,0.7,0.0,0.0,0.1
+50.0,0.7,0.0,0.0,0.0
+52.0,0.7,0.0,0.0,0.15
+55.0,0.7,0.0,0.0,0.15
+60.0,0.7,0.0,0.0,-0.1
+75.0,0.7,0.0,0.0,-0.1
+80.0,0.0,200.0,200.0,-0.1
+90.0,0.0,200.0,200.0,0.0
+100.0,0.0,0.0,0.0,0.0
+110.0,0.0,0.0,0.0,0.0
\ No newline at end of file
diff --git a/FullVehicleSim/engine.py b/FullVehicleSim/engine.py
index 40a915d..a564921 100644
--- a/FullVehicleSim/engine.py
+++ b/FullVehicleSim/engine.py
@@ -1,15 +1,17 @@
 from paramLoader import *
 import numpy as np
+from numpy import ndarray
 from Mech.braking import calcBrakeCooling, calcBrakeHeating, calcBrakeForce
 from Mech.aero import calcDrag, calcDownForce
 from Mech.steering import calcSlipAngle
 from Mech.general import calcResistiveForces
 from Electrical.powertrain import calcCurrent, calcMaxMotorTorque, calcMaxWheelTorque, calcMotorForce, calcMaxPower, calcVoltage
+from yaw_rate_model.double_bicycle_model import calcYawRate
 from scipy.integrate import RK45
 
 # Vibe coded but it looks about right so idk.
 # TODO: Verify that this is correct
-def calculateHeading(worldArray:np.ndarray, step:int) -> np.ndarray:
+def calculateHeading(worldArray:ndarray[np.float64], step:int) -> np.ndarray:
     time_increment = 1/Parameters["stepsPerSecond"]
     initial_heading = worldArray[step-1, varHeadingX:varHeadingZ] # Yes this removes Z, we just want X and Y for this simplification
     rotation_angle = worldArray[step-1, varYawRate] * time_increment
@@ -26,7 +28,7 @@ def calculateHeading(worldArray:np.ndarray, step:int) -> np.ndarray:
 
     return np.append(new_heading, 0)
 
-def stepState(worldArray:np.ndarray, step:int) -> np.ndarray:
+def stepState(worldArray:ndarray[np.float64], step:int) -> np.ndarray:
     """
     The order by which things get updated in this function is incredibly important. 
     If you calculate velocity before you calculate acceleration, 
@@ -75,9 +77,7 @@ def stepState(worldArray:np.ndarray, step:int) -> np.ndarray:
     arr[varCharge] = worldArray[step-1, varCharge] - worldArray[step, varCurrent] * delta / 3600.0
     arr[varPosX:varPosZ+1] = worldArray[step-1, varPosX:varPosZ+1] + worldArray[step-1, varVelX:varVelZ+1] * delta
     arr[varSpeed] = max(0, worldArray[step-1, varSpeed] + arr[varAcceleration] * delta) # Sometimes braking falls a tad below 0 so we just correct that because otherwise everything breaks
-    arr[varYawRate] = worldArray[step-1, varYawRate]
-    if worldArray[step, varSteerAngle] == 0:
-        arr[varYawRate] = 0
+    arr[varLateralVelocty], arr[varYawRate] = calcYawRate(worldArray, step)
     arr[varVelX:varVelZ+1] = arr[varSpeed] * worldArray[step-1, varHeadingX:varHeadingZ+1]
     arr[varHeadingX:varHeadingZ+1] = calculateHeading(worldArray, step)
 
diff --git a/FullVehicleSim/main.py b/FullVehicleSim/main.py
index 118422b..a388377 100644
--- a/FullVehicleSim/main.py
+++ b/FullVehicleSim/main.py
@@ -82,6 +82,8 @@
     worldArray[0, varHeadingX:varHeadingZ+1] = Parameters["initHeading"]
     worldArray[0, varPosX:varPosZ+1] = Parameters["initPosition"]
     worldArray[0, varVelX:varVelZ+1] = Parameters["initVelocity"]
+    worldArray[0, varLateralVelocty] = Parameters["InitLateralVelocity"] # velocity in y direction (needed for yaw rate)
+    worldArray[0, varYawRate] = Parameters["InitYawRate"]
     worldArray[:, varTime] = np.arange(0, Parameters["simulationDuration"] + 1/Parameters["stepsPerSecond"], 1/Parameters["stepsPerSecond"])
 
     start = time.time()
@@ -122,35 +124,37 @@
     torque = df['motorTorque']
     yawRate = df['yawRate']
     frontBrakeTemperature = df['frontBrakeTemperature']
-    ax1 = plt.subplot(411)
-    ax11 = ax1.twinx()
-    ax2 = plt.subplot(412)
-    ax22 = ax2.twinx()
-    ax3 = plt.subplot(413)
-    ax33 = ax3.twinx()
-    ax4 = plt.subplot(414)
+    # ax1 = plt.subplot(411)
+    # ax11 = ax1.twinx()
+    # ax2 = plt.subplot(412)
+    # ax22 = ax2.twinx()
+    # ax3 = plt.subplot(413)
+    # ax33 = ax3.twinx()
+    ax4 = plt.subplot(111)
     ax44 = ax4.twinx()
 
-    ax1.set_title("I (Blue)/V (Orange) vs Time")
-    ax1.set_xlabel("Time (s)")
-    ax1.set_ylabel("Current (A) / Voltage (V)")
-    ax1.plot(t, current, label="Current")
-    ax11.plot(t, voltage, label="Voltage", color='orange')
-    ax1.legend()
-
-    ax2.set_title("Speed vs Time")
-    ax2.set_xlabel("Time (s)")
-    ax2.set_ylabel("Speed (m/s)")
-    ax2.plot(t, speed)
-
-    ax3.set_title("Throttle (Blue)/Brakes (Orange) vs Time")
-    ax3.set_xlabel("Time (s)")
-    ax3.set_ylabel("Throttle (0-1)")
-    ax33.set_ylabel("Brake Pressure (PSI)")
-    ax3.plot(t, df["throttle"], label="Throttle")
-    ax33.plot(t, df["brakePressureFront"], color='orange')
-
-    ax4.set_title("rvt")
+    # ax1.set_title("I (Blue)/V (Orange) vs Time")
+    # ax1.set_xlabel("Time (s)")
+    # ax1.set_ylabel("Current (A) / Voltage (V)")
+    # ax1.plot(t, current, label="Current")
+    # ax11.plot(t, voltage, label="Voltage", color='orange')
+    # ax1.legend()
+
+    # ax2.set_title("Speed vs Time")
+    # ax2.set_xlabel("Time (s)")
+    # ax2.set_ylabel("Speed (m/s)")
+    # ax2.plot(t, speed)
+
+    # ax3.set_title("Throttle (Blue)/Brakes (Orange) vs Time")
+    # ax3.set_xlabel("Time (s)")
+    # ax3.set_ylabel("Throttle (0-1)")
+    # ax33.set_ylabel("Brake Pressure (PSI)")
+    # ax3.plot(t, df["throttle"], label="Throttle")
+    # ax33.plot(t, df["brakePressureFront"], color='orange')
+
+    ax4.set_xlabel("Time (s)")
+    ax4.set_ylabel("Yaw Rate (radians)")
+    ax4.set_title("yawRate")
     ax4.plot(t, yawRate)
 
     #ax4.set_ylim([0, 190])
diff --git a/FullVehicleSim/paramLoader.py b/FullVehicleSim/paramLoader.py
index 56909af..d9acdeb 100644
--- a/FullVehicleSim/paramLoader.py
+++ b/FullVehicleSim/paramLoader.py
@@ -67,6 +67,7 @@
 varMaxMotorTorque = 41
 varAcceleration = 42
 varWheelRPM = 43
+varLateralVelocty = 44
 
 # Automatically generate schema from defined variables
 def generate_variable_schema() -> Dict[int, str]:
diff --git a/FullVehicleSim/params.json5 b/FullVehicleSim/params.json5
index e01b1a5..d298c52 100644
--- a/FullVehicleSim/params.json5
+++ b/FullVehicleSim/params.json5
@@ -57,6 +57,16 @@
 
         "GasConstant": 8.31446261815324,
         "FaradaysConstant": 96485.33212,
+
+        "InitLateralVelocity": 0,
+        "InitYawRate": 0,
+        "tw": 1083.3862, // mm (simplified track width from steering axis to steering axis [steering axis is also simplified to be A-arm knuckle to A-arm knuckle])
+        "rackRatio": 0.332862903226, // 82.55/248 [4] mm rack displacement/deg pinion rotation
+        "rackShift": 0.0, //  mm of movement of the rack from left to right (left is - right is +)
+        "l_rack": 292.1, // [4] mm (width of steering rack casing)
+        "l_rod": 378.9426, // [3] mm (length of tie rod as left in FS-3 master CAD)
+        "d": 109.7788, // [3] mm (plan view distance between front axis and rack. negative because we have a front steer setup)
+        "l_arm": 71.628 // [3] mm (length of "steer arm", which is the distance from the center of the upright toe rod pickup to the KPA)
     },
     "Magic": {
 
@@ -171,5 +181,129 @@
         "pressureYB": -9.812999633140862e-05,
         "pressureYC": 0.9998338222503662,
         "gysign": -0.10000000149011612,
-    }
+
+        "q_v2": 1.9079276329655327e-36,
+        "loadA": -0.08525806665420532,
+        "loadB": 0.8007363080978394,
+        "loadC": 3.823115110397339,
+        "q_Fcx": 0.40706977248191833,
+        "q_Fcy": 0.40706977248191833,
+        "Omega": 1.9079276329655327e-36,
+        "q_Fz1": 0.10348843783140182,
+        "q_Fz2": 0.10000000149011612,
+        "P_pFz1": -0.38212499022483826
+    },
+    "Magic-Parameters": {
+        "q_v2": 1.9079276329655327e-36,
+        "Omega": 1.9079276329655327e-36,
+        "q_Fcx": 0.40706977248191833,
+        "q_Fcy": 0.40706977248191833,
+        "q_Fz1": 0.10348843783140182,
+        "q_Fz2": 0.10000000149011612,
+        "P_pFz1": -0.38212499022483826,
+        "shape-factor": 0.696268618106842,
+        "curvature-scaling-factor": 1.7177082300186157,
+        "horizontal-shift-factor": 1.0,
+        "vertical-shift-factor": 0.9601083397865295,
+        "zeta_1": 0.6045444011688232,
+        "p_px1": 0.013196180574595928,
+        "p_px2": 0.8705743551254272,
+        "p_px3": -0.9542407989501953,
+        "p_px4": -0.9245550036430359,
+        "p_dx1": 0.642947793006897,
+        "p_dx2": 0.9363532066345215,
+        "p_dx3": 0.0,
+        "p_ex1": 0.5635436177253723,
+        "p_ex2": -0.5660403966903687,
+        "p_ex3": 0.8500840663909912,
+        "p_ex4": -0.005640119314193726,
+        "p_kx1": 21.55539894104004,
+        "p_kx2": 13.510042190551758,
+        "p_kx3": -0.5750095248222351,
+        "p_hx1": 0.0021615000441670418,
+        "p_hx2": 0.0011597999837249517,
+        "p_vx1": 0.41343268752098083,
+        "p_vx2": -0.28164142370224,
+        "lambda_loadscalarlong": 1.1716148853302002,
+        "lambda_pressurescalarlong": 0.4518716633319855,
+        "tempXAPure": -0.01220773346722126,
+        "tempXBPure": 0.9834595322608948,
+        "tempXCPure": 3.0494847297668457,
+        "r_bx1": 13.225804328918457,
+        "r_bx2": 9.537531852722168,
+        "r_bx3": 0.0,
+        "r_cx1": 1.148189902305603,
+        "r_ex1": -0.23479725420475006,
+        "r_ex2": -0.27553272247314453,
+        "r_hx1": -0.11146939545869827,
+        "lambda_alphastar": 1.14445960521698,
+        "lambda_xalpha": 1.1735954284667969,
+        "lambda_combinedslipcoeff": 0.8948060870170593,
+        "combined_long_offset": 0.8635340929031372,
+        "tempXA": -0.001096199150197208,
+        "tempXB": -0.004315061029046774,
+        "tempXC": 0.9925193786621094,
+        "By_pure": 0.17780134081840515,
+        "p_cy1": 1.2041044235229492,
+        "p_dy1": -0.31575706601142883,
+        "p_dy2": -1.0540627241134644,
+        "p_dy3": 0.0,
+        "p_ey1": -1.6930402517318726,
+        "p_ey2": -2.0134072303771973,
+        "p_ey3": 0.21373338997364044,
+        "p_ey4": -6.697000026702881,
+        "p_ey5": 0.0,
+        "p_ky1": -15.324000358581543,
+        "p_ky2": 1.715000033378601,
+        "p_ky3": 0.3695000112056732,
+        "p_ky4": -6.697000026702881,
+        "p_ky5": 0.0,
+        "p_py1": -0.6255000233650208,
+        "p_py2": -0.06522999703884125,
+        "p_py3": 1.8217451724922284e-06,
+        "p_py4": -0.26475754380226135,
+        "Svy": 31.63314437866211,
+        "zeta3": 1.0,
+        "epsilon_y": 0.009999999776482582,
+        "lambda_alphastarypure": 0.49190351366996765,
+        "lambda_ey": -1.1078534126281738,
+        "lambda_kyalpha": 1.0,
+        "lambda_nominalload": 1.0,
+        "lambda_coeffscalary": 0.8558508157730103,
+        "lambda_loadscalarlat": 0.7529077529907227,
+        "lambda_pressurescalarlat": 1.0000001192092896,
+        "tempYAPure": 0.035267509520053864,
+        "tempYBPure": -4.318601608276367,
+        "tempYCPure": 0.130745992064476,
+        "Byk": 0.7075067758560181,
+        "Cyk": 1.7348066568374634,
+        "Eyk": 0.9768351912498474,
+        "Shyk": 0.6505736708641052,
+        "Svyk": -6.833913803100586,
+        "r_by1": 9.446654319763184,
+        "r_by2": 7.820000171661377,
+        "r_by3": 0.00203699991106987,
+        "r_by4": 0.0,
+        "r_cy1": 0.8584131598472595,
+        "r_ey1": -1.0929341316223145,
+        "r_ey2": -1.7355691194534302,
+        "r_hy1": 0.11233723163604736,
+        "r_hy2": -0.23296165466308594,
+        "r_vy1": 0.534748911857605,
+        "r_vy2": 0.48336413502693176,
+        "r_vy3": 0.0,
+        "r_vy4": 93.80660247802734,
+        "r_vy5": 1.9759562015533447,
+        "r_vy6": 23.584060668945312,
+        "lambda_yk": 1.0,
+        "lambda_vyk": 1.7570732831954956,
+        "zeta_2": 1.7570732831954956,
+        "pressureYA": -3.086921788053587e-05,
+        "pressureYB": -9.812999633140862e-05,
+        "pressureYC": 0.9998338222503662,
+        "gysign": -0.10000000149011612,
+        "loadA": -0.08525806665420532,
+        "loadB": 0.8007363080978394,
+        "loadC": 3.823115110397339
+  }
 }
diff --git a/FullVehicleSim/simulation_output.parquet b/FullVehicleSim/simulation_output.parquet
index 3c5c963..1606cf7 100644
Binary files a/FullVehicleSim/simulation_output.parquet and b/FullVehicleSim/simulation_output.parquet differ
diff --git a/FullVehicleSim/yaw_rate_model/ackermann.py b/FullVehicleSim/yaw_rate_model/ackermann.py
new file mode 100644
index 0000000..6bc8d94
--- /dev/null
+++ b/FullVehicleSim/yaw_rate_model/ackermann.py
@@ -0,0 +1,69 @@
+
+#steering wheel angle --> steering rack psition --> wheel steer angle (how static ackermann affects wheel angle function)
+# equations taken from "rack and pinion" section of: https://www.mathworks.com/help/vdynblks/ref/kinematicsteering.html
+import scipy
+import numpy
+import matplotlib.pyplot as plt
+import ipywidgets as widgets
+from ipywidgets import interact, interactive
+
+#global variables
+#-----------------------------
+# THIS SCRIPT USES MOSTLY FS-3 VALUES. FS-3 values denoted by [3], any theoretical or FS-4 values denoted by [4]
+#-----------------------------
+tw = 1083.3862 #mm (simplified track width from steering axis to steering axis [steering axis is also simplified to be A-arm knuckle to A-arm knuckle])
+rackRatio = 82.55/248 #[4] mm rack displacement/deg pinion rotation
+# wheelInput = 0.0 #in degrees of steering wheel movement (CW + CCW -)
+rackShift = 0.0 # mm of movement of the rack from left to right (left is - right is +)
+l_rack = 292.1 #[4] mm (width of steering rack casing)
+l_rod = 378.9426 #[3] mm (length of tie rod as left in FS-3 master CAD)
+d = 109.7788 #[3] mm (plan view distance between front axis and rack. negative because we have a front steer setup)
+l_arm = 71.628 #[3] mm (length of "steer arm", which is the distance from the center of the upright toe rod pickup to the KPA)
+
+def rackMovement(wheelInput): #returns the amount of L-R displacement (in mm) of the steering rack, with the right direction as "positive"
+
+    rackRatio = 82.55/248 # mm rack displacement/deg pinion rotation
+    rackShift: float = rackRatio*wheelInput # wheelInput
+    return rackShift
+
+def calculateAckermann(wheelInput: float): #calculates the steer angles of both wheels
+
+    l1Left = (0.5*(tw-l_rack)) - rackMovement(wheelInput) #l1 is the instantaneous parallel distance from the rack knuckle to steering axis (KPA). 
+    l1Right = (0.5*(tw-l_rack)) + rackMovement(wheelInput)
+    l_nought = (0.5*(tw-l_rack))
+    beta_nought = betaTrigSolver(l_nought) #used to find the initial "beta" geometry to determine the real steer angle at the wheels
+
+    beta_L = betaTrigSolver(l1Left) - beta_nought #additionally, because there is a static "beta" (simply just arm geometry), we must find the difference to find the actual wheel angles
+    beta_R = betaTrigSolver(l1Right) - beta_nought
+    
+    return beta_L, beta_R
+    #return beta_nought, betaTrigSolver(l1Left), betaTrigSolver(l1Right)
+    
+def betaTrigSolver(l1): #a separate function to solve the big bad trig equation
+    l2 = numpy.sqrt((l1**2) + (d**2)) #l2 is the instantaneous direct distance from rack knuckle to steering axis (KPA)
+    atan = numpy.arctan(d/l1) #first term of the "beta" equation
+
+    num = (l_arm**2) + (l2**2) - (l_rod**2) #just simplifying the calculation of the second term 
+    denom = 2*l_arm*l2
+    frac = num/denom
+    acos = numpy.arccos(frac)
+    beta = (numpy.pi/2) - atan - acos
+    return beta
+    #return frac
+
+def update(val): #update variables based off of interact()
+    global wheelInput
+    wheelInput = val
+    left_angle, right_angle = calculateAckermann()
+    #stat, bL, bR = calculateAckermann()
+    print(f"wheelInput = {wheelInput}")
+    print(f"rack movement = {rackMovement()}")
+    print("----------------------------")
+    print(f"left wheel radians = {left_angle}")
+    print(f"right wheel radians = {right_angle}")
+    print("----------------------------")
+    print(f"left wheel degrees = {numpy.rad2deg(left_angle)}")
+    print(f"right wheel degrees = {numpy.rad2deg(right_angle)}")
+    #print(f"Static value must be within [-1,1] = {stat}")
+    #print(f"Left value must be within [-1,1] = {bL}")
+    #print(f"Right value must be within [-1,1] = {bR}")
diff --git a/FullVehicleSim/yaw_rate_model/double_bicycle_model.py b/FullVehicleSim/yaw_rate_model/double_bicycle_model.py
index b0a78f2..8f49ad7 100644
--- a/FullVehicleSim/yaw_rate_model/double_bicycle_model.py
+++ b/FullVehicleSim/yaw_rate_model/double_bicycle_model.py
@@ -6,10 +6,14 @@
 """
 
 import numpy as np
+from numpy import ndarray
+import pandas as pd
 from dataclasses import dataclass
 from typing import Tuple, List
 import matplotlib.pyplot as plt
-
+import sys
+from paramLoader import *
+import math
 
 @dataclass
 class VehicleParameters:
@@ -20,7 +24,7 @@ class VehicleParameters:
     Lf: float = 0.8535  # Calculated from 46.32% front weight distribution
     Lr: float = 0.7365  # Calculated from weight distribution
     track_width_front: float = 1.234008  # 1234.008 mm
-    track_width_rear: float = 1.186  # 1186 mm
+    track_width_rear: float = 1.186  # 1186 m
     track_width: float = 1.234008  # Using front track for compatibility
 
     # Mass properties (with driver)
@@ -51,35 +55,142 @@ def __post_init__(self):
         assert self.yaw_inertia > 0, "Yaw inertia has to be positive"
         assert self.C_alpha > 0, "Tire stiffness has to be positive"
 
-
-@dataclass
 class TireModel:
-    stiffness: float
-    max_slip_angle: float = 0.3
-    saturation_method: str = "linear"
-    friction_coeff: float = 1.733  # Peak friction coefficient
 
-    def lateral_force(self, slip_angle: float, vertical_load: float) -> float:
-        if self.saturation_method == "linear":
-            return self.stiffness * slip_angle
+    def __init__(self, params: VehicleParameters, temperature, mechanicalParams, magicParams, axle, slipAngle, pressure=12, camber=0):
+        
+        self.magic = magicParams
+        self.mechanical = mechanicalParams
+        self.params = params
+
+        if axle == "front":
+            self.normalForce = self.params.mass * 9.81 * self.params.Lr / self.params.wheelbase / 2.0
+        elif axle == "rear":
+            self.normalForce = self.params.mass * 9.81 * self.params.Lf / self.params.wheelbase / 2.0
+
+        self.slipAngle = slipAngle
+        self.tirePressure = pressure
+        self.tireTemperature = temperature
+        self.actPressure = pressure # Actual PSI
+        self.camber = camber # Radians
+
+
+
+
+        #if(lat):
+        self.normDeltaLoadLat = self.normalizeLoadLat()
+        self.normDeltaPressureLat = self.normalizePressureLat()
+        #if(long):
+        self.normDeltaLoadLong = self.normalizeLoadLong()
+        self.normDeltaPressureLong = self.normalizePressureLong()
+
+        self.normalForce = self.getNormalLoad(self.normalForce)
+
+    def getNormalLoad(self, inputNormalForce):
+        # Neglecting last force
+        # I intentionally neglect the last Fx and Fy because that would involve a large rewrite of this.
+        sqrt_term = math.sqrt(9.81 * self.mechanical["unloaded-radius"])
+        term1 = (1 + self.magic["q_v2"] * abs(self.magic["Omega"]) * self.mechanical["unloaded-radius"]/sqrt_term - self.magic["q_Fcx"] - self.magic["q_Fcy"])
+        # We assume the deflection is 1 because idk how to do that
+        term2 = (self.magic["q_Fz1"] + self.magic["q_Fz2"] * self.camber**2) / self.mechanical["unloaded-radius"]
+        term3 = (1 + self.magic["P_pFz1"] * self.normDeltaPressureLong) * inputNormalForce
+
+        return term1 * term2 * term3
+
+    ##### ********************************
+    ##### LATERAL SLIP FUNCTION
+    ##### ********************************
+
+    def getLateralForce(self, worldArray:ndarray[np.float64], step:int):
+
+        self.velocityX = worldArray[step, varVelX]
+
+        Alphas = self.magic["lambda_alphastar"] * self.slipAngle * math.copysign(1, self.velocityX)
+        Byk = self.magic["r_by1"]# + self.magic["r_by4"] * math.sin(self.camber) ** 2) * math.cos(math.atan(self.magic["r_by2"] * (Alphas - self.magic["r_by3"]))) * self.magic["lambda_yk"]
+        Cyk = self.magic["r_cy1"]
+        Eyk = self.magic["r_ey1"] + self.magic["r_ey2"] * self.normDeltaLoadLat
+        Shyk = self.magic["r_hy1"] + self.magic["r_hy2"] * self.normDeltaLoadLat
+        
+        # Use Slip Ratio = (Wheel RPM - GPS Speed) / GPS Speed
+        rpm = worldArray[step, varWheelRPM]
+        angular_speed = (2 * math.pi * Parameters['wheelRadius'] * rpm) / 60
+        longitudinal_speed = worldArray[step, varSpeed]
+
+        if longitudinal_speed == 0: self.slipRatio = 0
+        else: self.slipRatio = (angular_speed - longitudinal_speed) / longitudinal_speed
+
+        Ks = self.slipRatio + Shyk
+        BykKs = Byk * Ks
+        BykShyk = Byk * Shyk
+        Gykappa = math.cos(Cyk * math.atan(BykKs - Eyk * (BykKs - math.atan(BykKs))))
+        Gykappazero =  math.cos(Cyk * math.atan(BykShyk - Eyk * (BykShyk - math.atan(BykShyk))))
+
+
+        Dvyk = self.mechanical["friction-coeff-lat"] * self.normalForce * (self.magic["r_vy1"] + self.magic["r_vy2"] * self.normDeltaLoadLat + self.magic["r_vy3"] * math.sin(self.camber)) * math.cos(math.atan(self.magic["r_vy4"] * math.sin(Alphas)))  * self.magic["zeta_2"]
+        Svyk = Dvyk * math.sin(self.magic["r_vy5"] * math.atan(self.magic["r_vy6"] * self.slipRatio)) * self.magic["lambda_vyk"]
+
+        #print(Byk, Cyk, Eyk, Shyk)
+
+        return Gykappa/Gykappazero * self.getLateralForcePure() #+ Svyk # + self.magic["Svyk"]
+
+    def getLateralForcePure(self):
+        Alphas = self.magic["lambda_alphastarypure"] * self.slipAngle * math.copysign(1,self.velocityX)
+
+        loadDependentPeak = self.magic["loadA"] * self.normalForce * self.normalForce + self.magic["loadB"] * self.normalForce + self.magic["loadC"]
+
+        self.Cy = self.magic["p_cy1"]
+        self.Dy = loadDependentPeak * self.getLateralCoefficientOfFriction() * self.normalForce * (self.magic["tempYAPure"] * self.tireTemperature ** 2 + self.magic["tempYBPure"] * self.tireTemperature + self.magic["tempYCPure"])
+        self.By = self.magic["By_pure"]
+        self.Ey = self.getLateralE(Alphas)
+
+        Svy = self.magic["Svy"]
+        return self.stdCurveSine(self.By, self.Cy, self.Dy, self.Ey, self.slipRatio) + Svy
+    
+    def getLateralCoefficientOfFriction(self):
+        return (self.magic["p_dy1"] + self.magic["p_dy2"] * self.normDeltaLoadLat) * (1 + self.magic["p_py3"] * self.normDeltaPressureLat + self.magic["p_py4"] * self.normDeltaPressureLat ** 2) * (1 - self.magic["p_dy3"] * math.sin(self.camber) ** 2) * self.magic["lambda_coeffscalary"]
+    
+    def getLateralE(self, Alphas):
+        term1 = (self.magic["p_ey1"] + self.magic["p_ey2"] * self.normDeltaLoadLat)
+        term2 = (1 + self.magic["p_ey5"] * math.sin(self.camber) ** 2 - (self.magic["p_ey3"] + self.magic["p_ey4"] * math.sin(self.camber)) * Alphas)
+        return term1 * term2 * self.magic["lambda_ey"]
+
+
+    ##### ********************************
+    ##### Standard Functioms
+    ##### ********************************
+
+    def stdCurveSine(self, Bx, Cx, Dx, Ex, slip):
+        BxSlip = Bx * slip
+        return Dx * math.sin( Cx * math.atan( BxSlip - Ex * (BxSlip - math.atan(BxSlip) ) ) )
+
+    def normalizeLoadLong(self):
+        return (self.normalForce - self.magic["lambda_loadscalarlong"] * self.normalForce) / (self.magic["lambda_loadscalarlong"] * self.normalForce)
+
+    def normalizeLoadLat(self):
+        return (self.normalForce - self.magic["lambda_loadscalarlat"] * self.normalForce) / (self.magic["lambda_loadscalarlat"] * self.normalForce)
 
-        elif self.saturation_method == "nonlinear":
-            # tanh saturation at high slip angles
-            # Saturates at F_max = mu * Fz (friction limit)
-            F_linear = self.stiffness * slip_angle
-            F_max = self.friction_coeff * vertical_load
-            return F_max * np.tanh(F_linear / F_max)
+    def normalizePressureLong(self):
+        # Only long because lat doesn't use it
+        return (self.tirePressure - self.magic["lambda_pressurescalarlong"] * self.tirePressure) / (self.magic["lambda_pressurescalarlong"] * self.tirePressure)
 
-        return 0.0
+    def normalizePressureLat(self):
+        # Only long because lat doesn't use it
+        return (self.tirePressure - self.magic["lambda_pressurescalarlat"] * self.tirePressure) / (self.magic["lambda_pressurescalarlat"] * self.tirePressure)
 
+    def updateParams(self, normalForce=-1, slipRatio=-1, velocityX=-1):
+        if normalForce != -1:
+            self.normalForce = normalForce
+        if slipRatio != -1:
+            self.slipRatio = slipRatio
+        if velocityX != -1:
+            self.velocityX = velocityX
 
 class DoubleBicycleModel:
     """2DOF bicycle model: v_y (lateral velocity) and r (yaw rate)"""
 
-    def __init__(self, params: VehicleParameters, tire_model: str = "linear"):
+    def __init__(self, params: VehicleParameters):
+        
         self.params = params
-        self.tire_front = TireModel(params.C_alpha_front, saturation_method=tire_model, friction_coeff=params.mu)
-        self.tire_rear = TireModel(params.C_alpha_rear, saturation_method=tire_model, friction_coeff=params.mu)
 
         self.state = np.array([0.0, 0.0])
         self.time_history = []
@@ -97,58 +208,119 @@ def get_slip_angles(self, v_y: float, r: float, v_x: float, delta: float) \
 
         return alpha_f, alpha_r
     
+    def rackMovement(self, wheelInput: float): #returns the amount of L-R displacement (in mm) of the steering rack, with the right direction as "positive"
+
+        rackShift: float = Parameters['rackRatio']*wheelInput # wheelInput
+        return rackShift
+
+    def calculateAckermann(self, wheelInput: float): #calculates the steer angles of both wheels
+
+        l1Left = (0.5*(Parameters["tw"]-Parameters["l_rack"])) - self.rackMovement(wheelInput) #l1 is the instantaneous parallel distance from the rack knuckle to steering axis (KPA). 
+        l1Right = (0.5*(Parameters["tw"]-Parameters["l_rack"])) + self.rackMovement(wheelInput)
+        l_nought = (0.5*(Parameters["tw"]-Parameters["l_rack"]))
+        beta_nought = self.betaTrigSolver(l_nought) #used to find the initial "beta" geometry to determine the real steer angle at the wheels
+
+        beta_L = self.betaTrigSolver(l1Left) - beta_nought #additionally, because there is a static "beta" (simply just arm geometry), we must find the difference to find the actual wheel angles
+        beta_R = self.betaTrigSolver(l1Right) - beta_nought
+        
+        return beta_L, beta_R
+        #return beta_nought, betaTrigSolver(l1Left), betaTrigSolver(l1Right)
+        
+    def betaTrigSolver(self, l1): #a separate function to solve the big bad trig equation
+        l2 = np.sqrt((l1**2) + (Parameters["d"]**2)) #l2 is the instantaneous direct distance from rack knuckle to steering axis (KPA)
+        atan = np.arctan(Parameters["d"]/l1) #first term of the "beta" equation
+
+        num = (Parameters["l_arm"]**2) + (l2**2) - (Parameters["l_rod"]**2) #just simplifying the calculation of the second term 
+        denom = 2*Parameters["l_arm"]*l2
+        frac = num/denom
+        acos = np.arccos(frac)
+        beta = (np.pi/2) - atan - acos
+        return beta
+        #return frac
+
     def dynamics(self, state: np.ndarray, v_x: float, delta: float,
-                 ax: float = 0.0) -> np.ndarray:
+                 worldArray: np.ndarray, step:int, ax: float = 0.0) -> np.ndarray:
         """Compute state derivatives [dv_y/dt, dr/dt]"""
         v_y, r = state
 
-        alpha_f, alpha_r = self.get_slip_angles(v_y, r, v_x, delta)
-
-        # Static weight distribution (no load transfer)
-        # Front axle load: weight farther back (at distance Lr from front)
-        # Rear axle load: weight farther forward (at distance Lf from rear)
-        Fz_f = self.params.mass * 9.81 * self.params.Lr / self.params.wheelbase / 2.0
-        Fz_r = self.params.mass * 9.81 * self.params.Lf / self.params.wheelbase / 2.0
-
-        Fy_f = self.tire_front.lateral_force(alpha_f, Fz_f)
-        Fy_r = self.tire_rear.lateral_force(alpha_r, Fz_r)
+        alpha_f, alpha_r = self.get_slip_angles(v_y, r, v_x, delta) # old way of getting slip angles
+
+        delta_fl, delta_fr = self.calculateAckermann(wheelInput=delta)
+
+        self.tire_fl = TireModel(
+            temperature=Parameters['ambientTemperature'], 
+            mechanicalParams=Parameters, 
+            magicParams=Magic, 
+            slipAngle=delta_fl,
+            axle="front",
+            params=params
+        )
+
+        self.tire_fr = TireModel( 
+            temperature=Parameters['ambientTemperature'], 
+            mechanicalParams=Parameters, 
+            magicParams=Magic, 
+            slipAngle=delta_fr,
+            axle="front",
+            params=params
+        )
+
+        self.tire_rear = TireModel( 
+            temperature=Parameters['ambientTemperature'], 
+            mechanicalParams=Parameters, 
+            magicParams=Magic, 
+            slipAngle=alpha_r,
+            axle="rear",
+            params=params
+        )
+
+        Fy_fl = -self.tire_fl.getLateralForce(worldArray, step)
+        Fy_fr = -self.tire_fr.getLateralForce(worldArray, step)
+        Fy_r = -self.tire_rear.getLateralForce(worldArray, step)
 
         # Account for both tires per axle
-        Fy_f_total = 2.0 * Fy_f
+        Fy_f_total = Fy_fl + Fy_fr
         Fy_r_total = 2.0 * Fy_r
 
         # Lateral and yaw accelerations
         a_y = (Fy_f_total * np.cos(delta) + Fy_r_total) / self.params.mass + v_x * r
         dv_y = a_y
 
-        M_yaw = self.params.Lf * Fy_f_total * np.cos(delta) - self.params.Lr * Fy_r_total
+        M_yaw = self.params.Lf * Fy_f_total - self.params.Lr * Fy_r_total
         dr = M_yaw / self.params.yaw_inertia
 
         return np.array([dv_y, dr])
     
-    def integrate_step(self, v_x: float, delta: float, dt: float,
+    def integrate_step(self, v_x: float, delta: float, dt: float, worldArray: np.ndarray, step:int,
                       ax: float = 0.0, method: str = "rk4") -> None:
         """Integrate one timestep using euler, rk2, or rk4"""
         if method == "euler":
-            k1 = self.dynamics(self.state, v_x, delta, ax)
+            k1 = self.dynamics(state=self.state, v_x=v_x, delta=delta, ax=ax, worldArray=worldArray, step=step)
             self.state = self.state + dt * k1
 
         elif method == "rk2":
-            k1 = self.dynamics(self.state, v_x, delta, ax)
-            k2 = self.dynamics(self.state + 0.5*dt*k1, v_x, delta, ax)
+            k1 = self.dynamics(state=self.state, v_x=v_x, delta=delta, ax=ax, worldArray=worldArray, step=step)
+            k2 = self.dynamics(state=self.state + 0.5*dt*k1, v_x=v_x, delta=delta, ax=ax, worldArray=worldArray, step=step)
             self.state = self.state + dt * k2
 
         elif method == "rk4":
-            k1 = self.dynamics(self.state, v_x, delta, ax)
-            k2 = self.dynamics(self.state + 0.5*dt*k1, v_x, delta, ax)
-            k3 = self.dynamics(self.state + 0.5*dt*k2, v_x, delta, ax)
-            k4 = self.dynamics(self.state + dt*k3, v_x, delta, ax)
+            k1 = self.dynamics(state=self.state, v_x=v_x, delta=delta, ax=ax, worldArray=worldArray, step=step)
+            k2 = self.dynamics(state=self.state + 0.5*dt*k1, v_x=v_x, delta=delta, ax=ax, worldArray=worldArray, step=step)
+            k3 = self.dynamics(state=self.state + 0.5*dt*k2, v_x=v_x, delta=delta, ax=ax, worldArray=worldArray, step=step)
+            k4 = self.dynamics(state=self.state + dt*k3, v_x=v_x, delta=delta, ax=ax,worldArray=worldArray, step=step)
             self.state = self.state + (dt/6.0) * (k1 + 2*k2 + 2*k3 + k4)
 
         else:
             raise ValueError(f"Unknown integration method: {method}")
     
-    def simulate(self, v_x: float, steering_inputs: List[float],
+    """
+    
+    v_x = longitudinal velocty (forward speed of vehicle (m/s))
+    steering_inputs = time series of steering angles 
+    
+    """
+    
+    def simulate(self, v_x: float, steering_inputs: List[float], worldArray: np.ndarray, step: int,
                 dt: float = 0.01, method: str = "rk4") -> Tuple[np.ndarray, np.ndarray]:
         """Run simulation with given steering input sequence"""
         self.state = np.array([0.0, 0.0])
@@ -158,7 +330,7 @@ def simulate(self, v_x: float, steering_inputs: List[float],
 
         for i, delta in enumerate(steering_inputs):
             t = (i + 1) * dt
-            self.integrate_step(v_x, delta, dt, ax=0.0, method=method)
+            self.integrate_step(v_x, delta, dt, ax=0.0, method=method, worldArray=worldArray, step=step)
 
             self.time_history.append(t)
             self.state_history.append(self.state.copy())
@@ -172,7 +344,6 @@ def reset(self):
         self.state_history = []
         self.input_history = []
 
-
 def validate_against_telemetry(csv_path: str, model: DoubleBicycleModel,
                               sample_window: int = 1000) -> dict:
     """Load telemetry and extract stats for model comparison"""
@@ -225,8 +396,8 @@ def validate_against_telemetry(csv_path: str, model: DoubleBicycleModel,
 
     return results
 
-
 def plot_response(model: DoubleBicycleModel, title: str = "Model Response"):
+
     if not model.time_history:
         print("No data. Run simulate() first.")
         return
@@ -259,8 +430,43 @@ def plot_response(model: DoubleBicycleModel, title: str = "Model Response"):
     plt.tight_layout()
     return fig
 
+# globally create these so calcYawRate() can use it.
+params = VehicleParameters()
+model = DoubleBicycleModel(params=params)
+
+def calcYawRate(worldArray:ndarray[np.float64], step: int) -> float:
+    
+    """
+    Calculate the Yaw Rate
+
+    :param worldArray: World State Array
+    :param step: Current step index
+    :return: Yaw Rate
+    """
+
+    dt = 1 / Parameters["stepsPerSecond"]
+
+    # load model's previous state so we can use it in integrate_step later
+    model.state = np.array([
+        worldArray[step-1, varLateralVelocty], # v_y
+        worldArray[step-1, varYawRate] # r
+    ])
+
+    # find steering angle for integrating the step
+    delta = worldArray[step, varSteerAngle]
+
+    # create a new state with new v_x and dt
+    v_x = worldArray[step, varSpeed]
+    model.integrate_step(v_x=v_x, delta=delta, dt=dt, worldArray=worldArray, step=step)
+
+    # extract yaw rate and lateral velocity
+    lateral_velocity = model.state[0]
+    yaw_rate = model.state[1]
+
+    return lateral_velocity, yaw_rate
 
 if __name__ == "__main__":
+
     print("\n--- FORMULA SLUG - DOUBLE BICYCLE YAW RATE MODEL ---\n")
 
     params = VehicleParameters()
@@ -271,7 +477,7 @@ def plot_response(model: DoubleBicycleModel, title: str = "Model Response"):
     print(f"  Yaw inertia: {params.yaw_inertia:.1f} kg·m²")
     print(f"  Tire stiffness: {params.C_alpha:.0f} N/rad")
 
-    model = DoubleBicycleModel(params, tire_model="linear")
+    model = DoubleBicycleModel(params=params)
 
     # Test 1: Step steer
     print("\nTEST 1: Step Steer")
@@ -286,7 +492,7 @@ def plot_response(model: DoubleBicycleModel, title: str = "Model Response"):
     ])
     steering_step = steering_step[:num_steps]
 
-    time_sim, states_sim = model.simulate(v_x, steering_step, dt=dt, method="rk4")
+    time_sim, states_sim = model.simulate(v_x, steering_step, dt=dt, method="rk4", )
 
     print(f"\nSpeed: {v_x:.1f} m/s ({v_x*3.6:.1f} km/h)")
     print(f"Duration: {duration:.2f} s")
@@ -296,8 +502,8 @@ def plot_response(model: DoubleBicycleModel, title: str = "Model Response"):
     print(f"  G-force: {v_x * states_sim[-1, 1] / 9.81:.3f}g")
 
     fig1 = plot_response(model, "Test 1: Step Steering")
-    fig1.savefig('/Users/brianlee/vscode_projects/formula_slug/fs_yawratemodel/test1_step_steer.png', dpi=150)
-    print("Saved to test1_step_steer.png")
+    # fig1.savefig('/Users/brianlee/vscode_projects/formula_slug/fs_yawratemodel/test1_step_steer.png', dpi=150)
+    # print("Saved to test1_step_steer.png")
 
     # Test 2: Ramp steer
     print("\nTEST 2: Ramp Steer")
@@ -322,8 +528,8 @@ def plot_response(model: DoubleBicycleModel, title: str = "Model Response"):
     print(f"  Steady-state G: {v_x * states_sim[-1, 1] / 9.81:.3f}g")
 
     fig2 = plot_response(model, "Test 2: Ramp Steering")
-    fig2.savefig('/Users/brianlee/vscode_projects/formula_slug/fs_yawratemodel/test2_ramp_steer.png', dpi=150)
-    print("Saved to test2_ramp_steer.png")
+    # fig2.savefig('/Users/brianlee/vscode_projects/formula_slug/fs_yawratemodel/test2_ramp_steer.png', dpi=150)
+    # print("Saved to test2_ramp_steer.png")
 
     # Test 3: Lane change
     print("\nTEST 3: Double Lane Change")
@@ -345,8 +551,8 @@ def plot_response(model: DoubleBicycleModel, title: str = "Model Response"):
     print(f"  Max G-force: {v_x * np.max(np.abs(states_sim[:, 1])) / 9.81:.3f}g")
 
     fig3 = plot_response(model, "Test 3: Double Lane Change")
-    fig3.savefig('/Users/brianlee/vscode_projects/formula_slug/fs_yawratemodel/test3_double_lanechange.png', dpi=150)
-    print("Saved to test3_double_lanechange.png")
+    # fig3.savefig('/Users/brianlee/vscode_projects/formula_slug/fs_yawratemodel/test3_double_lanechange.png', dpi=150)
+    # print("Saved to test3_double_lanechange.png")
 
     print("\n--- Done! ---")