mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-07-26 12:22:04 +08:00
torqued
This commit is contained in:
MoreTore
@@ -584,6 +584,8 @@ struct CarParams {
|
||||
steeringAngleDeadzoneDeg @5 :Float32;
|
||||
latAccelFactor @6 :Float32;
|
||||
latAccelOffset @7 :Float32;
|
||||
sigmoidSharpness @8 :Float32; # a
|
||||
sigmoidTorqueGain @9 :Float32; # b
|
||||
}
|
||||
|
||||
struct LongitudinalPIDTuning {
|
||||
|
||||
@@ -2114,6 +2114,10 @@ struct LiveTorqueParametersData {
|
||||
latAccelFactorFiltered @4 :Float32;
|
||||
latAccelOffsetFiltered @5 :Float32;
|
||||
frictionCoefficientFiltered @6 :Float32;
|
||||
sigmoidSharpnessRaw @13 :Float32;
|
||||
sigmoidTorqueGainRaw @14 :Float32;
|
||||
sigmoidSharpnessFiltered @15 :Float32;
|
||||
sigmoidTorqueGainFiltered @16 :Float32;
|
||||
totalBucketPoints @7 :Float32;
|
||||
decay @8 :Float32;
|
||||
maxResets @9 :Float32;
|
||||
|
||||
@@ -419,6 +419,8 @@ class CarInterfaceBase(ABC):
|
||||
tune.torque.ki = 0.1
|
||||
tune.torque.friction = params['FRICTION']
|
||||
tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR']
|
||||
tune.torque.sigmoidSharpness = params['SIGMOID_SHARPNESS']
|
||||
tune.torque.sigmoidTorqueGain = params['SIGMOID_TORQUE_GAIN']
|
||||
tune.torque.latAccelOffset = 0.0
|
||||
tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg
|
||||
|
||||
|
||||
@@ -34,9 +34,7 @@ class CarInterface(CarInterfaceBase):
|
||||
# An important thing to consider is that the slope at 0 should be > 0 (ideally >1)
|
||||
# This has big effect on the stability about 0 (noise when going straight)
|
||||
# ToDo: To generalize to other GMs, explore tanh function as the nonlinear
|
||||
non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
|
||||
assert non_linear_torque_params, "The params are not defined"
|
||||
a, b, c, _ = non_linear_torque_params
|
||||
a, b, c = torque_params.sigmoidSharpness, torque_params.sigmoidTorqueGain, torque_params.latAccelFactor
|
||||
steer_torque = (sig(latcontrol_inputs.lateral_acceleration * a) * b) + (latcontrol_inputs.lateral_acceleration * c)
|
||||
return float(steer_torque) + friction
|
||||
|
||||
|
||||
@@ -1,85 +1,78 @@
|
||||
legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
|
||||
legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION", "SIGMOID_SHARPNESS", "SIGMOID_TORQUE_GAIN"]
|
||||
### angle control
|
||||
# Nissan appears to have torque
|
||||
"NISSAN_XTRAIL" = [nan, 1.5, nan]
|
||||
"NISSAN_ALTIMA" = [nan, 1.5, nan]
|
||||
"NISSAN_LEAF_IC" = [nan, 1.5, nan]
|
||||
"NISSAN_LEAF" = [nan, 1.5, nan]
|
||||
"NISSAN_ROGUE" = [nan, 1.5, nan]
|
||||
"NISSAN_XTRAIL" = [nan, 1.5, nan, nan, nan]
|
||||
"NISSAN_ALTIMA" = [nan, 1.5, nan, nan, nan]
|
||||
"NISSAN_LEAF_IC" = [nan, 1.5, nan, nan, nan]
|
||||
"NISSAN_LEAF" = [nan, 1.5, nan, nan, nan]
|
||||
"NISSAN_ROGUE" = [nan, 1.5, nan, nan, nan]
|
||||
|
||||
# New subarus angle based controllers
|
||||
"SUBARU_FORESTER_2022" = [nan, 3.0, nan]
|
||||
"SUBARU_OUTBACK_2023" = [nan, 3.0, nan]
|
||||
"SUBARU_ASCENT_2023" = [nan, 3.0, nan]
|
||||
"SUBARU_FORESTER_2022" = [nan, 3.0, nan, nan, nan]
|
||||
"SUBARU_OUTBACK_2023" = [nan, 3.0, nan, nan, nan]
|
||||
"SUBARU_ASCENT_2023" = [nan, 3.0, nan, nan, nan]
|
||||
|
||||
# Toyota LTA also has torque
|
||||
"TOYOTA_RAV4_TSS2_2023" = [nan, 3.0, nan]
|
||||
"TOYOTA_RAV4_TSS2_2023" = [nan, 3.0, nan, nan, nan]
|
||||
|
||||
# Tesla has high torque
|
||||
"TESLA_AP1_MODELS" = [nan, 2.5, nan]
|
||||
"TESLA_AP2_MODELS" = [nan, 2.5, nan]
|
||||
"TESLA_MODELS_RAVEN" = [nan, 2.5, nan]
|
||||
# Tesla angle based controllers
|
||||
"TESLA_MODEL_3" = [nan, 2.5, nan, nan, nan]
|
||||
"TESLA_MODEL_Y" = [nan, 2.5, nan, nan, nan]
|
||||
|
||||
# Guess
|
||||
"FORD_BRONCO_SPORT_MK1" = [nan, 1.5, nan]
|
||||
"FORD_ESCAPE_MK4" = [nan, 1.5, nan]
|
||||
"FORD_EXPLORER_MK6" = [nan, 1.5, nan]
|
||||
"FORD_F_150_MK14" = [nan, 1.5, nan]
|
||||
"FORD_FOCUS_MK4" = [nan, 1.5, nan]
|
||||
"FORD_MAVERICK_MK1" = [nan, 1.5, nan]
|
||||
"FORD_F_150_LIGHTNING_MK1" = [nan, 1.5, nan]
|
||||
"FORD_MUSTANG_MACH_E_MK1" = [nan, 1.5, nan]
|
||||
"FORD_RANGER_MK2" = [nan, 1.5, nan]
|
||||
"FORD_BRONCO_SPORT_MK1" = [nan, 1.5, nan, nan, nan]
|
||||
"FORD_ESCAPE_MK4" = [nan, 1.5, nan, nan, nan]
|
||||
"FORD_EXPLORER_MK6" = [nan, 1.5, nan, nan, nan]
|
||||
"FORD_F_150_MK14" = [nan, 1.5, nan, nan, nan]
|
||||
"FORD_FOCUS_MK4" = [nan, 1.5, nan, nan, nan]
|
||||
"FORD_MAVERICK_MK1" = [nan, 1.5, nan, nan, nan]
|
||||
"FORD_F_150_LIGHTNING_MK1" = [nan, 1.5, nan, nan, nan]
|
||||
"FORD_MUSTANG_MACH_E_MK1" = [nan, 1.5, nan, nan, nan]
|
||||
"FORD_RANGER_MK2" = [nan, 1.5, nan, nan, nan]
|
||||
###
|
||||
|
||||
# No steering wheel
|
||||
"COMMA_BODY" = [nan, 1000, nan]
|
||||
"COMMA_BODY" = [nan, 1000, nan, nan, nan]
|
||||
|
||||
# Totally new cars
|
||||
"RAM_1500_5TH_GEN" = [2.0, 2.0, 0.05]
|
||||
"RAM_HD_5TH_GEN" = [1.4, 1.4, 0.05]
|
||||
"SUBARU_OUTBACK" = [2.0, 1.5, 0.2]
|
||||
"BUICK_BABYENCLAVE" = [1.45, 1.6, 0.2]
|
||||
"CADILLAC_ESCALADE" = [1.899999976158142, 1.842270016670227, 0.1120000034570694]
|
||||
"CADILLAC_ESCALADE_ESV_2019" = [1.15, 1.3, 0.2]
|
||||
"CADILLAC_XT4" = [1.45, 1.6, 0.2]
|
||||
"CHEVROLET_BOLT_EUV" = [2.0, 2.0, 0.05]
|
||||
"CHEVROLET_MALIBU_CC" = [1.85, 1.85, 0.075]
|
||||
"CHEVROLET_SILVERADO" = [1.9, 1.9, 0.112]
|
||||
"CHEVROLET_TRAILBLAZER" = [1.33, 1.9, 0.16]
|
||||
"CHEVROLET_TRAVERSE" = [1.33, 1.33, 0.18]
|
||||
"CHEVROLET_EQUINOX" = [2.5, 2.5, 0.05]
|
||||
"VOLKSWAGEN_CADDY_MK3" = [1.2, 1.2, 0.1]
|
||||
"VOLKSWAGEN_PASSAT_NMS" = [2.5, 2.5, 0.1]
|
||||
"VOLKSWAGEN_SHARAN_MK2" = [2.5, 2.5, 0.1]
|
||||
"HYUNDAI_SANTA_CRUZ_1ST_GEN" = [2.7, 2.7, 0.1]
|
||||
"KIA_SPORTAGE_5TH_GEN" = [2.6, 2.6, 0.1]
|
||||
"GENESIS_GV70_1ST_GEN" = [2.42, 2.42, 0.1]
|
||||
"GENESIS_GV60_EV_1ST_GEN" = [2.5, 2.5, 0.1]
|
||||
"KIA_SORENTO_4TH_GEN" = [2.5, 2.5, 0.1]
|
||||
"KIA_SORENTO_HEV_4TH_GEN" = [2.5, 2.5, 0.1]
|
||||
"KIA_NIRO_HEV_2ND_GEN" = [2.42, 2.5, 0.12]
|
||||
"KIA_NIRO_EV_2ND_GEN" = [2.05, 2.5, 0.14]
|
||||
"GENESIS_GV80" = [2.5, 2.5, 0.1]
|
||||
"KIA_CARNIVAL_4TH_GEN" = [1.75, 1.75, 0.15]
|
||||
"GMC_ACADIA" = [1.6, 1.6, 0.2]
|
||||
"LEXUS_IS_TSS2" = [2.0, 2.0, 0.1]
|
||||
"HYUNDAI_KONA_EV_2ND_GEN" = [2.5, 2.5, 0.1]
|
||||
"HYUNDAI_IONIQ_6" = [2.5, 2.5, 0.005]
|
||||
"HYUNDAI_AZERA_6TH_GEN" = [1.8, 1.8, 0.1]
|
||||
"HYUNDAI_AZERA_HEV_6TH_GEN" = [1.8, 1.8, 0.1]
|
||||
"KIA_K8_HEV_1ST_GEN" = [2.5, 2.5, 0.1]
|
||||
"HYUNDAI_CUSTIN_1ST_GEN" = [2.5, 2.5, 0.1]
|
||||
"LEXUS_GS_F" = [2.5, 2.5, 0.08]
|
||||
"HYUNDAI_STARIA_4TH_GEN" = [1.8, 2.0, 0.15]
|
||||
"RAM_1500_5TH_GEN" = [2.0, 2.0, 0.05, nan, nan]
|
||||
"RAM_HD_5TH_GEN" = [1.4, 1.4, 0.05, nan, nan]
|
||||
"SUBARU_OUTBACK" = [2.0, 1.5, 0.2, nan, nan]
|
||||
"CADILLAC_ESCALADE" = [1.899999976158142, 1.842270016670227, 0.1120000034570694, nan, nan]
|
||||
"CADILLAC_ESCALADE_ESV_2019" = [1.15, 1.3, 0.2, nan, nan]
|
||||
"CADILLAC_XT4" = [1.45, 1.6, 0.2, nan, nan]
|
||||
"CHEVROLET_BOLT_EUV" = [5.208971215698198, 2.0, 0.175, 2.6531724862969748, 1.0]
|
||||
"CHEVROLET_SILVERADO" = [3.91062562345149, 1.9, 0.112, 3.29974374, 1.0]
|
||||
"CHEVROLET_TRAILBLAZER" = [1.33, 1.9, 0.16, nan, nan]
|
||||
"CHEVROLET_TRAVERSE" = [1.33, 1.33, 0.18, nan, nan]
|
||||
"CHEVROLET_EQUINOX" = [2.5, 2.5, 0.05, nan, nan]
|
||||
"VOLKSWAGEN_CADDY_MK3" = [1.2, 1.2, 0.1, nan, nan]
|
||||
"VOLKSWAGEN_PASSAT_NMS" = [2.5, 2.5, 0.1, nan, nan]
|
||||
"VOLKSWAGEN_SHARAN_MK2" = [2.5, 2.5, 0.1, nan, nan]
|
||||
"HYUNDAI_SANTA_CRUZ_1ST_GEN" = [2.7, 2.7, 0.1, nan, nan]
|
||||
"KIA_SPORTAGE_5TH_GEN" = [2.6, 2.6, 0.1, nan, nan]
|
||||
"GENESIS_GV70_1ST_GEN" = [2.42, 2.42, 0.1, nan, nan]
|
||||
"GENESIS_GV60_EV_1ST_GEN" = [2.5, 2.5, 0.1, nan, nan]
|
||||
"KIA_SORENTO_4TH_GEN" = [2.5, 2.5, 0.1, nan, nan]
|
||||
"KIA_SORENTO_HEV_4TH_GEN" = [2.5, 2.5, 0.1, nan, nan]
|
||||
"KIA_NIRO_HEV_2ND_GEN" = [2.42, 2.5, 0.12, nan, nan]
|
||||
"KIA_NIRO_EV_2ND_GEN" = [2.05, 2.5, 0.14, nan, nan]
|
||||
"GENESIS_GV80" = [2.5, 2.5, 0.1, nan, nan]
|
||||
"KIA_CARNIVAL_4TH_GEN" = [1.75, 1.75, 0.15, nan, nan]
|
||||
"GMC_ACADIA" = [3.203074951953876, 1.6, 0.2, 4.78003305, 1.0]
|
||||
"LEXUS_IS_TSS2" = [2.0, 2.0, 0.1, nan, nan]
|
||||
"HYUNDAI_KONA_EV_2ND_GEN" = [2.5, 2.5, 0.1, nan, nan]
|
||||
"HYUNDAI_IONIQ_6" = [2.5, 2.5, 0.005, nan, nan]
|
||||
"HYUNDAI_AZERA_6TH_GEN" = [1.8, 1.8, 0.1, nan, nan]
|
||||
"HYUNDAI_AZERA_HEV_6TH_GEN" = [1.8, 1.8, 0.1, nan, nan]
|
||||
"KIA_K8_HEV_1ST_GEN" = [2.5, 2.5, 0.1, nan, nan]
|
||||
"HYUNDAI_CUSTIN_1ST_GEN" = [2.5, 2.5, 0.1, nan, nan]
|
||||
"LEXUS_GS_F" = [2.5, 2.5, 0.08, nan, nan]
|
||||
"HYUNDAI_STARIA_4TH_GEN" = [1.8, 2.0, 0.15, nan, nan]
|
||||
|
||||
# Dashcam or fallback configured as ideal car
|
||||
"MOCK" = [10.0, 10, 0.0]
|
||||
"MOCK" = [10.0, 10, 0.0, nan, nan]
|
||||
|
||||
# Manually checked
|
||||
"HONDA_CIVIC_2022" = [2.5, 1.2, 0.15]
|
||||
"HONDA_HRV_3G" = [2.5, 1.2, 0.2]
|
||||
|
||||
"MAZDA_3_2019" = [1.45, 3.0, 0.33]
|
||||
"MAZDA_CX_30" = [1.45, 3.0, 0.33]
|
||||
"MAZDA_CX_50" = [1.45, 3.0, 0.33]
|
||||
"HONDA_CIVIC_2022" = [2.5, 1.2, 0.15, nan, nan]
|
||||
"HONDA_HRV_3G" = [2.5, 1.2, 0.2, nan, nan]
|
||||
|
||||
@@ -1,84 +1,82 @@
|
||||
legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
|
||||
"ACURA_ILX" = [1.524988973896102, 0.519011053086259, 0.34236219253028]
|
||||
"ACURA_RDX" = [0.9987728568686902, 0.5323765166196301, 0.303218805715844]
|
||||
"ACURA_RDX_3G" = [1.4314459806646749, 0.33874701282109954, 0.18048847083897598]
|
||||
"AUDI_A3_MK3" = [1.5122414863077502, 1.7443517531719404, 0.15194151892450905]
|
||||
"AUDI_Q3_MK2" = [1.4439223359448605, 1.2254955789112076, 0.1413798895978097]
|
||||
"CHEVROLET_VOLT" = [1.5961527626411784, 1.8422651988094612, 0.1572393918005158]
|
||||
"CHRYSLER_PACIFICA_2018" = [2.07140, 1.3366521181047952, 0.13776367250652022]
|
||||
"CHRYSLER_PACIFICA_2020" = [1.86206, 1.509076559398423, 0.14328246159386085]
|
||||
"CHRYSLER_PACIFICA_2017_HYBRID" = [1.79422, 1.06831764583744, 0.116237]
|
||||
"CHRYSLER_PACIFICA_2018_HYBRID" = [2.08887, 1.2943025830995154, 0.114818]
|
||||
"CHRYSLER_PACIFICA_2019_HYBRID" = [1.90120, 1.1958788168371808, 0.131520]
|
||||
"GENESIS_G70" = [3.8520195946707947, 2.354697063349854, 0.06830285485626221]
|
||||
"HONDA_ACCORD" = [1.6893333799149202, 0.3246749081720698, 0.2120497022936265]
|
||||
"HONDA_CIVIC_BOSCH" = [1.691708637466905, 0.40132900729454185, 0.25460295304024094]
|
||||
"HONDA_CIVIC" = [1.6528895627785531, 0.4018518740819229, 0.25458812851328544]
|
||||
"HONDA_CLARITY" = [1.6528895627785531, 0.4018518740819229, 0.25458812851328544]
|
||||
"HONDA_CRV" = [0.7667141440182675, 0.5927571534745969, 0.40909087636157127]
|
||||
"HONDA_CRV_5G" = [2.01323205142022, 0.2700612209345081, 0.2238412881331528]
|
||||
"HONDA_CRV_HYBRID" = [2.072034634644233, 0.7152085160516978, 0.20237105008376083]
|
||||
"HONDA_FIT" = [1.5719981427109775, 0.5712761407108976, 0.110773383324281]
|
||||
"HONDA_HRV" = [2.0661212805710205, 0.7521343418694775, 0.17760375789242094]
|
||||
"HONDA_INSIGHT" = [1.5201671214069354, 0.5660229120683284, 0.25808042580281876]
|
||||
"HONDA_ODYSSEY" = [1.8774809275211801, 0.8394431662987996, 0.2096978613792822]
|
||||
"HONDA_PILOT" = [1.7262026201812795, 0.9470005614967523, 0.21351430733218763]
|
||||
"HONDA_RIDGELINE" = [1.4146525028237624, 0.7356572861629564, 0.23307177552211328]
|
||||
"HYUNDAI_ELANTRA_2021" = [3.169, 2.1259108157250735, 0.0819]
|
||||
"HYUNDAI_GENESIS" = [2.7807965280270794, 2.325, 0.0984484465421171]
|
||||
"HYUNDAI_IONIQ_5" = [3.172929, 2.713050, 0.096019]
|
||||
"HYUNDAI_IONIQ_EV_LTD" = [1.7662975472852054, 1.613755614526594, 0.17087579756306276]
|
||||
"HYUNDAI_IONIQ_PHEV" = [3.2928700076638537, 2.1193482926455656, 0.12463700961468778]
|
||||
"HYUNDAI_IONIQ_PHEV_2019" = [2.970807902012267, 1.6312321830002083, 0.1088964990357482]
|
||||
"HYUNDAI_KONA_EV" = [3.078814714619148, 2.307336938253934, 0.12359762054065548]
|
||||
"HYUNDAI_PALISADE" = [2.544642494803999, 1.8721703683337008, 0.1301424599248651]
|
||||
"HYUNDAI_SANTA_FE" = [3.0787027729757632, 2.6173437483495565, 0.1207019341823945]
|
||||
"HYUNDAI_SANTA_FE_HEV_2022" = [3.501877602644835, 2.729064118456137, 0.10384068104538963]
|
||||
"HYUNDAI_SANTA_FE_PHEV_2022" = [1.6953050513611045, 1.5837614296206861, 0.12672855941458458]
|
||||
"HYUNDAI_SONATA_LF" = [2.2200457811703953, 1.2967330275895228, 0.14039920986586393]
|
||||
"HYUNDAI_SONATA" = [2.9638737459977467, 2.1259108157250735, 0.07813665616927593]
|
||||
"HYUNDAI_SONATA_HYBRID" = [2.8990264092395734, 2.061410192222139, 0.0899805488717382]
|
||||
"HYUNDAI_TUCSON_4TH_GEN" = [2.960174, 2.860284, 0.108745]
|
||||
"JEEP_GRAND_CHEROKEE_2019" = [2.30972, 1.289689569171081, 0.117048]
|
||||
"JEEP_GRAND_CHEROKEE" = [2.27116, 1.4057367824262523, 0.11725947414922003]
|
||||
"KIA_EV6" = [3.2, 2.093457, 0.005]
|
||||
"KIA_K5_2021" = [2.405339728085138, 1.460032270828705, 0.11650989850813716]
|
||||
"KIA_NIRO_EV" = [2.9215954981365337, 2.1500583840260044, 0.09236802474810267]
|
||||
"KIA_SORENTO" = [2.464854685101844, 1.5335274218367956, 0.12056170567599558]
|
||||
"KIA_STINGER" = [2.7499043387418967, 1.849652021986449, 0.12048334239559202]
|
||||
"LEXUS_ES_TSS2" = [2.0357564999999997, 1.999082295195227, 0.101533]
|
||||
"LEXUS_NX" = [2.3525924753753613, 1.9731412277641067, 0.15168101064205927]
|
||||
"LEXUS_NX_TSS2" = [2.4331999786982936, 2.1045680431705414, 0.14099899317761067]
|
||||
"LEXUS_RX" = [1.6430539050086406, 1.181960058934143, 0.19768806040843034]
|
||||
"LEXUS_RX_TSS2" = [1.5375561442049257, 1.343166476215164, 0.1931062001527557]
|
||||
"MAZDA_CX9_2021" = [1.7601682915983443, 1.0889677335154337, 0.17713792194297195]
|
||||
"SKODA_SUPERB_MK3" = [1.166437404652981, 1.1686163012668165, 0.12194533036948708]
|
||||
"SUBARU_FORESTER" = [3.6617001649776793, 2.342197172531713, 0.11075960785398745]
|
||||
"SUBARU_IMPREZA" = [1.0670704910352047, 0.8234374840709592, 0.20986563268614938]
|
||||
"SUBARU_IMPREZA_2020" = [2.6068223389108303, 2.134872342760203, 0.15261513193561627]
|
||||
"TOYOTA_AVALON" = [2.5185770183845646, 1.7153346784214922, 0.10603968787111022]
|
||||
"TOYOTA_AVALON_2019" = [1.7036141952825095, 1.239619084240008, 0.08459830394899492]
|
||||
"TOYOTA_AVALON_TSS2" = [2.3154403649717357, 2.7777922854327124, 0.11453999639164605]
|
||||
"TOYOTA_CHR" = [1.5591084333664578, 1.271271459066948, 0.20259087058453193]
|
||||
"TOYOTA_CHR_TSS2" = [1.7678810166088303, 1.3742176337919942, 0.2319674583741509]
|
||||
"TOYOTA_CAMRY" = [2.0568162685952505, 1.7576185169559122, 0.108878753]
|
||||
"TOYOTA_CAMRY_TSS2" = [2.3548324999999997, 2.368900128946771, 0.118436]
|
||||
"TOYOTA_COROLLA" = [3.117154369115421, 1.8438132575043773, 0.12289685869250652]
|
||||
"TOYOTA_COROLLA_TSS2" = [1.991132339206426, 1.868866242720403, 0.19570063298031432]
|
||||
"TOYOTA_HIGHLANDER" = [1.8108348718624456, 1.6348421600679828, 0.15972686105120398]
|
||||
"TOYOTA_HIGHLANDER_TSS2" = [1.9617570834136164, 1.8611643317268927, 0.14519673256119725]
|
||||
"TOYOTA_MIRAI" = [2.506899832157829, 1.7417213930750164, 0.20182618449440565]
|
||||
"TOYOTA_PRIUS" = [1.60, 1.5023147650693636, 0.151515]
|
||||
"TOYOTA_PRIUS_TSS2" = [1.972600, 1.9104337425537743, 0.170968]
|
||||
"TOYOTA_RAV4" = [2.085695074355425, 2.2142832316984733, 0.13339165270103975]
|
||||
"TOYOTA_RAV4_TSS2" = [2.279239424615458, 2.087101966779332, 0.13682208413446817]
|
||||
"TOYOTA_RAV4H" = [1.9796257271652042, 1.7503987331707576, 0.14628860048885406]
|
||||
"TOYOTA_RAV4_TSS2_2022" = [2.241883248393209, 1.9304407208090029, 0.112174]
|
||||
"TOYOTA_SIENNA" = [1.689726, 1.3208264576110418, 0.140456]
|
||||
"TOYOTA_YARIS" = [2.22984, 1.86145, 0.168189]
|
||||
"VOLKSWAGEN_ARTEON_MK1" = [1.45136518053819, 1.3639364049316804, 0.23806361745695032]
|
||||
"VOLKSWAGEN_ATLAS_MK1" = [1.4677006726964945, 1.6733266634075656, 0.12959584092073367]
|
||||
"VOLKSWAGEN_GOLF_MK7" = [1.3750394140491293, 1.5814743077200641, 0.2018321939386586]
|
||||
"VOLKSWAGEN_JETTA_MK7" = [1.2271623034089392, 1.216955117387, 0.19437384688370712]
|
||||
"VOLKSWAGEN_PASSAT_MK8" = [1.3432120736752917, 1.7087275587362314, 0.19444383787326647]
|
||||
"VOLKSWAGEN_TIGUAN_MK2" = [0.9711965500094828, 1.0001565939459098, 0.1465626137072916]
|
||||
legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION", "SIGMOID_SHARPNESS", "SIGMOID_TORQUE_GAIN"]
|
||||
"ACURA_ILX" = [1.524988973896102, 0.519011053086259, 0.34236219253028, nan, nan]
|
||||
"ACURA_RDX" = [0.9987728568686902, 0.5323765166196301, 0.303218805715844, nan, nan]
|
||||
"ACURA_RDX_3G" = [1.4314459806646749, 0.33874701282109954, 0.18048847083897598, nan, nan]
|
||||
"AUDI_A3_MK3" = [1.5122414863077502, 1.7443517531719404, 0.15194151892450905, nan, nan]
|
||||
"AUDI_Q3_MK2" = [1.4439223359448605, 1.2254955789112076, 0.1413798895978097, nan, nan]
|
||||
"CHEVROLET_VOLT" = [1.5961527626411784, 1.8422651988094612, 0.1572393918005158, nan, nan]
|
||||
"CHRYSLER_PACIFICA_2018" = [2.07140, 1.3366521181047952, 0.13776367250652022, nan, nan]
|
||||
"CHRYSLER_PACIFICA_2020" = [1.86206, 1.509076559398423, 0.14328246159386085, nan, nan]
|
||||
"CHRYSLER_PACIFICA_2018_HYBRID" = [2.08887, 1.2943025830995154, 0.114818, nan, nan]
|
||||
"CHRYSLER_PACIFICA_2019_HYBRID" = [1.90120, 1.1958788168371808, 0.131520, nan, nan]
|
||||
"GENESIS_G70" = [3.8520195946707947, 2.354697063349854, 0.06830285485626221, nan, nan]
|
||||
"HONDA_ACCORD" = [1.6893333799149202, 0.3246749081720698, 0.2120497022936265, nan, nan]
|
||||
"HONDA_CIVIC_BOSCH" = [1.691708637466905, 0.40132900729454185, 0.25460295304024094, nan, nan]
|
||||
"HONDA_CIVIC" = [1.6528895627785531, 0.4018518740819229, 0.25458812851328544, nan, nan]
|
||||
"HONDA_CRV" = [0.7667141440182675, 0.5927571534745969, 0.40909087636157127, nan, nan]
|
||||
"HONDA_CRV_5G" = [2.01323205142022, 0.2700612209345081, 0.2238412881331528, nan, nan]
|
||||
"HONDA_CRV_HYBRID" = [2.072034634644233, 0.7152085160516978, 0.20237105008376083, nan, nan]
|
||||
"HONDA_FIT" = [1.5719981427109775, 0.5712761407108976, 0.110773383324281, nan, nan]
|
||||
"HONDA_HRV" = [2.0661212805710205, 0.7521343418694775, 0.17760375789242094, nan, nan]
|
||||
"HONDA_INSIGHT" = [1.5201671214069354, 0.5660229120683284, 0.25808042580281876, nan, nan]
|
||||
"HONDA_ODYSSEY" = [1.8774809275211801, 0.8394431662987996, 0.2096978613792822, nan, nan]
|
||||
"HONDA_PILOT" = [1.7262026201812795, 0.9470005614967523, 0.21351430733218763, nan, nan]
|
||||
"HONDA_RIDGELINE" = [1.4146525028237624, 0.7356572861629564, 0.23307177552211328, nan, nan]
|
||||
"HYUNDAI_ELANTRA_2021" = [3.169, 2.1259108157250735, 0.0819, nan, nan]
|
||||
"HYUNDAI_GENESIS" = [2.7807965280270794, 2.325, 0.0984484465421171, nan, nan]
|
||||
"HYUNDAI_IONIQ_5" = [3.172929, 2.713050, 0.096019, nan, nan]
|
||||
"HYUNDAI_IONIQ_EV_LTD" = [1.7662975472852054, 1.613755614526594, 0.17087579756306276, nan, nan]
|
||||
"HYUNDAI_IONIQ_PHEV" = [3.2928700076638537, 2.1193482926455656, 0.12463700961468778, nan, nan]
|
||||
"HYUNDAI_IONIQ_PHEV_2019" = [2.970807902012267, 1.6312321830002083, 0.1088964990357482, nan, nan]
|
||||
"HYUNDAI_KONA_EV" = [3.078814714619148, 2.307336938253934, 0.12359762054065548, nan, nan]
|
||||
"HYUNDAI_PALISADE" = [2.544642494803999, 1.8721703683337008, 0.1301424599248651, nan, nan]
|
||||
"HYUNDAI_SANTA_FE" = [3.0787027729757632, 2.6173437483495565, 0.1207019341823945, nan, nan]
|
||||
"HYUNDAI_SANTA_FE_HEV_2022" = [3.501877602644835, 2.729064118456137, 0.10384068104538963, nan, nan]
|
||||
"HYUNDAI_SANTA_FE_PHEV_2022" = [1.6953050513611045, 1.5837614296206861, 0.12672855941458458, nan, nan]
|
||||
"HYUNDAI_SONATA_LF" = [2.2200457811703953, 1.2967330275895228, 0.14039920986586393, nan, nan]
|
||||
"HYUNDAI_SONATA" = [2.9638737459977467, 2.1259108157250735, 0.07813665616927593, nan, nan]
|
||||
"HYUNDAI_SONATA_HYBRID" = [2.8990264092395734, 2.061410192222139, 0.0899805488717382, nan, nan]
|
||||
"HYUNDAI_TUCSON_4TH_GEN" = [2.960174, 2.860284, 0.108745, nan, nan]
|
||||
"JEEP_GRAND_CHEROKEE_2019" = [2.30972, 1.289689569171081, 0.117048, nan, nan]
|
||||
"JEEP_GRAND_CHEROKEE" = [2.27116, 1.4057367824262523, 0.11725947414922003, nan, nan]
|
||||
"KIA_EV6" = [3.2, 2.093457, 0.005, nan, nan]
|
||||
"KIA_K5_2021" = [2.405339728085138, 1.460032270828705, 0.11650989850813716, nan, nan]
|
||||
"KIA_NIRO_EV" = [2.9215954981365337, 2.1500583840260044, 0.09236802474810267, nan, nan]
|
||||
"KIA_SORENTO" = [2.464854685101844, 1.5335274218367956, 0.12056170567599558, nan, nan]
|
||||
"KIA_STINGER" = [2.7499043387418967, 1.849652021986449, 0.12048334239559202, nan, nan]
|
||||
"LEXUS_ES_TSS2" = [2.0357564999999997, 1.999082295195227, 0.101533, nan, nan]
|
||||
"LEXUS_NX" = [2.3525924753753613, 1.9731412277641067, 0.15168101064205927, nan, nan]
|
||||
"LEXUS_NX_TSS2" = [2.4331999786982936, 2.1045680431705414, 0.14099899317761067, nan, nan]
|
||||
"LEXUS_RX" = [1.6430539050086406, 1.181960058934143, 0.19768806040843034, nan, nan]
|
||||
"LEXUS_RX_TSS2" = [1.5375561442049257, 1.343166476215164, 0.1931062001527557, nan, nan]
|
||||
"MAZDA_CX9_2021" = [1.7601682915983443, 1.0889677335154337, 0.17713792194297195, nan, nan]
|
||||
"SKODA_SUPERB_MK3" = [1.166437404652981, 1.1686163012668165, 0.12194533036948708, nan, nan]
|
||||
"SUBARU_FORESTER" = [3.6617001649776793, 2.342197172531713, 0.11075960785398745, nan, nan]
|
||||
"SUBARU_IMPREZA" = [1.0670704910352047, 0.8234374840709592, 0.20986563268614938, nan, nan]
|
||||
"SUBARU_IMPREZA_2020" = [2.6068223389108303, 2.134872342760203, 0.15261513193561627, nan, nan]
|
||||
"TOYOTA_AVALON" = [2.5185770183845646, 1.7153346784214922, 0.10603968787111022, nan, nan]
|
||||
"TOYOTA_AVALON_2019" = [1.7036141952825095, 1.239619084240008, 0.08459830394899492, nan, nan]
|
||||
"TOYOTA_AVALON_TSS2" = [2.3154403649717357, 2.7777922854327124, 0.11453999639164605, nan, nan]
|
||||
"TOYOTA_CHR" = [1.5591084333664578, 1.271271459066948, 0.20259087058453193, nan, nan]
|
||||
"TOYOTA_CHR_TSS2" = [1.7678810166088303, 1.3742176337919942, 0.2319674583741509, nan, nan]
|
||||
"TOYOTA_CAMRY" = [2.0568162685952505, 1.7576185169559122, 0.108878753, nan, nan]
|
||||
"TOYOTA_CAMRY_TSS2" = [2.3548324999999997, 2.368900128946771, 0.118436, nan, nan]
|
||||
"TOYOTA_COROLLA" = [3.117154369115421, 1.8438132575043773, 0.12289685869250652, nan, nan]
|
||||
"TOYOTA_COROLLA_TSS2" = [1.991132339206426, 1.868866242720403, 0.19570063298031432, nan, nan]
|
||||
"TOYOTA_HIGHLANDER" = [1.8108348718624456, 1.6348421600679828, 0.15972686105120398, nan, nan]
|
||||
"TOYOTA_HIGHLANDER_TSS2" = [1.9617570834136164, 1.8611643317268927, 0.14519673256119725, nan, nan]
|
||||
"TOYOTA_MIRAI" = [2.506899832157829, 1.7417213930750164, 0.20182618449440565, nan, nan]
|
||||
"TOYOTA_PRIUS" = [1.60, 1.5023147650693636, 0.151515, nan, nan]
|
||||
"TOYOTA_PRIUS_TSS2" = [1.972600, 1.9104337425537743, 0.170968, nan, nan]
|
||||
"TOYOTA_RAV4" = [2.085695074355425, 2.2142832316984733, 0.13339165270103975, nan, nan]
|
||||
"TOYOTA_RAV4_TSS2" = [2.279239424615458, 2.087101966779332, 0.13682208413446817, nan, nan]
|
||||
"TOYOTA_RAV4H" = [1.9796257271652042, 1.7503987331707576, 0.14628860048885406, nan, nan]
|
||||
"TOYOTA_RAV4_TSS2_2022" = [2.241883248393209, 1.9304407208090029, 0.112174, nan, nan]
|
||||
"TOYOTA_SIENNA" = [1.689726, 1.3208264576110418, 0.140456, nan, nan]
|
||||
"TOYOTA_YARIS" = [2.22984, 1.86145, 0.168189, nan, nan]
|
||||
"VOLKSWAGEN_ARTEON_MK1" = [1.45136518053819, 1.3639364049316804, 0.23806361745695032, nan, nan]
|
||||
"VOLKSWAGEN_ATLAS_MK1" = [1.4677006726964945, 1.6733266634075656, 0.12959584092073367, nan, nan]
|
||||
"VOLKSWAGEN_GOLF_MK7" = [1.3750394140491293, 1.5814743077200641, 0.2018321939386586, nan, nan]
|
||||
"VOLKSWAGEN_JETTA_MK7" = [1.2271623034089392, 1.216955117387, 0.19437384688370712, nan, nan]
|
||||
"VOLKSWAGEN_PASSAT_MK8" = [1.3432120736752917, 1.7087275587362314, 0.19444383787326647, nan, nan]
|
||||
"VOLKSWAGEN_TIGUAN_MK2" = [0.9711965500094828, 1.0001565939459098, 0.1465626137072916, nan, nan]
|
||||
+318
-31
@@ -28,18 +28,59 @@ FRICTION_SANITY_QLOG = 0.8
|
||||
STEER_MIN_THRESHOLD = 0.02
|
||||
MIN_FILTER_DECAY = 50
|
||||
MAX_FILTER_DECAY = 250
|
||||
LAT_ACC_THRESHOLD = 1
|
||||
STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1), (0.1, 0.2), (0.2, 0.3), (0.3, 0.5)]
|
||||
MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100])
|
||||
LAT_ACC_THRESHOLD = 4 # m/s^2 maximum lateral acceleration allowed
|
||||
LOOKBACK = 0.5 # secs for sensor standard deviation calculation
|
||||
|
||||
STEER_BUCKET_BOUNDS = [
|
||||
(-1.0, -0.9), (-0.9, -0.8), (-0.8, -0.7), (-0.7, -0.6), (-0.6, -0.5),
|
||||
(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1),
|
||||
(0.1, 0.2), (0.2, 0.3), (0.3, 0.5), (0.5, 0.6), (0.6, 0.7),
|
||||
(0.7, 0.8), (0.8, 0.9), (0.9, 1.0)
|
||||
]
|
||||
|
||||
MIN_BUCKET_POINTS = np.array([
|
||||
100, 100, 100, 100, 100,
|
||||
100, 300, 500, 500, 500,
|
||||
500, 300, 100, 100, 100,
|
||||
100, 100, 100,
|
||||
])
|
||||
MIN_ENGAGE_BUFFER = 2 # secs
|
||||
|
||||
VERSION = 1 # bump this to invalidate old parameter caches
|
||||
ALLOWED_CARS = ['toyota', 'hyundai']
|
||||
VERSION = 2 # bump this to invalidate old parameter caches
|
||||
ALLOWED_BRANDS = ['toyota', 'hyundai']
|
||||
ALLOWED_CARS = ['MAZDA_3_2019']
|
||||
|
||||
|
||||
def sig_centered(z):
|
||||
pos = 1.0 / (1.0 + np.exp(-z)) - 0.5
|
||||
neg = np.exp(z) / (1.0 + np.exp(z)) - 0.5
|
||||
return np.where(z >= 0.0, pos, neg) # branch-free vectorised
|
||||
|
||||
|
||||
def model(x, a, b, c, d):
|
||||
xs = x - d
|
||||
return sig_centered(a * xs) * b + c * xs
|
||||
|
||||
|
||||
def jacobian(x, a, b, c, d):
|
||||
xs = x - d
|
||||
# plain σ for derivative (cheaper than calling centred helper again)
|
||||
s = 1.0 / (1.0 + np.exp(-np.clip(a * xs, -50.0, 50.0)))
|
||||
ds = s * (1.0 - s) # σ′(z)
|
||||
sc = s - 0.5 # (σ − 0.5) value
|
||||
|
||||
# Cols: ∂f/∂a, ∂f/∂b, ∂f/∂c, ∂f/∂d (N × 4)
|
||||
return np.column_stack([
|
||||
b * ds * xs, # a-derivative
|
||||
sc, # b-derivative
|
||||
xs, # c-derivative
|
||||
-b * a * ds - c # d-derivative
|
||||
])
|
||||
|
||||
|
||||
def slope2rot(slope):
|
||||
sin = np.sqrt(slope**2 / (slope**2 + 1))
|
||||
cos = np.sqrt(1 / (slope**2 + 1))
|
||||
sin = np.sqrt(slope ** 2 / (slope ** 2 + 1))
|
||||
cos = np.sqrt(1 / (slope ** 2 + 1))
|
||||
return np.array([[cos, -sin], [sin, cos]])
|
||||
|
||||
|
||||
@@ -52,9 +93,10 @@ class TorqueBuckets(PointBuckets):
|
||||
|
||||
|
||||
class TorqueEstimator(ParameterEstimator):
|
||||
def __init__(self, CP, decimated=False):
|
||||
def __init__(self, CP, decimated=False, track_all_points=False):
|
||||
self.hist_len = int(HISTORY / DT_MDL)
|
||||
self.lag = 0.0
|
||||
self.track_all_points = track_all_points # for offline analysis, without max lateral accel or max steer torque filters
|
||||
if decimated:
|
||||
self.min_bucket_points = MIN_BUCKET_POINTS / 10
|
||||
self.min_points_total = MIN_POINTS_TOTAL_QLOG
|
||||
@@ -71,12 +113,30 @@ class TorqueEstimator(ParameterEstimator):
|
||||
|
||||
self.offline_friction = 0.0
|
||||
self.offline_latAccelFactor = 0.0
|
||||
self.offline_sigmoidSharpness = 0.0
|
||||
self.offline_sigmoidTorqueGain = 0.0
|
||||
|
||||
self.resets = 0.0
|
||||
self.use_params = CP.carName in ALLOWED_CARS and CP.lateralTuning.which() == 'torque'
|
||||
self.use_params = CP.brand in ALLOWED_BRANDS and CP.lateralTuning.which() == 'torque'
|
||||
self.use_params |= CP.carFingerprint in ALLOWED_CARS
|
||||
|
||||
if CP.lateralTuning.which() == 'torque':
|
||||
self.offline_friction = CP.lateralTuning.torque.friction
|
||||
self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor
|
||||
self.offline_sigmoidSharpness = CP.lateralTuning.torque.sigmoidSharpness
|
||||
self.offline_sigmoidTorqueGain = CP.lateralTuning.torque.sigmoidTorqueGain
|
||||
|
||||
# # override params for offline analysis
|
||||
# self.offline_sigmoidSharpness = 3.8
|
||||
# self.offline_sigmoidTorqueGain = 1.0
|
||||
# self.offline_latAccelFactor = 0.1
|
||||
# self.offline_friction = 0.33
|
||||
|
||||
|
||||
|
||||
cloudlog.info(f"using params: {self.use_params=}, {CP.lateralTuning.which()=}, {CP.carFingerprint=}")
|
||||
cloudlog.info(f"offline params: {self.offline_sigmoidSharpness=}, {self.offline_sigmoidTorqueGain=}, {self.offline_latAccelFactor=}, {self.offline_friction=}")
|
||||
|
||||
|
||||
self.reset()
|
||||
|
||||
@@ -84,11 +144,20 @@ class TorqueEstimator(ParameterEstimator):
|
||||
'latAccelFactor': self.offline_latAccelFactor,
|
||||
'latAccelOffset': 0.0,
|
||||
'frictionCoefficient': self.offline_friction,
|
||||
'sigmoidSharpness': self.offline_sigmoidSharpness,
|
||||
'sigmoidTorqueGain': self.offline_sigmoidTorqueGain,
|
||||
'points': []
|
||||
}
|
||||
# if any of the initial params are NaN, set them to 0.0 but skip "points"
|
||||
initial_params = {k: (0.0 if np.isnan(v) else v) for k, v in initial_params.items() if k != 'points'}
|
||||
|
||||
self.decay = MIN_FILTER_DECAY
|
||||
self.min_lataccel_factor = (1.0 - self.factor_sanity) * self.offline_latAccelFactor
|
||||
self.max_lataccel_factor = (1.0 + self.factor_sanity) * self.offline_latAccelFactor
|
||||
self.min_sigmoid_sharpness = (1.0 - self.factor_sanity) * self.offline_sigmoidSharpness
|
||||
self.max_sigmoid_sharpness = (1.0 + self.factor_sanity) * self.offline_sigmoidSharpness
|
||||
self.min_sigmoid_torque_gain = (1.0 - self.factor_sanity) * self.offline_sigmoidTorqueGain
|
||||
self.max_sigmoid_torque_gain = (1.0 + self.factor_sanity) * self.offline_sigmoidTorqueGain
|
||||
self.min_friction = (1.0 - self.friction_sanity) * self.offline_friction
|
||||
self.max_friction = (1.0 + self.friction_sanity) * self.offline_friction
|
||||
|
||||
@@ -107,7 +176,9 @@ class TorqueEstimator(ParameterEstimator):
|
||||
initial_params = {
|
||||
'latAccelFactor': cache_ltp.latAccelFactorFiltered,
|
||||
'latAccelOffset': cache_ltp.latAccelOffsetFiltered,
|
||||
'frictionCoefficient': cache_ltp.frictionCoefficientFiltered
|
||||
'frictionCoefficient': cache_ltp.frictionCoefficientFiltered,
|
||||
'sigmoidSharpness': cache_ltp.sigmoidSharpnessFiltered,
|
||||
'sigmoidTorqueGain': cache_ltp.sigmoidTorqueGainFiltered,
|
||||
}
|
||||
initial_params['points'] = cache_ltp.points
|
||||
self.decay = cache_ltp.decay
|
||||
@@ -116,17 +187,26 @@ class TorqueEstimator(ParameterEstimator):
|
||||
except Exception:
|
||||
cloudlog.exception("failed to restore cached torque params")
|
||||
params.remove("LiveTorqueParameters")
|
||||
self.pre_load_points(initial_params)
|
||||
self.estimate_params()
|
||||
|
||||
else:
|
||||
self.pre_load_points(initial_params)
|
||||
self.estimate_params()
|
||||
|
||||
self.filtered_params = {}
|
||||
for param in initial_params:
|
||||
self.filtered_params[param] = FirstOrderFilter(initial_params[param], self.decay, DT_MDL)
|
||||
|
||||
def get_restore_key(self, CP, version):
|
||||
a, b = None, None
|
||||
def get_restore_key(CP, version):
|
||||
a, b , c, d = None, None , None, None
|
||||
if CP.lateralTuning.which() == 'torque':
|
||||
a = CP.lateralTuning.torque.friction
|
||||
b = CP.lateralTuning.torque.latAccelFactor
|
||||
return (CP.carFingerprint, CP.lateralTuning.which(), a, b, version)
|
||||
a = CP.lateralTuning.torque.sigmoidSharpness
|
||||
b = CP.lateralTuning.torque.sigmoidTorqueGain
|
||||
c = CP.lateralTuning.torque.friction
|
||||
d = CP.lateralTuning.torque.latAccelFactor
|
||||
|
||||
return (CP.carFingerprint, CP.lateralTuning.which(), a, b, c, d, version)
|
||||
|
||||
def reset(self):
|
||||
self.resets += 1.0
|
||||
@@ -137,20 +217,103 @@ class TorqueEstimator(ParameterEstimator):
|
||||
min_points_total=self.min_points_total,
|
||||
points_per_bucket=POINTS_PER_BUCKET,
|
||||
rowsize=3)
|
||||
self.all_torque_points = []
|
||||
|
||||
def estimate_params(self):
|
||||
points = self.filtered_points.get_points(self.fit_points)
|
||||
# total least square solution as both x and y are noisy observations
|
||||
# this is empirically the slope of the hysteresis parallelogram as opposed to the line through the diagonals
|
||||
def estimate_params(self) -> tuple:
|
||||
"""
|
||||
Fit the 4-parameter steering-torque curve and extract
|
||||
a single static-friction amplitude (sigma_f).
|
||||
|
||||
Returns (a, b, c, d, sigma_f) or (np.nan, …) on failure.
|
||||
"""
|
||||
# ── 1. gather data ──────────────────────────────────────────
|
||||
pts = self.filtered_points.get_points(self.fit_points)
|
||||
if pts.size == 0:
|
||||
cloudlog.info("No points to fit.")
|
||||
return (np.nan,)*5
|
||||
# ── 2 linear fit for friction estimate ───────────────────
|
||||
try:
|
||||
_, _, v = np.linalg.svd(points, full_matrices=False)
|
||||
slope, offset = -v.T[0:2, 2] / v.T[2, 2]
|
||||
_, spread = np.matmul(points[:, [0, 2]], slope2rot(slope)).T
|
||||
_, _, v = np.linalg.svd(pts, full_matrices=False)
|
||||
slope, _ = -v.T[0:2, 2] / v.T[2, 2]
|
||||
_, spread = np.matmul(pts[:, [0, 2]], slope2rot(slope)).T
|
||||
friction_coeff = np.std(spread) * FRICTION_FACTOR
|
||||
except np.linalg.LinAlgError as e:
|
||||
cloudlog.exception(f"Error computing live torque params: {e}")
|
||||
slope = offset = friction_coeff = np.nan
|
||||
return slope, offset, friction_coeff
|
||||
friction_coeff = np.nan
|
||||
|
||||
x = pts[:, 2].astype(float) # lateral acceleration
|
||||
y = pts[:, 0].astype(float) # steering torque
|
||||
|
||||
# ── 3. Gauss-Newton / LM fit for (a,b,c,d) ─────────────────
|
||||
b0 = np.clip(np.ptp(y), 0.1, 2.0)
|
||||
params = np.array([3.0, b0, 0.0, 0.0]) # [a,b,c,d]
|
||||
lam, tol, it_max = 1e-3, 1e-5, 20 # λ lambda, tolerance, max iters
|
||||
|
||||
for it in range(it_max):
|
||||
a, b, c, d = params
|
||||
r = model(x, a, b, c, d) - y
|
||||
J = jacobian(x, a, b, c, d)
|
||||
H = J.T @ J
|
||||
g = J.T @ r
|
||||
try:
|
||||
delta = np.linalg.solve(H + lam*np.eye(4), -g)
|
||||
except np.linalg.LinAlgError:
|
||||
cloudlog.warning("GN fit failed to solve for delta")
|
||||
return (np.nan,)*5
|
||||
if not np.all(np.isfinite(delta)):
|
||||
cloudlog.warning("Non-finite GN step – aborting")
|
||||
return (np.nan,)*5
|
||||
|
||||
params_new = params + delta
|
||||
#bounds
|
||||
params_new[0] = np.clip(params_new[0], 0.0, 10.0) # a: sigmoid sharpness
|
||||
params_new[1] = np.clip(params_new[1], 0.0, 2.0) # b: sigmoid torque gain
|
||||
params_new[2] = np.clip(params_new[2], 0.0, 5.0) # c: lat accel factor
|
||||
params_new[3] = np.clip(params_new[3], -.3, 0.3) # d: lat accel offset
|
||||
|
||||
if np.max(np.abs(delta)) < tol:
|
||||
params = params_new
|
||||
break
|
||||
params = params_new
|
||||
|
||||
# if we hit max iters, we don't have a solution
|
||||
if it == it_max - 1:
|
||||
cloudlog.warning("GN fit failed to converge")
|
||||
return (np.nan,)*5
|
||||
|
||||
a, b, c, d = params
|
||||
if not np.all(np.isfinite(params)):
|
||||
cloudlog.warning("Invalid parameters after GN fit")
|
||||
return (np.nan,)*5
|
||||
|
||||
# ── 3. friction estimate from residual envelope ───────────
|
||||
# resid = y - model(x, a, b, c, d)
|
||||
|
||||
# # bin residuals vs x to get local σ(x)
|
||||
# bins = 40
|
||||
# idx = np.argsort(x)
|
||||
# x_sorted, r_sorted = x[idx], resid[idx]
|
||||
# edges = np.linspace(x.min(), x.max(), bins + 1)
|
||||
# centers = 0.5 * (edges[1:] + edges[:-1])
|
||||
# sigmas = np.array([
|
||||
# np.std(r_sorted[(x_sorted >= lo) & (x_sorted < hi)])
|
||||
# for lo, hi in zip(edges[:-1], edges[1:])
|
||||
# ])
|
||||
|
||||
# tail_sigma = sigmas[[0, -1]].mean() # baseline noise
|
||||
# peak_sigma = sigmas[np.argmin(np.abs(centers))] # widest point
|
||||
# sigma_f = max(peak_sigma - tail_sigma, 0.0) * FRICTION_FACTOR
|
||||
|
||||
cloudlog.info(
|
||||
f"GN fit {it+1:02d} iters: "
|
||||
f"a={a:.4f} b={b:.4f} c={c:.4f} d={d:.4f} σ_f={friction_coeff:.4f}"
|
||||
)
|
||||
|
||||
#print(f"GN fit {it+1:02d} iters: a={a:.4f} b={b:.4f} c={c:.4f} d={d:.4f} σ_f={friction_coeff:.4f}")
|
||||
|
||||
self.nonlinear_params = np.array([a, b, c, d])
|
||||
self.friction_coeff = friction_coeff
|
||||
return a, b, c, d, friction_coeff
|
||||
|
||||
def update_params(self, params):
|
||||
self.decay = min(self.decay + DT_MDL, MAX_FILTER_DECAY)
|
||||
@@ -169,6 +332,9 @@ class TorqueEstimator(ParameterEstimator):
|
||||
self.raw_points["carState_t"].append(t + self.lag)
|
||||
self.raw_points["vego"].append(msg.vEgo)
|
||||
self.raw_points["steer_override"].append(msg.steeringPressed)
|
||||
self.raw_points["steer_angle"].append(msg.steeringAngleDeg)
|
||||
elif which == "liveCalibration":
|
||||
self.calibrator.feed_live_calib(msg)
|
||||
elif which == "liveDelay":
|
||||
self.lag = msg.lateralDelay
|
||||
elif which == "liveLocationKalman":
|
||||
@@ -180,7 +346,9 @@ class TorqueEstimator(ParameterEstimator):
|
||||
vego = np.interp(t, self.raw_points['carState_t'], self.raw_points['vego'])
|
||||
steer = np.interp(t, self.raw_points['carOutput_t'], self.raw_points['steer_torque'])
|
||||
lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY)
|
||||
if all(active) and (not any(steer_override)) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD) and (abs(lateral_acc) <= LAT_ACC_THRESHOLD):
|
||||
steering_angle_std = np.std(np.interp(np.arange(t - LOOKBACK, t + self.lag, DT_MDL),
|
||||
self.raw_points['carState_t'], self.raw_points['steer_angle']))
|
||||
if all(active) and (not any(steer_override)) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD) and (abs(lateral_acc) <= LAT_ACC_THRESHOLD) and (steering_angle_std < 1.0):
|
||||
self.filtered_points.add_point(float(steer), float(lateral_acc))
|
||||
|
||||
def get_msg(self, valid=True, with_points=False, frogpilot_toggles=None):
|
||||
@@ -192,13 +360,15 @@ class TorqueEstimator(ParameterEstimator):
|
||||
|
||||
# Calculate raw estimates when possible, only update filters when enough points are gathered
|
||||
if self.filtered_points.is_calculable():
|
||||
latAccelFactor, latAccelOffset, frictionCoeff = self.estimate_params()
|
||||
sigmoidSharpness, sigmoidTorqueGain, latAccelFactor, latAccelOffset, frictionCoeff = self.estimate_params()
|
||||
liveTorqueParameters.latAccelFactorRaw = float(latAccelFactor)
|
||||
liveTorqueParameters.latAccelOffsetRaw = float(latAccelOffset)
|
||||
liveTorqueParameters.frictionCoefficientRaw = float(frictionCoeff)
|
||||
liveTorqueParameters.sigmoidSharpnessRaw = float(sigmoidSharpness)
|
||||
liveTorqueParameters.sigmoidTorqueGainRaw = float(sigmoidTorqueGain)
|
||||
|
||||
if self.filtered_points.is_valid():
|
||||
if any(val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff]):
|
||||
if any(val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff, sigmoidSharpness, sigmoidTorqueGain]):
|
||||
cloudlog.exception("Live torque parameters are invalid.")
|
||||
liveTorqueParameters.liveValid = False
|
||||
self.reset()
|
||||
@@ -206,21 +376,136 @@ class TorqueEstimator(ParameterEstimator):
|
||||
liveTorqueParameters.liveValid = True
|
||||
latAccelFactor = np.clip(latAccelFactor, self.min_lataccel_factor, self.max_lataccel_factor)
|
||||
frictionCoeff = np.clip(frictionCoeff, self.min_friction, self.max_friction)
|
||||
self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': frictionCoeff})
|
||||
self.update_params({'latAccelFactor': latAccelFactor,
|
||||
'latAccelOffset': latAccelOffset,
|
||||
'frictionCoefficient': frictionCoeff,
|
||||
'sigmoidSharpness': sigmoidSharpness,
|
||||
'sigmoidTorqueGain': sigmoidTorqueGain,
|
||||
})
|
||||
|
||||
if with_points:
|
||||
liveTorqueParameters.points = self.filtered_points.get_points()[:, [0, 2]].tolist()
|
||||
|
||||
liveTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x if not frogpilot_toggles.use_custom_lat_accel_factor else frogpilot_toggles.steer_lat_accel_factor)
|
||||
liveTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x)
|
||||
liveTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x)
|
||||
liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x if not frogpilot_toggles.use_custom_steer_friction else frogpilot_toggles.steer_friction)
|
||||
liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x)
|
||||
liveTorqueParameters.sigmoidSharpnessFiltered = float(self.filtered_params['sigmoidSharpness'].x)
|
||||
liveTorqueParameters.sigmoidTorqueGainFiltered = float(self.filtered_params['sigmoidTorqueGain'].x)
|
||||
liveTorqueParameters.totalBucketPoints = len(self.filtered_points)
|
||||
liveTorqueParameters.decay = self.decay
|
||||
liveTorqueParameters.maxResets = self.resets
|
||||
return msg
|
||||
|
||||
def pre_load_points(self, initial_params) -> None:
|
||||
"""
|
||||
Seed the buckets with synthetic points built from the initial tune.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
initial_params : tuple
|
||||
(latAccelFactor, latAccelOffset, frictionCoefficient, sigmoidSharpness, sigmoidTorqueGain)
|
||||
(c,d,f,a,b)
|
||||
"""
|
||||
a = initial_params['sigmoidSharpness']
|
||||
b = initial_params['sigmoidTorqueGain']
|
||||
c = initial_params['latAccelFactor']
|
||||
d = initial_params['latAccelOffset']
|
||||
friction = initial_params['frictionCoefficient']
|
||||
print("Pre-loading points for synthetic data: ", initial_params)
|
||||
cloudlog.info(f"Pre-loading points for synthetic data: {initial_params}")
|
||||
assert d == 0.0, "latAccelOffset must be 0.0 for synthetic data"
|
||||
rng = np.random.default_rng(42)
|
||||
x_sample = rng.uniform(-4, 4, 10000)
|
||||
sigma_base = 0.10
|
||||
lat_accel_jitter = x_sample + rng.normal(0, sigma_base, size=x_sample.shape)
|
||||
envelope = np.exp(-(lat_accel_jitter / 1.0) ** 2)
|
||||
|
||||
steer_jitter = (
|
||||
model(lat_accel_jitter, a, b, c, d)
|
||||
+ rng.normal(0, sigma_base, size=x_sample.shape)
|
||||
+ rng.normal(0, friction * envelope, size=x_sample.shape)
|
||||
)
|
||||
|
||||
for τ, a_lat in zip(steer_jitter, lat_accel_jitter):
|
||||
self.filtered_points.add_point(τ, a_lat)
|
||||
|
||||
def save_filtered_points(self, base_filename="bucket_plot", file_ext=".png"):
|
||||
import matplotlib.pyplot as plt
|
||||
all_points = [] # Collect all bucket points for the combined plot
|
||||
|
||||
# Iterate over each bucket in the filtered_points object
|
||||
for bounds in self.filtered_points.x_bounds:
|
||||
# Get the data for the current bucket. Each bucket is expected to be a list of points.
|
||||
bucket_data = self.filtered_points.buckets.get(bounds, [])
|
||||
|
||||
# Check if the bucket has any data
|
||||
if not bucket_data:
|
||||
print(f"No data points in bucket {bounds}")
|
||||
continue
|
||||
|
||||
# Convert bucket data to a numpy array for processing
|
||||
bucket_points = np.array(bucket_data.arr)
|
||||
if bucket_points.size == 0:
|
||||
print(f"No data points in bucket {bounds}")
|
||||
continue
|
||||
|
||||
# Append these points to all_points for the combined plot
|
||||
all_points.append(bucket_points)
|
||||
|
||||
|
||||
# Create one combined plot if there are any points
|
||||
if all_points:
|
||||
combined = np.concatenate(all_points, axis=0)
|
||||
steer_all = combined[:, 0]
|
||||
lateral_all = combined[:, 2]
|
||||
|
||||
# ── figure ───────────────────────────────────────────────
|
||||
plt.figure(figsize=(16, 4))
|
||||
|
||||
|
||||
# fitted curve + friction band
|
||||
a, b, c, d = self.nonlinear_params # 4-tuple
|
||||
sigma_f = getattr(self, "friction_coeff", 0.0)
|
||||
|
||||
x_line = np.linspace(-4, 4, 400)
|
||||
y_fit = model(x_line, a, b, c, d)
|
||||
|
||||
plt.plot(x_line, y_fit, color="red", lw=2, label="Fitted curve")
|
||||
if sigma_f > 0:
|
||||
plt.plot(x_line, y_fit + sigma_f, color="blue", ls="--", lw=1.5, label="friction band")
|
||||
plt.plot(x_line, y_fit - sigma_f, color="blue", ls="--", lw=1.5, label="")
|
||||
# fill in the area between the two curves
|
||||
plt.fill_between(x_line, y_fit - sigma_f, y_fit + sigma_f, color="grey", alpha=0.3)
|
||||
plt.scatter(lateral_all, steer_all, s=8, alpha=0.4, label="Filtered samples")
|
||||
|
||||
# ── cosmetics ────────────────────────────────────────────
|
||||
plt.xlim(-4, 4)
|
||||
plt.ylim(-1, 1)
|
||||
plt.xlabel("Lateral acceleration (m/s²)")
|
||||
plt.ylabel("Steering torque (Nm equiv)")
|
||||
plt.title("Torque vs lateral acceleration (all buckets)")
|
||||
# print the current parameters
|
||||
plt.text(0.05, 0.9, f"Friction: {self.friction_coeff:.3f}", transform=plt.gca().transAxes)
|
||||
plt.text(0.05, 0.85, f"LatAccelFactor: {self.filtered_params['latAccelFactor'].x:.3f}", transform=plt.gca().transAxes)
|
||||
plt.text(0.05, 0.8, f"SigmoidSharpness: {self.filtered_params['sigmoidSharpness'].x:.3f}", transform=plt.gca().transAxes)
|
||||
plt.text(0.05, 0.75, f"SigmoidTorqueGain: {self.filtered_params['sigmoidTorqueGain'].x:.3f}", transform=plt.gca().transAxes)
|
||||
plt.text(0.05, 0.7, f"LatAccelOffset: {self.filtered_params['latAccelOffset'].x:.3f}", transform=plt.gca().transAxes)
|
||||
plt.text(0.05, 0.65, f"Decay: {self.decay:.3f}", transform=plt.gca().transAxes)
|
||||
plt.text(0.05, 0.6, f"Valid: {self.filtered_points.is_valid()}", transform=plt.gca().transAxes)
|
||||
|
||||
|
||||
plt.grid(True)
|
||||
plt.legend()
|
||||
plt.tight_layout()
|
||||
|
||||
filename_all = f"{base_filename}_all{file_ext}"
|
||||
plt.savefig(filename_all)
|
||||
plt.close()
|
||||
print(f"Combined plot saved as {filename_all}")
|
||||
|
||||
|
||||
def main(demo=False):
|
||||
|
||||
config_realtime_process([0, 1, 2, 3], 5)
|
||||
|
||||
pm = messaging.PubMaster(['liveTorqueParameters'])
|
||||
@@ -246,7 +531,9 @@ def main(demo=False):
|
||||
|
||||
# 4Hz driven by liveLocationKalman
|
||||
if sm.frame % 5 == 0:
|
||||
pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks(), with_points=False, frogpilot_toggles=frogpilot_toggles))
|
||||
pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks()))
|
||||
# if sm.frame % 120 == 0:
|
||||
# estimator.save_filtered_points()
|
||||
|
||||
# Cache points every 60 seconds while onroad
|
||||
if sm.frame % 240 == 0:
|
||||
|
||||
Reference in New Issue
Block a user