Use both pitch and roll angles in steering compensation

[simontheflutist]

The roll compensation formula in the 0.8.13 blog post is

$$ \dot v = \ldots + g \theta $$

where $\theta$ is the car's roll angle (treated via a small-angle approximation). Considering that locationd provides both roll $\theta$ and pitch $\phi$, I would propose to replace this formula with

$$ \dot v = \ldots + g \cos \phi \sin \theta. $$

Linearization shows that $\cos \phi \sin \theta = \theta + o(|\phi| + |\theta|)$ for small angles $\phi$ and $\theta$, but it occurs to me that including second-order effects might help with further reducing the steering error that remains.

[simontheflutist]

Here is a Python function to that compares my proposed compensation to the existing compensation in terms of grade and cross slope.

import numpy as np

def kinematic_orientation_compensation(cross_slope, grade):
    sin_theta = cross_slope / (1. + cross_slope**2)**0.5
    cos_phi = 1. / (1. + grade**2)**0.5
    
    # incumbent compensation factor
    theta = np.arcsin(sin_theta)
    # new compensation factor
    a = sin_theta * cos_phi
    
    print(f"linear compensation error is {(theta-a)/a * 100:.2f}%")
    print(f"which is {(theta - a) * 9.8:.3f} m/s/s")

For an extreme example, let's combine the following values:

• up to 10% superelevation in WSDOT guidelines
• 23% grade on a street in SF

>>> kinematic_orientation_compensation(cross_slope=0.1, grade=0.23)
linear compensation error is 2.78%
which is 0.026 m/s/s

If this acceleration went uncorrected for, say, 10 seconds, the horizontal drift would be 1.3 meters. Of course, the openpilot control loop (including the planner) would be able to soak up much of the error in due time. But it is always better not to overwork the integral controller.

[Verylukyguy]

If this acceleration went uncorrected for, say, 10 seconds, the horizontal drift would be 1.3 meters. Of course, the openpilot control loop (including the planner) would be able to soak up much of the error in due time. But it is always better not to overwork the integral controller.

It has been widely noted that on long smooth curves OpenPilot seems to "give up" at some point and if you touch the steering wheel for a second, OpenPilot "resets" and completes the curve on its own.

A couple years ago, @JasonJShuler saw this and said that is was likely a bug buried deep in OpenPilot from the beginning.
I wonder if this is it.

[simontheflutist]

@twilsonco latest patch is a better implementation than feeding straight into control. This plugs into the existing orientation-for-control filtering strategy in paramsd, avoiding difficult questions of pitch noise.

[simontheflutist]

It has been widely noted that on long smooth curves OpenPilot seems to "give up" at some point and if you touch the steering wheel for a second, OpenPIlot "resets" and completes the curve on its own.

What I am concerned about is the implementation of live parameter estimation in paramsd and torqued (e.g. observability issue?) Adaptive control is difficult to do right, and history is full of examples such as the MIT rule where a very reasonable controller failed in unexpected ways.

[Verylukyguy]

@twilsonco latest patch is a better implementation than feeding straight into control. This plugs into the existing orientation-for-control filtering strategy in paramsd, avoiding difficult questions of pitch noise.

With you guys working together, I can't wait to see the results.

[nuwandavek]

@simontheflutist this is conceptually in the correct direction. The bulk of the work, like with roll compensation, is in making sure the pitch value from the localizer can be completely trusted, on a large dataset. Roll from the localizer had a failure (from various causes) of once every thousand minutes or so, which had to be validated and addressed. I'm assuming it is the same for pitch. A few things like tapping the device I think cause pitch spikes, which need to be validated too. I'm adding this to my list of things to do, but may not get to it soon.

[simontheflutist]

But is the pitch significantly worse than roll? If not, a paramsd feedforward would seem to benefit from the noise attenuation inherent to parameter kf.

[nuwandavek]

It may not be significantly worse than roll (this needs to be verified though). But there are a few ways in which roll is different from pitch. For example, the device does not move/shake the same way in roll-direction than in pitch direction, when it is tapped, or when the car goes over potholes. This can lead to some sketchy and subtle lateral movement. But all of these things can be verified and handled.
If you're using this in your fork at the moment, I'd suggest you look at the above things. Also, you want to use the calibratedOrietationNED since there is non trivial pitch difference due to mounting, that we adjust for.

[nuwandavek]

Not a priority at the moment. Will revisit later when appropriate.