...
With some simple trigonometry one can obtain
Rr = w / tan(c * u)
which means that
R = sqrt(Rr^2 + w^2/4) = sqrt(w^2 / tan(c * u) + w^2 / 4)
The wheelbase can be directly measured on car. The only unknown parameter of the model is the steer factor c.
...
Leaps affect the measured heading too since it is computed from positions. I have thus implemented a model-based filter to eliminate heading jumps. The filter is applied in the car, not in the CPS because it needs the speed from the encoder. In short, the filter detects when a leap occurs and use the model heading instead of using the measurement when that happens. The algorithm is computes the following quantities:
...
measured_heading
...
= arctan((curr_x2 - prev_x2) / (curr_x1 - prev_x1))
This is the measured heading which is affected by leaps. The CPS actually computes and sends this but it makes some filtering first. When jumps are very small, this filtering causes the measured heading to become wrong but not enough to discard it. For consistency reasons then we keep the newly computed measured heading instead of the CPS one.
model_heading = prev_speed * dT / R = 2 * prev_speed * tan(c*prev_steer) / (w * sqrt(4 * tan^2(c*prev_steer)))
Where R is the curvature radius obtained from the model described in section STEER INPUT - CURVATURE RADIUS RELATIONSHIP, c is the steer factor and w is the wheelbase of the car. This is the current heading according to the model.
measured_Dpos = sqrt((prev_x1 - curr_x1)^2 + (prev_x2 - curr_
...
x2)^2)
The distance covered measured from the previous step.
model_Dpos = prev_speed *
...
dT
The distance that the car should have covered. The algorithm is the following:
- if |measure_heading - model_heading| > 30
- keep model_heading
- else if |measure_heading - model_heading| > 20 and |measure_Dpos - model_Dpos| > 0.35 * model_Dpos // this is the uncertain case
- keep model_heading
- else
- keep measured_heading
- keep measured_heading
...
- Some leaps occurs more or less in the direction of the car but they can be still detected because the car covers a much longer (or shorter) distance than it should. The second if-statement detects this kind of leaps. Thresholds have been determined empirically. You can see this filter as a simple camera change detector that applies a Kalman filter with time-varying error covariance. The measurement error covariance is 0 when camera change is not detected (i.e. the Kalman filter keeps the measurement) and non-zero otherwise. On the contrary, the a priori estimate error covariance is 0 when camera change is detected (i.e. the Kalman filter keeps the model) and non-zero otherwise.
Figure Dheading.png shows Dheading = |filtered_heading - previous_heading| in time. As you can see basically all the leaps caused by the change of the tracking camera are removed. This improved the path following performances.
...