...
We recompute the measured heading because the CPS filters it and, when the jump is very small, the measured heading becomes wrong but not enough wrong to discard it. For consistency reasons then we keep the newly computed measured heading instead of the CPS one. 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 _D_heading 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.
...