Draw the current FeatureVector location on the plot
Fix getAngleToOrigin that fixes the headingMeasurement + filter tweaks