Improve state estimation (particularly during DVL beam failures)

[Daniel Volya]

We rely mostly on the DVL to return good pings in order to obtain a state estimation. This can result signifcant state drifts if the pings are not good... So a potential solution is to dynamically set coefficents in the kalman filter to vary the trust.

[Kevin Allen]

Hijacking this ticket as a meta ticket for various ways to improve state estimation.

• #381
• #83

From reading the KF code and speaking with Forest, it DOES seem DVL beam loses are handled correctly.