radard: add missing accel data for vision-only leads

[jyoung8607]

Description

For vision-only leads, neither the lead car accel value nor its corresponding Tau factor currently propagate to the radarState leads used by the longitudinal MPC. The vision-only accel data isn't the best, but seems a lot better than nothing.

The absence may help explain why VOACC has trouble promptly reacting to a steady-state lead car that begins to slow down. IIRC there's also supposed to be some pull-forward effect when departing from a stop in traffic, populating aLeadK may help keep up with traffic if I understood correctly.

Verification

Anecdotally I think VOACC (non-E2E policy) drives better with this change. To test it more rigorously, I cheesed up a script to plot the existing longitudinal CI test Maneuvers with and without aLeadK to simulate the MPC behavior with a VOACC lead.

In this current-state VOACC simulation, with a steady state 20m/s lead decelerating at 1m/s (quite tame), we can just barely make the stop, ending up too close. For a similar lead decelerating at 2m/s (moderate), we close to a collision and an MPC crash. I didn't bother with the 3m/s test. The simulation doesn't apply the aTau factor, but that's also added in this PR and should make the "good" stops better.

---

steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2 -- aLeadK reported

Baseline, good stop.

---

steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2 -- aLeadK set to zero

Uncomfortably close stop.

---

steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2 -- aLeadK reported

Baseline, good stop.

---

steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2 -- aLeadK set to zero

Collision, MPC crash.

[redacid95]

This has been working awesome in my HB Civic. Night and day difference in how OP reacts on VOACC. I would say that the follow distance is a bit too high the way that this acts, and would recommend that if RadarDisabled, then the follow distance be lowered. I am still playing with what those values should be, but this is a great change!

[morrislee]

I can attest that this helps with voacc performance drastically!

[haraschax]

I'm not a huge fan of the global state, I'm also not sure the VOACC can really be relied on to do the accel estimation well. Do you get similar or better results to setting the a_lead_tau to something like 0.3 or 0.5 when in VOACC mode? I suspect that will work well and would merge that if you like it.

[morrislee]

I'm not a huge fan of the global state, I'm also not sure the VOACC can really be relied on to do the accel estimation well. Do you get similar or better results to setting the a_lead_tau to something like 0.3 or 0.5 when in VOACC mode? I suspect that will work well and would merge that if you like it.

I will try with 0.3 cut off and report back, so far, I have the vlead, vleadk, and vlead acceleration scaled to the error, seems to work very reliability right now. I will make the change and test

[jyoung8607]

I'm not a huge fan of the global state,

Agreed. This was a minimum-LoC hack to demonstrate performance of the conceptual fix.

I'm also not sure the VOACC can really be relied on to do the accel estimation well.

Also agreed. That said, the goal isn't to outperform radar, it's to outperform zero.

Do you get similar or better results to setting the a_lead_tau to something like 0.3 or 0.5 when in VOACC mode? I suspect that will work well and would merge that if you like it.

I have not tried but I can certainly do so. If aLeadTau can be stateless, this PR can be reduced to two lines.

I have about 300 miles of driving to do this week, will mark as Draft until the weekend and report back.

[MarcoTheDingo]

Tested with my 22 Civic, 0.3/stateless seems to be delayed in reacting to stopping as well as slow acceleration, both with manually changing the speed as well as with lead car speed changes. Maybe it's just me, but it felt that the stateful configuration felt more responsive.

I will also add that cut ins, even when the lead has a good gap, and is at a faster speed, openpilot will still brake about ~5 mph. Very strange. Example: 5130484aa8069bad|2023-01-18--18-51-06--3 to 4

[sshane]

@HaraldSchafer do we want this PR or should we close?

[jyoung8607]

I drove the current version of this PR several weeks ago. Subjectively I feel it's an improvement over baseline master, but not nearly as much as the original version of this PR. Others who have tested seem to feel the same way.

That's not to say the static aLeadTau is necessarily bad policy. As an example, it's very possible for aggressive policy to mask issues with actuator lag or underperformance. I haven't had time to return to this PR and evaluate data beyond subjective driving-feel.

I would like to keep this PR open for now. @HaraldSchafer agreed there's a problem and that we should do something, but the particular something is TBD. We had some DM back-and-forth about how to measure the problem, the toolsets we were using. I'd like to spend a little more time on those tools so we can compare data.

I think the outcome will probably be logically equivalent to the original version of this PR, refactored for best code rather than least-touched LoC. I don't think it will make sense to have two different policy reactions for radar vs. vision, and if comma eventually makes the decision to move toward low static aLeadTau for ALL leads, this PR will not be the vehicle to effect the change.

[redacid95]

Here is what I found in my testing. Follow Distance being set to 1.2 and Comfort brake being set to 2.4 have the best results with the original PR. I personally, until something better comes along, will keep my own fork with the original version of this PR with my own distance modifications. I drove almost 6 hours with only contextual situation or my own impatience with stupid drivers causing me to have to disengage. This has been the best change for VOACC I have ever experienced in over a year I have been using my comma device. I required to give it a little gas from time to time, but that mostly had to do with curvature accel limits. With the distance mods + this change + WD Model it was able to keep almost perfect pace with the car in front of me.

I would like to note that this change has also made it more comfortable when heavy traffic slows down. I used to HAVE to disengaged when going 80mph and traffic suddenly goes down <30mph, but with the original changes I have had to disengage not a single time in those cases even with the distance modification I mentioned earlier. I also have a MUCH better experience going down steep hills, where before I had to disengage cause it braked too much with the lead car very far away, now it keeps a better distance and brakes much more accurately.

Only issue I have had was in low light situations, but that is no different than before. It seems to want to brake more at night and have a harder time keeping accurate distance. Again though, this happen before this change too.

Here are the segments of my long road trip for your viewing pleasure. There is plenty of other areas I have tested this in, but I feel like this has the most data in it I could provide.

f1055ddb93ff2d3b|2023-02-02--05-15-22
f1055ddb93ff2d3b|2023-02-02--11-03-48

I am by no means an expert on how all this works, so I understand that you all may not want the original way this was coded, but I hope my feedback, and segments will help with the decision to merge this into master.

[jyoung8607]

Closing because this is superseded by Harald's work in #27493. It includes a version of the change in this PR, and he's able to A/B test on a car with radar ground truth and feel/measure the differences, and I don't have that ability here.