Reduce steering error with better lane width calculation

[Gernby]

This change is related to this PR from a couple weeks ago, and was prompted by the comment today from @zorrobyte about steering error when 1 lane line disappeared before a turn.

After some discussion with with the community on Discord, I tested a few changes that made a very significant improvement for lateral stability.

The root issues with the current lane width logic are:

• Lane width estimate changes too fast, which causes the MPC to overreact to noise in the lane lines
• Speed-based lane width has too much impact when 1 lane line has a low probability
• Lane width change is only time-based, not distance.

The proposed logic addresses all of those issues.

Below is a screen capture of my dashboard before and after this change on the same turn within a few minutes of each other.

Here is a video of the turn

Here is a link to the drive in Cabana
https://my.comma.ai/cabana/?route=ddd3e089e7bbe0fc%7C2019-07-22--19-18-49&max=22&url=https%3A%2F%2Fchffrprivate-vzn.azureedge.net%2Fchffrprivate3%2Fv2%2Fddd3e089e7bbe0fc%2F6d8537a9b83f1a8c6695f84e3de7a108_2019-07-22--19-18-49

[zorrobyte]

I have objectively tested this PR with 0.6.1 on my '19 Prius Prime Advanced with good steering angle data and have concluded that straight line performance is in fact, enhanced. I will perform more testing over the next few days and update this PR if I have additional findings.

Good work @Gernby

@rbiasini this PR is based on a fix for this other PR:
#737

[ncflagg]

I tested a version of this fix a few hours older (diff showed here - Gernby@a324a47) and ran it for a few hours today while trying other INDI tuning numbers. It's a much more consistent lateral experience on my stock 2017 Prius Prime Premium. Until now, I ran timeconstant = 3.0 (vs 1.0) to calm over-active lane-keeping; not regular ping-pong, but frequent,noticeable corrections. I found today that timeconstant = 1.0 is tolerable now.

I'll be testing the updated version in this PR tomorrow, but am already pretty happy with what I've seen. I'm excited to see where this PR leads.

[ncflagg]

3d6af31 isn't nearly as good as the commit I previously referenced.

[Gernby]

@ncflagg I removed the change that limited the probability of the speed_lane_width. That seemed to provide better performance.

[zorrobyte]

def mean(numbers):
    return float(sum(numbers)) / max(len(numbers), 1)

self.lane_width = 3
self.frame = 0
self.readings = []

# Find current lanewidth
    if l_prob > 0.49 and r_prob > 0.49:
        self.frame += 1
        if self.frame % 20 == 0:
            self.frame = 0
            current_lane_width = sorted((2.8, abs(l_poly[3] - r_poly[3]), 3.6))[1]
            max_samples = 30
            self.readings.append(current_lane_width)
            avg = mean(self.readings)
            self.lanewidth = avg
            if len(self.readings) == max_samples:
                self.readings.pop(0)

I'm just going to leave this here. Great interstate camber handling, I can loose a lane line and stay perfectly centered. I even had success on an unmarked road with OP. No more exit diving.

I don't know what the hell is going on with stock OP

[Gernby]

@zorrobyte I'm not sure what the last screenshot is supposed to show, but it seems that you've reversed lane_width_certainty and lane_width_estimate. The certainty shouldn't ever be larger than 1.0, and the estimate would normally be about 3.7.

[zorrobyte]

Well, anyway - thanks for that @Gernby

However, the figure it estimates is still 3.68 while the actual lane width is 2.73
current_lane_width = abs(1.4096942 - -1.3224893)

@pd0wm has said he'll look into the learner and fix it if needed.

[Gernby]

After testing this PR logic side-by-side with existing logic, I'm closing this PR. It seems that standard logic has fewer vulnerabilities.