...
- if |measure_heading - model_heading| > 3030degrees
- keep model_heading
- else if |measure_heading - model_heading| > 20 20degrees and |measure_Dpos - model_Dpos| > 0.35 * model_Dpos // this is the uncertain case
- keep model_heading
- else
- keep measured_heading
...