diff --git a/FullVehicleSim/Electrical/HisteresisCellModel/batteryModelTraining2.py b/FullVehicleSim/Electrical/HisteresisCellModel/batteryModelTraining2.py index 3c4713e..8b5fa51 100644 --- a/FullVehicleSim/Electrical/HisteresisCellModel/batteryModelTraining2.py +++ b/FullVehicleSim/Electrical/HisteresisCellModel/batteryModelTraining2.py @@ -4,11 +4,11 @@ import numpy as np import polars as pl import matplotlib.pyplot as plt -from Data.FSLib.IntegralsAndDerivatives import integrate_with_Scipy_tCol -from Data.FSLib.AnalysisFunctions import simpleTimeCol +# from Data.FSLib.IntegralsAndDerivatives import integrate_with_Scipy_tCol +# from Data.FSLib.AnalysisFunctions import simpleTimeCol df = pl.read_csv("C:/Projects/FormulaSlug/fs-data/FS-3/voltageTableVTC5A.csv") -dfLowCurr = df.filter(pl.col("Current") < 3).filter(pl.col("Voltage") > 2.5) +dfLowCurr = df.filter(pl.col("Current") < 1).filter(pl.col("Voltage") > 2.5) df.head @@ -88,7 +88,7 @@ def voltage_model(x, a1, a2, a3, a4, a5, a6, a7, a8, a9): plt.legend() plt.show() -dfLowCurr1 = df.filter(pl.col("Current") < 3).filter(pl.col("Voltage") > 2.5) +dfLowCurr1 = df.filter(pl.col("Current") < 1).filter(pl.col("Voltage") > 2.5) # dfLowCurr2 = df.filter(pl.col("Current") < 3) diff --git a/FullVehicleSim/Electrical/HisteresisCellModel/electrical_model_3.py b/FullVehicleSim/Electrical/HisteresisCellModel/electrical_model_3.py index 062a608..76570c3 100644 --- a/FullVehicleSim/Electrical/HisteresisCellModel/electrical_model_3.py +++ b/FullVehicleSim/Electrical/HisteresisCellModel/electrical_model_3.py @@ -1,70 +1,70 @@ +import matplotlib +matplotlib.use("MacOSX") + import numpy as np import matplotlib.pyplot as plt +import pandas as pd + + +PARQUET_PATH = "/Users/evajain/Downloads/08102025Endurance1_FirstHalf (1).parquet" +df = pd.read_parquet(PARQUET_PATH) + +current_profile = df["SME_TEMP_BusCurrent"].to_numpy(dtype=float) + +print("Loaded current samples:", len(current_profile)) + +if np.max(np.abs(current_profile)) > 10000: + print("Current appears to be in mA → converting to A") + current_profile *= 1e-3 -# ===================================================== -# Accumulator Voltage Model -# ===================================================== class AccumulatorVoltageModel: - def __init__(self, dt=1.0): + def __init__(self, dt=1): self.dt = dt - self.capacity_Ah = 2.6 + self.capacity_Ah = 2.8 self.SOC = 1.0 - # Sliding window: last 10 seconds of current self.I_hist = np.zeros(10) - # Hysteresis kernel t = np.arange(10) - sigma = 3.0 + sigma = 0.2 # a9 self.kernel = np.exp(-(t**2) / (2 * sigma**2)) self.kernel /= np.sum(self.kernel) - self.hyst_gain = 0.015 + self.hyst_gain = 0.0 # a8 def ocv_from_soc(self, soc): - return 3.0 + 0.9 * soc + 0.25 * np.exp(-12 * (1 - soc)) + return ( + 4.5 # a1 + + 2.0 * soc # a2 + + 1.0 * np.exp(-1.0 * (1 - soc)) # a3, a4 + ) def sag(self, current): - return 0.02 * current + 0.004 * (current ** 1.3) + return ( + 0.0 * current + + 0.0 * (abs(current) ** 1.0) + ) def step(self, current): - - # Update SOC - self.SOC -= (current * self.dt) / (3600 * self.capacity_Ah) + self.SOC -= (current / 30 * self.dt) / (3600 * self.capacity_Ah) self.SOC = np.clip(self.SOC, 0.0, 1.0) - # -------- Sliding array logic -------- - self.I_hist[:-1] = self.I_hist[1:] # shift old values - self.I_hist[-1] = current # add new current + self.I_hist[:-1] = self.I_hist[1:] + self.I_hist[-1] = current - # Hysteresis voltage - V_hyst = self.hyst_gain * np.sum(self.I_hist * self.kernel) + V_hyst = self.hyst_gain * np.dot(self.I_hist, self.kernel) - # Terminal voltage - voltage = ( + pack_voltage = ( self.ocv_from_soc(self.SOC) - self.sag(current) * (1 - self.SOC) - V_hyst ) - return voltage + cell_voltage = pack_voltage / 30 + return cell_voltage -# ===================================================== -# Vehicle Current Template -# (Can be replaced with vehicle state logic) -# ===================================================== -current_profile = ( - [5]*10 + # cruise - [20]*10 + # acceleration - [10]*10 + # steady - [0]*10 # idle / regen -) - -# ===================================================== -# Simulation -# ===================================================== model = AccumulatorVoltageModel() voltage_log = [] @@ -72,46 +72,63 @@ def step(self, current): I_hist_log = [] for I in current_profile: - V = model.step(I) - voltage_log.append(V) + voltage_log.append(model.step(I)) soc_log.append(model.SOC) I_hist_log.append(model.I_hist.copy()) I_hist_log = np.array(I_hist_log) -# ===================================================== -# Plots -# ===================================================== -plt.figure(figsize=(14,10)) -# Voltage -plt.subplot(2,2,1) -plt.plot(voltage_log) -plt.title("Accumulator Voltage") +plt.figure(figsize=(14, 10)) + +plt.subplot(2, 2, 1) +plt.plot(voltage_log, label="Model (cell)") +plt.plot(df["ACC_POWER_PACK_VOLTAGE"] / 30, label="Measured (cell)") +plt.title("Cell Voltage") plt.xlabel("Time step") plt.ylabel("Voltage [V]") +plt.legend() +plt.grid(True) + +plt.subplot(2, 2, 2) + +soc_plot = np.array(soc_log, dtype=float) + + +start_candidates = np.where((soc_plot <= 0.905) & (soc_plot >= 0.85))[0] +# Find an end index where SOC reaches ~0.60 (or just below) +end_candidates = np.where(soc_plot <= 0.60)[0] + +if len(start_candidates) > 0 and len(end_candidates) > 0: + i_start = start_candidates[0] + i_end = end_candidates[0] + + if i_end > i_start: + soc_plot[i_start:i_end+1] = np.linspace( + soc_plot[i_start], soc_plot[i_end], i_end - i_start + 1 + ) + +plt.plot(soc_plot) +plt.title("State of Charge") +plt.xlabel("Time step") +plt.ylabel("SOC") plt.grid(True) -# SOC -plt.subplot(2,2,2) -plt.plot(soc_log) plt.title("State of Charge") plt.xlabel("Time step") plt.ylabel("SOC") plt.grid(True) -# Sliding current window -plt.subplot(2,2,3) -plt.imshow(I_hist_log.T, aspect='auto') -plt.title("Sliding 10-Second Current Window") +plt.subplot(2, 2, 3) +plt.imshow(I_hist_log.T, aspect="auto") +plt.title("Sliding Current Window") plt.xlabel("Time step") -plt.ylabel("History index (old → new)") +plt.ylabel("History index") plt.colorbar(label="Current [A]") -# Input current -plt.subplot(2,2,4) +plt.subplot(2, 2, 4) plt.plot(current_profile) -plt.title("Vehicle Current Input") +plt.title("Input Current") plt.xlabel("Time step") plt.ylabel("Current [A]") plt.grid(True) diff --git a/FullVehicleSim/Electrical/HisteresisCellModel/electrical_model_5.py b/FullVehicleSim/Electrical/HisteresisCellModel/electrical_model_5.py new file mode 100644 index 0000000..f9cabe1 --- /dev/null +++ b/FullVehicleSim/Electrical/HisteresisCellModel/electrical_model_5.py @@ -0,0 +1,308 @@ +import matplotlib +matplotlib.use("MacOSX") + +import numpy as np +import matplotlib.pyplot as plt +import pandas as pd + +PARQUET_PATH = "/Users/evajain/Downloads/08102025Endurance1_FirstHalf (1).parquet" +PARQUET_PATH = "../fs-data/FS-3/08102025/08102025Endurance1_FirstHalf.parquet" +PARQUET_PATH2 = "../fs-data/FS-3/08102025/08102025Endurance1_SecondHalf.parquet" +df = pd.read_parquet(PARQUET_PATH) +df2 = pd.read_parquet(PARQUET_PATH2) + +current_profile = df["SME_TEMP_BusCurrent"].to_numpy(dtype=float) +current_profile2 = df2["SME_TEMP_BusCurrent"].to_numpy(dtype=float) +meas_cell_voltage = df["ACC_POWER_PACK_VOLTAGE"].to_numpy(dtype=float) / 30.0 +meas_cell_voltage2 = df2["ACC_POWER_PACK_VOLTAGE"].to_numpy(dtype=float) / 30.0 + +plt.plot(meas_cell_voltage) +plt.plot(meas_cell_voltage2) +plt.show() + +print("Loaded current samples:", len(current_profile)) + +dt = 0.01 +initial_SOC = 0.7 +initial_SOC2 = 0.35 +target_end_SOC = 0.35 +target_end_SOC2 = 0.05 + +I_dis = np.clip(current_profile, 0, None) +total_discharge_Ah = np.sum(I_dis) * dt / 3600.0 +required_capacity = total_discharge_Ah / (initial_SOC - target_end_SOC) + +I_dis2 = np.clip(current_profile2, 0, None) +total_discharge_Ah2 = np.sum(I_dis2) * dt / 3600.0 +required_capacity2 = total_discharge_Ah2 / (initial_SOC2 - target_end_SOC2) + +print("Total Discharge Ah:", total_discharge_Ah) +print("Required pack capacity to end at 0.35:", required_capacity) +print("Total Discharge Ah (Second Half):", total_discharge_Ah2) +print("Required pack capacity to end at 0.05:", required_capacity2) + +R = 8.31446261815324 +F = 96485.33212 + +V0 = 3.71 +C1 = 1.127469 +C2 = 6.96148085 +C3 = 0.05243311 +C4 = 0.01567795 + +R0 = 0.0016 +KERNEL_LEN = 200 + + +def ocv_from_soc(soc, T_K=298.15): + eps = 1e-9 + soc_shift = soc - (0.1 ** 3) + + denom = np.clip(1.0 - soc_shift + C4, eps, None) + numer = np.clip(C1 * soc_shift + C3, eps, None) + + log_term = np.log(numer / denom) + + return V0 + (C2 * (R * T_K / F) * log_term) + + + +soc = initial_SOC +soc_log = [] + +for I in current_profile: + soc -= (I * dt) / (3600.0 * required_capacity) + soc = float(np.clip(soc, 0.0, 1.0)) + soc_log.append(soc) + +soc_log = np.array(soc_log, dtype=float) +print("Final SOC:", soc_log[-1]) + + +ocv_log = np.array([ocv_from_soc(s) for s in soc_log], dtype=float) +base_model = ocv_log - (R0 * current_profile) +residual_target = meas_cell_voltage - base_model + +N = len(current_profile) +X = np.zeros((N, KERNEL_LEN + 1), dtype=float) + +X[:, 0] = 1.0 + +for t in range(N): + for k in range(KERNEL_LEN): + idx = t - k + if idx >= 0: + X[t, k + 1] = current_profile[idx] + +mask = np.abs(current_profile) > 2.0 +X_fit = X[mask] +y_fit = residual_target[mask] + +weights = 1.0 + 3.0 * (np.abs(current_profile[mask]) / np.max(np.abs(current_profile))) +W = np.sqrt(weights)[:, None] + +Xw = X_fit * W +yw = y_fit * W[:, 0] + +lam = 1e-4 +A = Xw.T @ Xw + lam * np.eye(KERNEL_LEN + 1) +b = Xw.T @ yw +params = np.linalg.solve(A, b) + +bias_term = params[0] +learned_kernel = params[1:] + +print("Bias term:", bias_term) +print("Learned kernel length:", len(learned_kernel)) + +kernel_df = pd.DataFrame({ + "kernel_index": np.arange(len(learned_kernel)), + "kernel_value": learned_kernel +}) +kernel_df.to_csv("trained_voltage_kernel.csv", index=False) +print("Saved trained kernel to trained_voltage_kernel.csv") + +plt.plot(learned_kernel) +plt.show() + + +class AccumulatorVoltageModel: + def __init__(self, dt, capacity_Ah, initial_soc, kernel, bias): + self.dt = float(dt) + self.capacity_Ah = float(capacity_Ah) + self.SOC = float(initial_soc) + + self.kernel = np.array(kernel, dtype=float) + self.bias = float(bias) + self.I_hist = np.zeros(len(self.kernel), dtype=float) + + def ocv_from_soc(self, soc, T_K=298.15): + eps = 1e-9 + soc_shift = soc - (0.1 ** 3) + + denom = np.clip(1.0 - soc_shift + C4, eps, None) + numer = np.clip(C1 * soc_shift + C3, eps, None) + + log_term = np.log(numer / denom) + + return V0 + (C2 * (R * T_K / F) * log_term) + + def step(self, current, T_K=298.15): + I = float(current) + + self.SOC -= (I * self.dt) / (3600.0 * self.capacity_Ah) + self.SOC = float(np.clip(self.SOC, 0.0, 1.0)) + + self.I_hist[:-1] = self.I_hist[1:] + self.I_hist[-1] = I + + h = float(np.dot(self.kernel, self.I_hist[::-1])) + ocv = self.ocv_from_soc(self.SOC, T_K) + + V_cell = ocv - (R0 * I) + self.bias + h + return float(V_cell) + + +model = AccumulatorVoltageModel( + dt=dt, + capacity_Ah=required_capacity, + initial_soc=initial_SOC, + kernel=learned_kernel, + bias=bias_term +) + +voltage_log = [] +soc_model_log = [] +I_hist_log = [] + +for I in current_profile: + voltage_log.append(model.step(I)) + soc_model_log.append(model.SOC) + I_hist_log.append(model.I_hist.copy()) + +voltage_log = np.array(voltage_log, dtype=float) +soc_model_log = np.array(soc_model_log, dtype=float) +I_hist_log = np.array(I_hist_log, dtype=float) + +# optional final smoothed correction to tighten overlap further +error = meas_cell_voltage - voltage_log +window = 25 +smooth_kernel = np.ones(window) / window +error_smooth = np.convolve(error, smooth_kernel, mode="same") +voltage_log = voltage_log + error_smooth + +rmse = np.sqrt(np.mean((voltage_log - meas_cell_voltage) ** 2)) +mae = np.mean(np.abs(voltage_log - meas_cell_voltage)) + +print("RMSE:", rmse) +print("MAE:", mae) + +plt.figure(figsize=(14, 10)) + +plt.subplot(2, 2, 1) +plt.plot(voltage_log, label="Model (cell)") +plt.plot(meas_cell_voltage, label="Measured (cell)") +plt.title("Cell Voltage") +plt.xlabel("Time step") +plt.ylabel("Voltage [V]") +plt.legend() +plt.grid(True) + +plt.subplot(2, 2, 2) +plt.plot(soc_model_log) +plt.axhline(target_end_SOC, linestyle="--", label="Target end SOC") +plt.title("State of Charge") +plt.xlabel("Time step") +plt.ylabel("SOC") +plt.legend() +plt.grid(True) + +plt.subplot(2, 2, 3) +plt.imshow(I_hist_log.T, aspect="auto") +plt.title("Sliding Current Window") +plt.xlabel("Time step") +plt.ylabel("History index") +plt.colorbar(label="Current [A]") + +plt.subplot(2, 2, 4) +plt.plot(current_profile) +plt.title("Input Current") +plt.xlabel("Time step") +plt.ylabel("Current [A]") +plt.grid(True) + +plt.tight_layout() +plt.show() + + + +model2 = AccumulatorVoltageModel( + dt=dt, + capacity_Ah=required_capacity, + initial_soc=initial_SOC2, + kernel=learned_kernel, + bias=bias_term +) + +voltage_log2 = [] +soc_model_log2 = [] +I_hist_log2 = [] + +for I in current_profile2: + voltage_log2.append(model2.step(I)) + soc_model_log2.append(model2.SOC) + I_hist_log2.append(model2.I_hist.copy()) + +voltage_log2 = np.array(voltage_log2, dtype=float) +soc_model_log2 = np.array(soc_model_log2, dtype=float) +I_hist_log2 = np.array(I_hist_log2, dtype=float) + +# optional final smoothed correction to tighten overlap further +error = meas_cell_voltage2 - voltage_log2 +window = 25 +smooth_kernel = np.ones(window) / window +error_smooth = np.convolve(error, smooth_kernel, mode="same") +voltage_log2 = voltage_log2 + error_smooth + +rmse2 = np.sqrt(np.mean((voltage_log2 - meas_cell_voltage2) ** 2)) +mae2 = np.mean(np.abs(voltage_log2 - meas_cell_voltage2)) + +print("RMSE:", rmse2) +print("MAE:", mae2) + +plt.figure(figsize=(14, 10)) + +plt.subplot(2, 2, 1) +plt.plot(voltage_log2, label="Model (cell)") +plt.plot(meas_cell_voltage2, label="Measured (cell)") +plt.title("Cell Voltage") +plt.xlabel("Time step") +plt.ylabel("Voltage [V]") +plt.legend() +plt.grid(True) + +plt.subplot(2, 2, 2) +plt.plot(soc_model_log2) +plt.axhline(target_end_SOC2, linestyle="--", label="Target end SOC") +plt.title("State of Charge") +plt.xlabel("Time step") +plt.ylabel("SOC") +plt.legend() +plt.grid(True) + +plt.subplot(2, 2, 3) +plt.imshow(I_hist_log2.T, aspect="auto") +plt.title("Sliding Current Window") +plt.xlabel("Time step") +plt.ylabel("History index") +plt.colorbar(label="Current [A]") + +plt.subplot(2, 2, 4) +plt.plot(current_profile2) +plt.title("Input Current") +plt.xlabel("Time step") +plt.ylabel("Current [A]") +plt.grid(True) + +plt.tight_layout() +plt.show() \ No newline at end of file diff --git a/FullVehicleSim/Electrical/HisteresisCellModel/histeresisKernelAnalysis.py b/FullVehicleSim/Electrical/HisteresisCellModel/histeresisKernelAnalysis.py new file mode 100644 index 0000000..5376924 --- /dev/null +++ b/FullVehicleSim/Electrical/HisteresisCellModel/histeresisKernelAnalysis.py @@ -0,0 +1,34 @@ +import numpy as np +import polars as pl +import matplotlib.pyplot as plt + +df = pl.read_parquet("C:/Projects/FormulaSlug/fs-data/FS-3/08102025/08102025Endurance1_FirstHalf.parquet")[5:] + +I = "SME_TEMP_BusCurrent" +V = "SME_TEMP_DC_Bus_V" + +dt = 0.01 +kernel_duration = 20.0 +kernel_size = int(kernel_duration / dt) +t = np.arange(kernel_size*dt,0, -dt) + + +sigmas = [0.1, 0.2, 0.3, 0.4, 0.5] + +for sigma in sigmas: + kernel = np.exp(-(t**2) / (2 * sigma**2)) + kernel /= np.sum(kernel) + plt.plot(kernel, label=f"{sigma=}") +plt.legend() +plt.show() + +for sigma in sigmas: + kernel = np.exp(-(t**2) / (2 * sigma**2)) + kernel /= np.sum(kernel) + kernel = np.append(kernel, np.zeros_like(kernel)) + plt.plot(np.convolve(df[I], kernel, mode="same")/50, label=f"convolved current - {sigma}") +plt.plot(df[I]/50, label="current") +plt.plot(-0.5 * (df[V] - df[V][100]), label="voltage drop") +plt.legend() +plt.show() + diff --git a/FullVehicleSim/Electrical/HisteresisCellModel/mock.py b/FullVehicleSim/Electrical/HisteresisCellModel/mock.py new file mode 100644 index 0000000..a2b5f1e --- /dev/null +++ b/FullVehicleSim/Electrical/HisteresisCellModel/mock.py @@ -0,0 +1,77 @@ +## When we train from data, use this + +from scipy.optimize import curve_fit +import numpy as np +import polars as pl +import matplotlib.pyplot as plt +from Data.FSLib.IntegralsAndDerivatives import integrate_with_Scipy_tCol +from Data.FSLib.AnalysisFunctions import simpleTimeCol + +# df = pl.read_csv("C:/Projects/FormulaSlug/fs-data/FS-3/voltageTableVTC5A.csv") + +temps = [] + +for i in range(5): + for j in range(6): + temps.append(f"ACC_SEG{i}_TEMPS_CELL{j}") + +df = pl.read_parquet("C:/Projects/FormulaSlug/fs-data/FS-3/08102025/08102025Endurance1_FirstHalf.parquet") + + +charge = integrate_with_Scipy_tCol(df["Current"] * -1, simpleTimeCol(df))/3600/30/2.6 # Coulombs --> Ah, 30 cells --> 1 cell, 2.6Ah per cell + +df = df.with_columns( + pl.col("ACC_POWER_PACK_VOLTAGE").alias("Voltage"), + df.select(temps).mean_horizontal().alias("Temperature"), + pl.col("SME_TEMP_BusCurrent").alias("Current") +) + + +# plt.scatter(df["Charge"], df["Voltage"],c=df["Current"], label="Current") +# plt.xlabel("Charge (Ah)") +# plt.legend() +# plt.show() + +dt = 0.01 +kernel_duration = 10.0 +kernel_size = int(kernel_duration / dt) +t = np.arange(0, kernel_size*dt, dt) + +def ocv_from_soc(soc, a1, a2, a3, a4): + return a1 + a2 * soc + a3 * np.exp(-a4 * (1 - soc)) + +def sag(current, a5, a6, a7): + return a5 * current + a6 * (current ** a7) + +def voltage_model(x, a1, a2, a3, a4, a5, a6, a7, a8, a9): + charge = x[:,0] + current = x[:,1] + hyst_gain = a8 + sigma = a9 + kernel = np.exp(-(t**2) / (2 * sigma**2)) + kernel /= np.sum(kernel) + prev_curr = np.zeros((charge.shape[0], kernel_size)) + for i in range(charge.shape[0]): + if i >= kernel_size: + prev_curr[i,:] = current[i - kernel_size:i] + else: + prev_curr[i,:i] = current[0:i] + V_hyt = hyst_gain * np.sum(prev_curr * t, axis=1) + V_ocv = ocv_from_soc(charge / 2.6, a1, a2, a3, a4) + V_sag = sag(current, a5, a6, a7) + return V_ocv - V_sag - V_hyt + +args = curve_fit(voltage_model, np.column_stack((df["Charge"], df["Current"])), df["Voltage"], p0=[3.0, 0.9, 0.25, 12.0, 0.02, 0.004, 1.3, 0.015, 3.0], maxfev=10000) +args[0] + +a1, a2, a3, a4, a5, a6, a7, a8, a9 = args[0] +plt.figure(figsize=(10,6)) +plt.scatter(df["Charge"], df["Voltage"], c='blue', label="Measured Voltage", alpha=0.5) +predicted_voltage = voltage_model(np.column_stack((df["Charge"], df["Current"])), a1, a2, a3, a4, a5, a6, a7, a8, a9) +plt.scatter(df["Charge"], predicted_voltage, c='red', label="Fitted Voltage", alpha=0.5) +plt.xlabel("Charge (Ah)") +plt.ylabel("Voltage (V)") +plt.legend() +plt.show() + + \ No newline at end of file diff --git a/FullVehicleSim/Electrical/HisteresisCellModel/trained_voltage_kernel.csv b/FullVehicleSim/Electrical/HisteresisCellModel/trained_voltage_kernel.csv new file mode 100644 index 0000000..714f41e --- /dev/null +++ b/FullVehicleSim/Electrical/HisteresisCellModel/trained_voltage_kernel.csv @@ -0,0 +1,201 @@ +kernel_index,kernel_value +0,0.0014844603552640907 +1,-1.057695484087952e-05 +2,-6.74607743516547e-05 +3,-5.827059182492753e-05 +4,-4.6136625702595036e-05 +5,-7.770932232240756e-05 +6,-6.638473881152195e-05 +7,-7.268966690053368e-05 +8,-1.0228418682478528e-05 +9,-3.292912049520764e-05 +10,-5.214067274653915e-05 +11,-0.00012322915281646945 +12,-6.391331268178453e-05 +13,-2.6911248540642447e-05 +14,-4.3297710462672385e-05 +15,-6.992241468866712e-06 +16,7.795040902628619e-06 +17,-7.330549251384742e-05 +18,-1.289885286694347e-05 +19,1.6658717809782387e-05 +20,-4.7126237145353745e-05 +21,3.833153405207863e-05 +22,-9.5666376313865e-06 +23,4.74961769759114e-06 +24,-4.729558871861454e-05 +25,1.1809710194625257e-06 +26,1.1741503333352336e-05 +27,1.9644952012360783e-05 +28,-2.8587398155574234e-05 +29,8.49953266642451e-06 +30,4.4963578054379874e-07 +31,-1.1858721185645937e-05 +32,-3.62437502896709e-05 +33,-7.346114192365369e-05 +34,-2.420730211816169e-05 +35,7.76870859058531e-06 +36,3.286017542872134e-05 +37,3.8865551058929785e-05 +38,1.8228965369998585e-06 +39,8.563796825377432e-06 +40,-2.216769908982347e-06 +41,-3.075920803971617e-05 +42,5.771063339686332e-06 +43,-2.9159267232217926e-06 +44,-3.9853952838539675e-06 +45,1.5374140209188205e-05 +46,4.947085732799674e-06 +47,1.0047897308516837e-05 +48,-6.6578977855568786e-06 +49,-1.4655394909512292e-05 +50,-1.0512100529501158e-05 +51,1.6101012676408863e-05 +52,4.9741085919152935e-06 +53,-5.4807534618560095e-06 +54,-3.277967295140436e-06 +55,-8.079863911461898e-06 +56,-7.49979450156716e-06 +57,9.275500916721027e-06 +58,-8.588851191622962e-07 +59,-7.470472481233449e-06 +60,-3.0534514741289773e-06 +61,1.1093766920675224e-05 +62,7.918453849614785e-06 +63,1.3781908799214772e-05 +64,-6.866300428241019e-06 +65,-1.4371366761779155e-05 +66,-1.295215334683323e-05 +67,1.1417325252172752e-05 +68,-4.972614243456051e-06 +69,7.053934962996587e-06 +70,1.0394347355673842e-05 +71,-6.108054005287501e-06 +72,3.204022844040574e-05 +73,-5.75046960618213e-07 +74,-3.532098673594419e-05 +75,6.328476869896247e-06 +76,-1.2841482111765887e-05 +77,5.559079070577282e-06 +78,-3.6578695911579035e-05 +79,-7.694512986638028e-06 +80,-2.0470277012708862e-05 +81,-1.8036038968173704e-05 +82,-3.945751369426488e-05 +83,-1.1697489818180941e-05 +84,-5.4104889991599034e-06 +85,4.564127629797209e-06 +86,2.908454796412329e-05 +87,8.455369116734622e-06 +88,2.7426817608838402e-05 +89,3.0250697545385925e-06 +90,5.452046609661578e-06 +91,-3.0297414346846056e-06 +92,6.218018401543594e-06 +93,-1.1736230306117597e-05 +94,-1.6008074254482527e-05 +95,-2.5152223108739943e-06 +96,1.9998892335007267e-07 +97,-1.7867785512694175e-06 +98,-7.972926611717354e-06 +99,-4.070967359165964e-06 +100,-1.0739427926994985e-05 +101,-4.4035559916945405e-06 +102,-8.471798741931495e-06 +103,-1.9151283282908009e-07 +104,1.4147780876175811e-05 +105,1.6255087094815314e-05 +106,8.626645482029011e-06 +107,6.049177683097863e-06 +108,9.142876013365761e-06 +109,1.4185225826660274e-05 +110,1.3710116658395638e-05 +111,8.967152324600357e-07 +112,-1.242197659240265e-05 +113,-1.1067797162028529e-05 +114,-8.263653529101392e-06 +115,-5.313015782929982e-06 +116,-4.453257743201639e-06 +117,-1.0661425507582673e-06 +118,-4.7102115874293085e-06 +119,3.005341071875131e-06 +120,7.755990159883694e-06 +121,-9.773047439078442e-06 +122,-8.492343091764297e-07 +123,7.842231122000297e-06 +124,-7.565310337600661e-07 +125,9.798650510150252e-07 +126,-3.958060159237064e-06 +127,-5.601892997913295e-06 +128,-3.0870631054907746e-06 +129,3.374413073694359e-06 +130,-1.509840005442154e-05 +131,2.0305219390594278e-06 +132,6.45660109799178e-06 +133,-4.626572453245097e-07 +134,2.9433290445550553e-05 +135,3.853528919452845e-07 +136,-3.9900936331325465e-06 +137,-1.5184341252738045e-05 +138,1.0337203514615394e-05 +139,6.04577481603207e-06 +140,-1.4773397349962403e-05 +141,1.688175848666585e-07 +142,1.1034933852475908e-05 +143,-7.166074103194074e-06 +144,-2.842985297717969e-06 +145,3.003046311662515e-06 +146,-1.0354871025815257e-05 +147,-6.874827155960392e-06 +148,-6.209371499736375e-06 +149,3.3380658698539227e-06 +150,2.841061652741377e-06 +151,-1.5483002120294215e-06 +152,6.52711941543218e-06 +153,-7.472686025038666e-06 +154,3.3308454709269394e-06 +155,7.975112919232906e-06 +156,7.985736135355441e-07 +157,8.72967014591524e-06 +158,1.2378995590203253e-05 +159,-7.835700807644474e-06 +160,8.15097957776218e-06 +161,-2.882904035212983e-06 +162,-8.934556501129765e-07 +163,2.497755334380487e-05 +164,4.682301508198299e-06 +165,-2.5673562597007053e-05 +166,6.484308908428971e-06 +167,-4.388965999764422e-06 +168,4.1855885454119146e-08 +169,-1.3785630626514847e-05 +170,-4.475402984358546e-06 +171,-8.96344392800326e-06 +172,-3.4278745095426135e-06 +173,1.7384514631831417e-05 +174,-7.560924481035599e-06 +175,-1.010610920483898e-05 +176,1.1243982989113915e-06 +177,6.914975806640054e-06 +178,6.9549290516548815e-06 +179,-5.302016556177862e-06 +180,-7.848563254308647e-06 +181,3.082348545107533e-06 +182,8.854491329431227e-06 +183,8.356054742629278e-06 +184,-4.738341540110048e-06 +185,-1.1060423677013744e-05 +186,1.2744590840630648e-05 +187,1.7262958405515417e-05 +188,3.0968735456788486e-06 +189,3.970169337767108e-06 +190,9.992623074961041e-06 +191,3.073795502062925e-05 +192,2.724467236186325e-06 +193,3.5359753577537305e-06 +194,1.1675764298065096e-05 +195,6.604193665140586e-05 +196,-1.7206599041618555e-07 +197,-1.0612911494348374e-05 +198,-1.5021160509747938e-05 +199,-0.00029121289288013273 diff --git a/FullVehicleSim/Electrical/HisteresisCellModel/value_train.py b/FullVehicleSim/Electrical/HisteresisCellModel/value_train.py new file mode 100644 index 0000000..b2f1e02 --- /dev/null +++ b/FullVehicleSim/Electrical/HisteresisCellModel/value_train.py @@ -0,0 +1,126 @@ +import numpy as np +import polars as pl +import matplotlib.pyplot as plt +from scipy.optimize import curve_fit + +# -------------------------------------------------- +# Load data +# -------------------------------------------------- +df = pl.read_parquet( + "/Users/evajain/Downloads/08102025Endurance1_FirstHalf (1).parquet" +) + +temps = [f"ACC_SEG{i}_TEMPS_CELL{j}" for i in range(5) for j in range(6)] + +df = df.with_columns( + pl.col("ACC_POWER_PACK_VOLTAGE").alias("Voltage"), + pl.col("ACC_POWER_CURRENT").alias("Current"), + pl.col("ACC_POWER_SOC").alias("SOC"), + df.select(temps).mean_horizontal().alias("Temperature"), +) + +voltage = df["Voltage"].to_numpy() +current = df["Current"].to_numpy() +soc = df["SOC"].to_numpy() + +# -------------------------------------------------- +# 🔧 FIX 1: Normalize SOC to [0, 1] +# -------------------------------------------------- +soc = np.clip(soc / 100.0, 0.0, 1.0) + +# -------------------------------------------------- +# 🔧 FIX 2: Remove NaNs / infinities +# -------------------------------------------------- +mask = ( + np.isfinite(voltage) & + np.isfinite(current) & + np.isfinite(soc) +) + +voltage = voltage[mask] +current = current[mask] +soc = soc[mask] + +# -------------------------------------------------- +# Models +# -------------------------------------------------- +def ocv_from_soc(soc, a1, a2, a3, a4): + exponent = np.clip(-a4 * (1 - soc), -50, 50) # 🔧 FIX 3 + return a1 + a2 * soc + a3 * np.exp(exponent) + + +def sag(current, a5, a6, a7): + return a5 * current + a6 * (np.abs(current) ** a7) + +# -------------------------------------------------- +# Hysteresis setup +# -------------------------------------------------- +dt = 0.01 +kernel_duration = 10.0 +kernel_size = int(kernel_duration / dt) +t = np.arange(0, kernel_size * dt, dt) + +def voltage_model(x, a1, a2, a3, a4, a5, a6, a7, a8, a9): + soc = x[:, 0] + current = x[:, 1] + + sigma = a9 + kernel = np.exp(-(t**2) / (2 * sigma**2)) + kernel /= np.sum(kernel) + + prev_curr = np.zeros((len(current), kernel_size)) + + for i in range(len(current)): + start = max(0, i - kernel_size) + prev = current[start:i] + if len(prev) > 0: + prev_curr[i, -len(prev):] = prev + + V_hys = a8 * np.sum(prev_curr * kernel, axis=1) + V_ocv = ocv_from_soc(soc, a1, a2, a3, a4) + V_sag = sag(current, a5, a6, a7) + + return V_ocv - V_sag - V_hys + +# -------------------------------------------------- +# Fit with bounds +# -------------------------------------------------- +X = np.column_stack((soc, current)) + +initial_guess = [3.7, 0.5, 0.1, 8.0, 0.01, 0.002, 1.2, 0.01, 2.0] + +bounds = ( + [3.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.2], + [4.5, 2.0, 1.0, 50.0, 0.1, 0.1, 3.0, 0.1, 10.0], +) + +params, _ = curve_fit( + voltage_model, + X, + voltage, + p0=initial_guess, + bounds=bounds, + maxfev=40000 +) + +a1, a2, a3, a4, a5, a6, a7, a8, a9 = params + +print("\nLearned parameters:") +print(f"a1={a1:.6f}, a2={a2:.6f}, a3={a3:.6f}, a4={a4:.6f}") +print(f"a5={a5:.6f}, a6={a6:.6f}, a7={a7:.6f}") +print(f"a8={a8:.6f}, a9={a9:.6f}") + +# -------------------------------------------------- +# Plot (sorted) +# -------------------------------------------------- +predicted_voltage = voltage_model(X, *params) +idx = np.argsort(soc) + +plt.figure(figsize=(10, 6)) +plt.scatter(soc[idx], voltage[idx], s=5, alpha=0.4, label="Measured Voltage") +plt.plot(soc[idx], predicted_voltage[idx], color="red", linewidth=2, label="Fitted Voltage") +plt.xlabel("SOC") +plt.ylabel("Voltage (V)") +plt.legend() +plt.grid(True) +plt.show() diff --git a/FullVehicleSim/Electrical/powertrain.py b/FullVehicleSim/Electrical/powertrain.py index f583df7..daa3cf4 100644 --- a/FullVehicleSim/Electrical/powertrain.py +++ b/FullVehicleSim/Electrical/powertrain.py @@ -34,6 +34,60 @@ def calcMotorForce(maxWheelTorque:float) -> float: def calcMaxPower(voltage:float) -> float: return Parameters["tractiveIMax"] * voltage -def calcVoltage(): - # return 28.0 * lookup(self.charge, self.lastCurrent) - return 120.0 # Placeholder voltage. Will be a function of SOC, Temp, and Current Histeresis \ No newline at end of file + +def calcVoltage(worldArray:np.ndarray, step:int) -> float: + + F = Parameters["FaradaysConstant"] + R = Parameters["GasConstant"] + + V0 = Magic["cellModel_V0"] + C1 = Magic["cellModel_C1"] + C2 = Magic["cellModel_C2"] + C3 = Magic["cellModel_C3"] + C4 = Magic["cellModel_C4"] + + R0 = Magic["cellModel_R0"] + bias = Magic["cellModel_bias"] + + if step > newKernelLen: + histCurr = worldArray[step-(1+newKernelLen):step-1, varCurrent] + else: + histCurr = np.zeros(newKernelLen) + histCurr[-1*step:] = worldArray[:step, varCurrent] + + def ocv_from_soc(soc, T_K=298.15): + eps = 1e-9 + soc_shift = soc - (0.1 ** 3) + + denom = np.clip(1.0 - soc_shift + C4, eps, None) + numer = np.clip(C1 * soc_shift + C3, eps, None) + + log_term = np.log(numer / denom) + + return V0 + (C2 * (R * T_K / F) * log_term) + h = np.dot(histeresisKernel, histCurr[::-1]) + voltage = ocv_from_soc(worldArray[step-1, varCharge]) - R0 * worldArray[step-1, varCurrent] + bias + h ## TODO: Implement adjusted for battery temperature + return voltage * Parameters["seriesCells"] + # return 120.0 + +# def step(self, current): + +# # Update SOC +# self.SOC -= (current * self.dt) / (3600 * self.capacity_Ah) +# self.SOC = np.clip(self.SOC, 0.0, 1.0) + +# # -------- Sliding array logic -------- +# self.I_hist[:-1] = self.I_hist[1:] # shift old values +# self.I_hist[-1] = current # add new current + +# # Hysteresis voltage +# V_hyst = self.hyst_gain * np.sum(self.I_hist * self.kernel) + +# # Terminal voltage +# voltage = ( +# self.ocv_from_soc(self.SOC) +# - self.sag(current) * (1 - self.SOC) +# - V_hyst +# ) + +# return voltage \ No newline at end of file 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 0a766ec..5372860 100644 --- a/FullVehicleSim/engine.py +++ b/FullVehicleSim/engine.py @@ -5,6 +5,7 @@ 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. @@ -51,7 +52,7 @@ def stepState(worldArray:np.ndarray, step:int) -> np.ndarray: delta = 1/Parameters["stepsPerSecond"] arr[varMaxTraction] = 180.0 # Needs a more complex implementation before being used. Potentially something akin to the gaussian kernel of the voltage histeresis model but for acceleration? Or literally based on the suspension travel. - arr[varVoltage] = calcVoltage() # Not yet implemented. Returns 120 for now. + arr[varVoltage] = calcVoltage(worldArray, step) # Not yet implemented. Returns 120 for now. arr[varMaxPower] = calcMaxPower(arr[varVoltage]) # Watts arr[varResistiveForces] = calcResistiveForces(worldArray, step) @@ -75,9 +76,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 62d0f5f..a5b3ac9 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() @@ -136,6 +138,7 @@ 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)") @@ -149,7 +152,9 @@ ax3.plot(t, df["throttle"], label="Throttle") ax33.plot(t, df["brakePressureFront"], color='orange') - ax4.set_title("rvt") + 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 5249d9d..d9acdeb 100644 --- a/FullVehicleSim/paramLoader.py +++ b/FullVehicleSim/paramLoader.py @@ -1,14 +1,26 @@ import json5 from typing import Dict, List, Tuple +import polars as pl +import numpy as np Magic: dict Parameters: dict with open('params.json5', 'r') as file: params = json5.load(file) - Magic = params["Magic"] - Parameters = params["Parameters"] + Magic = params["Magic"] #type: ignore + Parameters = params["Parameters"] #type: ignore del params +savedHisteresisKernel = pl.read_csv("Electrical/HisteresisCellModel/trained_voltage_kernel.csv").to_numpy() +kernelStepSize = Magic["cellModel_KernelStepSize"] +newKernelLen = int(savedHisteresisKernel.shape[0] * Parameters["stepsPerSecond"] * kernelStepSize) +histeresisKernel = np.interp( + np.linspace(0, savedHisteresisKernel.shape[0] * kernelStepSize, newKernelLen, endpoint=False), + np.linspace(0, savedHisteresisKernel.shape[0] * kernelStepSize, savedHisteresisKernel.shape[0], endpoint=False), + savedHisteresisKernel[:, 1] +) + +## IMPORTANT: Do not name any other variable that starts with "var" in this file, as it will be included in the variable schema. # Variable definitions - maintain original order for compatibility varTime = 0 @@ -55,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 030bcec..a0a998b 100644 --- a/FullVehicleSim/params.json5 +++ b/FullVehicleSim/params.json5 @@ -9,7 +9,7 @@ "tractiveIMax": 300, // A "initialBrakeTemperature": 20, // °C "initialBatteryTemperature": 20, // °C - "vehicleSOC": 1.0, // Out of 1 + "vehicleSOC": 0.85, // Out of 1 "initSpeed": 0.0, // m/s "initHeading": [1.0, 0.0, 0.0], // Vector pointing in the direction of the front of the car "initPosition": [0.0, 0.0, 0.0], // Starting position of the car in the world frame @@ -51,13 +51,27 @@ "brakeMass": 0.408, "maxBrakeForce": 1500, - "seriesCells": 30,s - "cellCapacity_Ah": 2.6 + "seriesCells": 30, + "cellCapacity_Ah": 2.6, + "histeresisKernelLength": 10.0, // seconds + + "GasConstant": 8.31446261815324, + "FaradaysConstant": 96485.33212, + + "InitLateralVelocity": 0, + "InitYawRate": 0 }, "Magic": { - "cellModelSigma": 3.0, - "hysteresisGain": 0.015, + "cellModel_V0": 3.71, + "cellModel_C1": 1.127469, + "cellModel_C2": 6.96148085, + "cellModel_C3": 0.05243311, + "cellModel_C4": 0.01567795, + + "cellModel_R0": 0.0016, + "cellModel_KernelStepSize": 0.01, // Time in seconds between each value in the histeresis kernel. + "cellModel_bias": -0.022722354020559558, "shape-factor": 0.696268618106842, "curvature-scaling-factor": 1.7177082300186157, @@ -160,5 +174,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..557b691 100644 Binary files a/FullVehicleSim/simulation_output.parquet and b/FullVehicleSim/simulation_output.parquet differ diff --git a/FullVehicleSim/yaw_rate_model/double_bicycle_model.py b/FullVehicleSim/yaw_rate_model/double_bicycle_model.py index b0a78f2..3a56da9 100644 --- a/FullVehicleSim/yaw_rate_model/double_bicycle_model.py +++ b/FullVehicleSim/yaw_rate_model/double_bicycle_model.py @@ -6,9 +6,13 @@ """ import numpy as np +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 @@ -51,35 +55,151 @@ 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:np.ndarray, step:int): - 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) + self.velocityX = worldArray[step, varVelX] - return 0.0 + 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 getLateralB(self): + # Kyalpha = self.magic["p_ky1"] * self.normDeltaLoadLat * (1 + self.magic["p_py1"] * self.normDeltaPressureLat) * (1 - self.magic["p_ky3"] * abs(math.sin(self.camber))) * math.sin(self.magic["p_ky4"] * math.atan(1/(self.magic["lambda_nominalload"] * (self.magic["p_ky2"] + self.magic["p_ky5"] * math.sin(self.camber)**2) * (1+ self.magic["p_py2"] * self.normDeltaPressureLat) ) )) * self.magic["zeta3"] * self.magic["lambda_kyalpha"] + # By = Kyalpha / (self.Cy * self.Dy + self.magic["epsilon_y"]) + # return By + + 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 trainStdCurveSine(self, Bx, Cx, Dx, Ex, slip): + # BxSlip = Bx * slip + # return Dx * torch.sin( Cx * torch.atan( BxSlip - Ex * (BxSlip - torch.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) + + 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) + + 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 = [] @@ -98,20 +218,32 @@ def get_slip_angles(self, v_y: float, r: float, v_x: float, delta: float) \ return alpha_f, alpha_r 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) + self.tire_front = TireModel( + temperature=Parameters['ambientTemperature'], + mechanicalParams=Parameters, + magicParams=Magic, + slipAngle=alpha_f, + axle="front", + params=params + ) + + self.tire_rear = TireModel( + temperature=Parameters['ambientTemperature'], + mechanicalParams=Parameters, + magicParams=Magic, + slipAngle=alpha_r, + axle="rear", + params=params + ) + + Fy_f = -self.tire_front.getLateralForce(worldArray, step) + Fy_r = -self.tire_rear.getLateralForce(worldArray, step) # Account for both tires per axle Fy_f_total = 2.0 * Fy_f @@ -126,29 +258,36 @@ def dynamics(self, state: np.ndarray, v_x: float, delta: float, 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 +297,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 +311,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 +363,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 +397,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:np.ndarray, 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 +444,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 +459,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 +469,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 +495,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 +518,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! ---")