position_estimator_inav fixes

 - Fixed accelerometer bias estimation
 - GPS validity conditions fixed
 - Some debug code to catch NaN position bug on startup
 - Some cosmetic fixes

Tested in flight, accelerometer bias estimation works nice, can be merged, but I want to catch the bug with NaN estimation first.