AP_NavEKF: Replace debug code with proper monitoring of divergence test
DataFlash: Add logging for EKF divergence rate test
Replay: Add data logging for EKF divergence rate test