Improve kalman filter for gnss

[mazar-farran]

We need to prevent bad readings from going into the kalman filter. Bad readings ruin future data even if the future data is healthy.

1) The kalman filter should be reset if there is a large difference between the current data and the previous good data. This covers the case where we lose connection for a long time and get it back. 2) No bad data should enter. We should exclude data from the kalman filter if HDOP is bad. Possibly other criteria

[Masaya-RT]

Basic sanity check on GNSS raw data before adding to Kalman filter -> Alexei drove but still seeing issue

-> Investigating; Checking what parameters to include in data

• Alexei found reason for problem this morning: Testing updated logic overnight, found prob still exists. -> spent more time this morning. Long story short, we have mix of data incoming. DoP is coming separately from location, and data logger mixes both. DoP and Location mix results in invalid data -> even with changes, these messages are still making it into the DB -> resulted in the same issue we see now

[mazar-farran]

Alexei found the root cause of the bad results. The kalman filter was being initialized to 0 instead of using the latest data, so whenever the datalogger was reset, there would be a period where all gnss data was inaccurate until it had enough data to offset the initial 0 value.