Toyota: don't calculate angle offset when angle rate is high

[sshane]

With this fix, the offset from route ad5a3fa719bc2f83|2023-10-17--20-02-16 below now starts at 0.027 with a test models run.

If we initialize while the steering rate is high, the angle offset will not be accurate:

[adeebshihadeh]

Can you make sure to test this thoroughly? I hit this pretty easily.

[sshane]

Looked at 548,384 segments last 90 days from TSS2 platforms:

Numbers

Error on offset vs. at least +5 segments in future and when route starts above 100 deg/s:

First segments: 883
Before mean, std   : 1.5571950783550692 2.2399094277354172
100 deg/s mean, std: 0.8213051985465408 0.7448367971991677
50 deg/s mean, std : 0.6715002067722017 0.5895577415502311

Times to get an offset when route starts above 100 deg/s:

First segments: 2116
Before mean, std   : 230.71266540642722 582.6594207722336
100 deg/s mean, std: 350.4111531190926 714.3440208348153
50 deg/s mean, std : 391.9347826086956 744.6270482102273

Overall times:

First segments: 5130
Before mean, std   : 169.80565302144248 561.0539985819265
100 deg/s mean, std: 220.19103313840156 635.7524038978505
50 deg/s mean, std : 241.84736842105264 660.8085851857994

---

Sanity checks

This never prints, except two legit cases where it was mostly in park at a high angle and then rate:

  if (new_angle_offset_100ds is None) != (new_angle_offset_50ds is None):
    print('new angle offsets do not match', platform, segment, new_angle_offset_100ds, new_angle_offset_50ds)
    print(time_to_offset_new_100ds, time_to_offset_new_50ds)
    print()

Output

new angle offsets do not match TOYOTA RAV4 2019 f6695ef06f916f23|2023-07-19--21-41-35--0 2.1548 None
5299 5895

new angle offsets do not match TOYOTA RAV4 2019 8aad58f199ab2bed|2023-09-10--20-57-00--0 -1.005100000000013 None
5568 5930

This also only has 7 legit cases:

  if (new_angle_offset_50ds is None) != (initial_angle_offset is None):
    print('new angle offset existence does not match original', platform, segment)
    print(new_angle_offset_100ds, new_angle_offset_50ds, initial_angle_offset)

An example: https://github.com/commaai/openpilot/assets/25857203/6a87903f-70c0-4fba-91de-c533f96bab97

Output

new angle offset existence does not match original TOYOTA RAV4 2019 f6695ef06f916f23|2023-07-19--21-41-35--0
2.1548 None 2.1547999382019043
new angle offset existence does not match original TOYOTA COROLLA TSS2 2019 b12372ab7a7a2cb2|2023-09-30--00-46-45--0
None None 4.429301738739014
new angle offset existence does not match original TOYOTA RAV4 2019 8aad58f199ab2bed|2023-09-10--20-57-00--0
-1.005100000000013 None 8.504498481750488
new angle offset existence does not match original TOYOTA CAMRY HYBRID 2021 5d4b81649a4bce56|2023-10-10--12-46-07--0
None None 1.9003969430923462
new angle offset existence does not match original LEXUS RX 2020 b0d686668fb49a0b|2023-05-18--18-37-59--0
None None -4.60699987411499
new angle offset existence does not match original TOYOTA COROLLA TSS2 2019 b2a420a551c95f61|2023-06-01--12-20-27--0
None None 5.9596028327941895
new angle offset existence does not match original TOYOTA CAMRY HYBRID 2021 e9c218439775bc4c|2023-09-14--16-38-21--0
None None 1.6048030853271484

---

Specific cases

Here are cases where the online offset changes significantly within a segment

View

big diff TOYOTA COROLLA TSS2 2019 2cd916ec939a0cdb|2023-08-17--09-21-03--0
online offset this segment: 9.2888 -> 5.1606
final angle offset from segment 9: 1.4687  # offset from future
estimated fast filtered angle offset: 1.4529  # this is an estimated ground truth based on this segment
               new calc angle offset: 0.4953 (100 deg/s), 2.187 (50 deg/s)
time to offset: 35 frames (old conditions), 459 frames (100 deg/s), 468 frames (50 deg/s)
---
big diff TOYOTA RAV4 2022 3af132f650bd6b72|2023-08-09--17-11-05--0
online offset this segment: -9.5863 -> -5.4677
final angle offset from segment 45: 1.6951
estimated fast filtered angle offset: 2.5209
               new calc angle offset: 2.652 (100 deg/s), 2.7476 (50 deg/s)
time to offset: 343 frames (old conditions), 1865 frames (100 deg/s), 1868 frames (50 deg/s)
---
big diff TOYOTA RAV4 HYBRID 2022 2475fb3eb2ffcc2e|2023-06-05--05-20-07--0
online offset this segment: -9.9808 -> -4.7428
final angle offset from segment 16: 0.198
estimated fast filtered angle offset: -0.3757
               new calc angle offset: -1.0646 (100 deg/s), -0.2657 (50 deg/s)
time to offset: 394 frames (old conditions), 804 frames (100 deg/s), 873 frames (50 deg/s)
---
big diff TOYOTA RAV4 2022 1e0487c8a03931a1|2023-09-05--16-04-24--0
online offset this segment: 8.6976 -> 4.6187
final angle offset from segment 7: 1.1855
estimated fast filtered angle offset: 1.727
               new calc angle offset: 4.3699 (100 deg/s), 2.7474 (50 deg/s)
time to offset: 455 frames (old conditions), 470 frames (100 deg/s), 498 frames (50 deg/s)
---
big diff TOYOTA RAV4 HYBRID 2022 9f7a308d4490eb77|2023-07-02--15-08-15--0
online offset this segment: -9.8965 -> -3.4559
final angle offset from segment 191: 1.3888
estimated fast filtered angle offset: 1.9513
               new calc angle offset: 4.7963 (100 deg/s), 3.5743 (50 deg/s)
time to offset: 52 frames (old conditions), 481 frames (100 deg/s), 492 frames (50 deg/s)
---
big diff TOYOTA HIGHLANDER 2020 25593b1a5f760b07|2023-06-26--20-39-56--0
online offset this segment: -16.7353 -> -8.7005
final angle offset from segment 4: -2.0513
estimated fast filtered angle offset: -1.6438
               new calc angle offset: -1.8883 (100 deg/s), -1.8379 (50 deg/s)
time to offset: 399 frames (old conditions), 947 frames (100 deg/s), 1025 frames (50 deg/s)
---
big diff TOYOTA HIGHLANDER 2020 75157fa04b7760e2|2023-09-12--18-11-50--0
online offset this segment: 9.5158 -> 5.353
final angle offset from segment None: None
estimated fast filtered angle offset: 0.3212
               new calc angle offset: 0.2828 (100 deg/s), 0.2828 (50 deg/s)
time to offset: 14 frames (old conditions), 1225 frames (100 deg/s), 1225 frames (50 deg/s)
---
big diff TOYOTA COROLLA TSS2 2019 c33e7ea5e01511d3|2023-05-17--19-52-46--0
online offset this segment: 15.2111 -> 8.1798
final angle offset from segment 97: 1.8593
estimated fast filtered angle offset: 2.1737
               new calc angle offset: 0.8299 (100 deg/s), 1.4296 (50 deg/s)
time to offset: 77 frames (old conditions), 491 frames (100 deg/s), 499 frames (50 deg/s)
---
big diff TOYOTA RAV4 2022 1c16ee70deee2972|2023-05-17--11-05-32--0
online offset this segment: -13.1695 -> -4.5734
final angle offset from segment None: None
estimated fast filtered angle offset: 2.3048
               new calc angle offset: 4.6012 (100 deg/s), 3.8734 (50 deg/s)
time to offset: 432 frames (old conditions), 683 frames (100 deg/s), 691 frames (50 deg/s)
---
big diff TOYOTA RAV4 2019 336f33d84fa8d5db|2023-06-05--21-12-02--0
online offset this segment: 9.9874 -> 4.3418
final angle offset from segment 6: 0.3648
estimated fast filtered angle offset: -0.0282
               new calc angle offset: 1.3337 (100 deg/s), 0.634 (50 deg/s)
time to offset: 190 frames (old conditions), 257 frames (100 deg/s), 265 frames (50 deg/s)
---
big diff TOYOTA RAV4 HYBRID 2022 6fe3baa907616d50|2023-05-26--10-23-13--0
online offset this segment: -10.4561 -> -4.5896
final angle offset from segment 34: 0.021
estimated fast filtered angle offset: -0.2976
               new calc angle offset: -1.4073 (100 deg/s), -1.4164 (50 deg/s)
time to offset: 0 frames (old conditions), 38 frames (100 deg/s), 41 frames (50 deg/s)
---
big diff TOYOTA RAV4 2022 dfb11f122b6fa282|2023-06-19--18-13-56--0
online offset this segment: -14.0783 -> -4.4322
final angle offset from segment None: None
estimated fast filtered angle offset: 3.7956
               new calc angle offset: 1.7699 (100 deg/s), 2.7889 (50 deg/s)
time to offset: 235 frames (old conditions), 283 frames (100 deg/s), 285 frames (50 deg/s)
---
big diff TOYOTA RAV4 2019 b6508eacf6f008df|2023-05-25--22-27-43--0
online offset this segment: -7.3651 -> -2.7292
final angle offset from segment 20: 1.5743
estimated fast filtered angle offset: 1.2492
               new calc angle offset: 1.0009 (100 deg/s), 2.3053 (50 deg/s)
time to offset: 230 frames (old conditions), 250 frames (100 deg/s), 253 frames (50 deg/s)
---
big diff TOYOTA RAV4 2022 7ac1ec97ce175121|2023-06-20--17-27-54--0
online offset this segment: 13.3538 -> 6.6541
final angle offset from segment None: None
estimated fast filtered angle offset: 0.4961
               new calc angle offset: -0.6512 (100 deg/s), -0.3684 (50 deg/s)
time to offset: 457 frames (old conditions), 835 frames (100 deg/s), 872 frames (50 deg/s)
---
big diff TOYOTA RAV4 2019 91f940f4a03e5406|2023-10-02--12-51-17--0
online offset this segment: 11.3907 -> 6.626
final angle offset from segment None: None
estimated fast filtered angle offset: 3.0409
               new calc angle offset: 6.0879 (100 deg/s), 4.1825 (50 deg/s)
time to offset: 213 frames (old conditions), 235 frames (100 deg/s), 259 frames (50 deg/s)
---
big diff TOYOTA MIRAI 2021 390a85b0f0d7a93b|2023-05-05--08-58-11--0
online offset this segment: 8.9257 -> 4.551
final angle offset from segment 31: 0.8939
estimated fast filtered angle offset: 0.5704
               new calc angle offset: 1.9536 (100 deg/s), 0.8998 (50 deg/s)
time to offset: 599 frames (old conditions), 650 frames (100 deg/s), 690 frames (50 deg/s)
---
big diff TOYOTA RAV4 2019 180ea5df0f0b4db9|2023-05-25--08-42-03--0
online offset this segment: 10.5571 -> 6.023
final angle offset from segment 36: 0.502
estimated fast filtered angle offset: 1.9646
               new calc angle offset: 1.9018 (100 deg/s), 3.7207 (50 deg/s)
time to offset: 621 frames (old conditions), 993 frames (100 deg/s), 1021 frames (50 deg/s)
---
big diff TOYOTA RAV4 2019 e1905174a6abbc3c|2023-06-02--22-59-34--0
online offset this segment: -14.3816 -> -5.5266
final angle offset from segment 8: 1.988
estimated fast filtered angle offset: 1.9228
               new calc angle offset: 3.8844 (100 deg/s), 2.7693 (50 deg/s)
time to offset: 45 frames (old conditions), 410 frames (100 deg/s), 425 frames (50 deg/s)
---
big diff TOYOTA RAV4 2022 4d13f4feeb6a3d94|2023-08-31--07-28-45--0
online offset this segment: 8.1538 -> 3.3246
final angle offset from segment None: None
estimated fast filtered angle offset: -0.8139
               new calc angle offset: 2.1625 (100 deg/s), 1.073 (50 deg/s)
time to offset: 157 frames (old conditions), 174 frames (100 deg/s), 183 frames (50 deg/s)
---
big diff TOYOTA RAV4 2022 f0a34b105563fa46|2023-05-30--20-28-35--0
online offset this segment: -15.8466 -> -6.7772
final angle offset from segment 14: 0.7096
estimated fast filtered angle offset: 1.0219
               new calc angle offset: 3.5351 (100 deg/s), 1.5241 (50 deg/s)
time to offset: 157 frames (old conditions), 645 frames (100 deg/s), 649 frames (50 deg/s)
---
big diff TOYOTA RAV4 2022 1c16ee70deee2972|2023-07-29--08-19-35--0
online offset this segment: 12.0626 -> 7.1576
final angle offset from segment None: None
estimated fast filtered angle offset: 2.0233
               new calc angle offset: 4.4096 (100 deg/s), 2.7066 (50 deg/s)
time to offset: 423 frames (old conditions), 1214 frames (100 deg/s), 1222 frames (50 deg/s)
---
big diff TOYOTA RAV4 2019 cca5f152e7f637f0|2023-05-30--07-12-35--0
online offset this segment: 18.7656 -> 13.323
final angle offset from segment 73: 4.6985
estimated fast filtered angle offset: 5.2165
               new calc angle offset: 5.207 (100 deg/s), 5.3004 (50 deg/s)
time to offset: 807 frames (old conditions), 1650 frames (100 deg/s), 1761 frames (50 deg/s)
---
big diff TOYOTA COROLLA TSS2 2019 0dee9a3888231f76|2023-09-05--17-48-11--0
online offset this segment: -11.6808 -> -5.0206
final angle offset from segment 2: -0.9906
estimated fast filtered angle offset: -0.1256
               new calc angle offset: 0.2693 (100 deg/s), -0.5547 (50 deg/s)
time to offset: 382 frames (old conditions), 673 frames (100 deg/s), 737 frames (50 deg/s)
---
big diff TOYOTA HIGHLANDER 2020 24bef3a2265d4af2|2023-08-19--13-04-55--0
online offset this segment: 9.8158 -> 5.64
final angle offset from segment None: None
estimated fast filtered angle offset: 1.4597
               new calc angle offset: 1.7756 (100 deg/s), 0.732 (50 deg/s)
time to offset: 559 frames (old conditions), 1300 frames (100 deg/s), 1350 frames (50 deg/s)
---
big diff TOYOTA RAV4 2019 8610459df75c5ac1|2023-05-24--08-25-59--0
online offset this segment: 5.2687 -> 0.6739
final angle offset from segment 6: -2.7743
estimated fast filtered angle offset: -2.5974
               new calc angle offset: -2.0167 (100 deg/s), -1.9889 (50 deg/s)
time to offset: 406 frames (old conditions), 701 frames (100 deg/s), 709 frames (50 deg/s)
---
big diff TOYOTA COROLLA TSS2 2019 d0bef3e100712fce|2023-05-01--11-47-40--0
online offset this segment: 7.4216 -> 3.3316
final angle offset from segment 5: -0.0168
estimated fast filtered angle offset: -0.1547
               new calc angle offset: -0.9649 (100 deg/s), -1.064 (50 deg/s)
time to offset: 484 frames (old conditions), 824 frames (100 deg/s), 870 frames (50 deg/s)
---
big diff TOYOTA RAV4 2022 7ac1ec97ce175121|2023-07-06--12-37-50--0
online offset this segment: -10.9662 -> -5.7906
final angle offset from segment 20: -0.691
estimated fast filtered angle offset: -0.9217
               new calc angle offset: 2.2496 (100 deg/s), 0.6353 (50 deg/s)
time to offset: 213 frames (old conditions), 658 frames (100 deg/s), 664 frames (50 deg/s)
---
big diff TOYOTA RAV4 2019 8d99fb063bc66a26|2023-05-22--21-27-52--0
online offset this segment: -5.804 -> -0.8546
final angle offset from segment None: None
estimated fast filtered angle offset: 2.5845
               new calc angle offset: 4.2574 (100 deg/s), 3.0606 (50 deg/s)
time to offset: 0 frames (old conditions), 81 frames (100 deg/s), 160 frames (50 deg/s)
---
big diff TOYOTA HIGHLANDER 2020 878a25353867b7e1|2023-07-05--16-47-29--0
online offset this segment: -3.6227 -> 0.5664
final angle offset from segment 4: 4.0559
estimated fast filtered angle offset: 4.312
               new calc angle offset: 3.3552 (100 deg/s), 4.2937 (50 deg/s)
time to offset: 55 frames (old conditions), 573 frames (100 deg/s), 632 frames (50 deg/s)
---
big diff TOYOTA RAV4 2019 28947140cc79e4cf|2023-07-18--22-36-24--0
online offset this segment: 10.1538 -> 4.098
final angle offset from segment 27: 0.0343
estimated fast filtered angle offset: -0.055
               new calc angle offset: 0.3155 (100 deg/s), 0.9838 (50 deg/s)
time to offset: 194 frames (old conditions), 569 frames (100 deg/s), 575 frames (50 deg/s)
---
big diff TOYOTA RAV4 HYBRID 2022 0dbef6854d19f908|2023-04-28--17-39-34--0
online offset this segment: 15.2692 -> 7.7221
final angle offset from segment 2: 1.7551
estimated fast filtered angle offset: 0.2263
               new calc angle offset: -1.2625 (100 deg/s), -0.4964 (50 deg/s)
time to offset: 322 frames (old conditions), 657 frames (100 deg/s), 661 frames (50 deg/s)
---
big diff TOYOTA RAV4 2022 1c16ee70deee2972|2023-05-21--10-15-50--0
online offset this segment: -11.3728 -> -3.8682
final angle offset from segment 22: 3.1358
estimated fast filtered angle offset: 3.9658
               new calc angle offset: 5.9746 (100 deg/s), 5.4468 (50 deg/s)
time to offset: 336 frames (old conditions), 1001 frames (100 deg/s), 1008 frames (50 deg/s)
---
big diff TOYOTA COROLLA TSS2 2019 b2a420a551c95f61|2023-07-30--13-06-31--0
online offset this segment: 9.5808 -> 5.5169
final angle offset from segment None: None
estimated fast filtered angle offset: 1.8179
               new calc angle offset: 0.8569 (100 deg/s), 1.7602 (50 deg/s)
time to offset: 353 frames (old conditions), 748 frames (100 deg/s), 749 frames (50 deg/s)
---
big diff TOYOTA HIGHLANDER 2020 bf17b81eba9bf96e|2023-06-02--16-01-07--0
online offset this segment: 8.6731 -> 4.3512
final angle offset from segment None: None
estimated fast filtered angle offset: -1.0644
               new calc angle offset: -1.0726 (100 deg/s), -1.5841 (50 deg/s)
time to offset: 240 frames (old conditions), 2069 frames (100 deg/s), 2090 frames (50 deg/s)
---
big diff TOYOTA COROLLA TSS2 2019 e8d19bce0da0148f|2023-04-26--19-29-08--0
online offset this segment: 10.774 -> 6.2389
final angle offset from segment None: None
estimated fast filtered angle offset: 2.4228
               new calc angle offset: 4.453 (100 deg/s), 2.3761 (50 deg/s)
time to offset: 0 frames (old conditions), 39 frames (100 deg/s), 183 frames (50 deg/s)
---
big diff TOYOTA HIGHLANDER HYBRID 2020 e8805c57d88ac65c|2023-05-19--19-15-28--0
online offset this segment: -8.4056 -> -4.1297
final angle offset from segment 10: -0.066
estimated fast filtered angle offset: -0.3036
               new calc angle offset: 0.3229 (100 deg/s), 1.105 (50 deg/s)
time to offset: 85 frames (old conditions), 370 frames (100 deg/s), 373 frames (50 deg/s)
---
big diff TOYOTA HIGHLANDER HYBRID 2020 6d0254ba44b664b0|2023-06-23--08-44-05--0
online offset this segment: 5.799 -> 1.05
final angle offset from segment 3: -3.8346
estimated fast filtered angle offset: -4.1856
               new calc angle offset: -3.9021 (100 deg/s), -3.1629 (50 deg/s)
time to offset: 618 frames (old conditions), 877 frames (100 deg/s), 931 frames (50 deg/s)
---
big diff TOYOTA RAV4 HYBRID 2022 77f07def1eec9d4e|2023-05-14--09-17-34--0
online offset this segment: 8.9596 -> 4.3062
final angle offset from segment 7: 0.4334
estimated fast filtered angle offset: 0.3472
               new calc angle offset: 0.7464 (100 deg/s), 0.6084 (50 deg/s)
time to offset: 545 frames (old conditions), 601 frames (100 deg/s), 606 frames (50 deg/s)
---
big diff TOYOTA RAV4 2022 1c16ee70deee2972|2023-04-26--15-43-51--0
online offset this segment: 15.367 -> 8.6134
final angle offset from segment None: None
estimated fast filtered angle offset: 2.5711
               new calc angle offset: 3.898 (100 deg/s), 2.8859 (50 deg/s)
time to offset: 354 frames (old conditions), 1560 frames (100 deg/s), 1566 frames (50 deg/s)
---
big diff TOYOTA RAV4 2022 dfb11f122b6fa282|2023-04-24--10-36-54--0
online offset this segment: -14.8557 -> -4.3483
final angle offset from segment 20: 3.5349
estimated fast filtered angle offset: 3.8014
               new calc angle offset: 2.0725 (100 deg/s), 3.2901 (50 deg/s)
time to offset: 97 frames (old conditions), 146 frames (100 deg/s), 154 frames (50 deg/s)
---
big diff TOYOTA HIGHLANDER HYBRID 2020 fe9f9a84fe26591c|2023-09-09--10-17-01--0
online offset this segment: -7.9819 -> -3.934
final angle offset from segment None: None
estimated fast filtered angle offset: 0.4783
               new calc angle offset: 1.0586 (100 deg/s), 0.2414 (50 deg/s)
time to offset: 970 frames (old conditions), 1688 frames (100 deg/s), 1731 frames (50 deg/s)
---
big diff TOYOTA RAV4 2019 4a4e7f053b5730d9|2023-09-14--07-54-05--0
online offset this segment: 12.9841 -> 5.9159
final angle offset from segment None: None
estimated fast filtered angle offset: -0.026
               new calc angle offset: -2.0891 (100 deg/s), -0.6735 (50 deg/s)
time to offset: 338 frames (old conditions), 576 frames (100 deg/s), 657 frames (50 deg/s)
---
big diff TOYOTA RAV4 HYBRID 2023 c5e5740d799f017f|2023-10-03--23-42-03--0
online offset this segment: -4.9946 -> -0.6669
final angle offset from segment 20: 2.8401
estimated fast filtered angle offset: 3.0978
               new calc angle offset: 6.6983 (100 deg/s), 4.5537 (50 deg/s)
time to offset: 552 frames (old conditions), 882 frames (100 deg/s), 888 frames (50 deg/s)
---
big diff TOYOTA CAMRY HYBRID 2021 297b4b460f361603|2023-07-22--04-08-47--0
online offset this segment: 14.0615 -> 7.114
final angle offset from segment 11: 0.0242
estimated fast filtered angle offset: 0.5388
               new calc angle offset: -0.0596 (100 deg/s), 1.7938 (50 deg/s)
time to offset: 263 frames (old conditions), 1217 frames (100 deg/s), 1228 frames (50 deg/s)