diff --git a/cereal/car.capnp b/cereal/car.capnp index 9f3505338..c498aed65 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -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 { diff --git a/cereal/log.capnp b/cereal/log.capnp index c06556e94..c65df9dea 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -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; diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 31f6a1f6c..20aab2714 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -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 diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 6c373316e..d6983f232 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -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 diff --git a/selfdrive/car/torque_data/override.toml b/selfdrive/car/torque_data/override.toml index 53f5ace97..50b2ed9bd 100644 --- a/selfdrive/car/torque_data/override.toml +++ b/selfdrive/car/torque_data/override.toml @@ -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] \ No newline at end of file +"HONDA_CIVIC_2022" = [2.5, 1.2, 0.15, nan, nan] +"HONDA_HRV_3G" = [2.5, 1.2, 0.2, nan, nan] diff --git a/selfdrive/car/torque_data/params.toml b/selfdrive/car/torque_data/params.toml index d2bf536b6..cc7b65757 100644 --- a/selfdrive/car/torque_data/params.toml +++ b/selfdrive/car/torque_data/params.toml @@ -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] \ No newline at end of file diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 3f2812694..b47097564 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -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: