initialise vertical position correctly

2026-10-20 07:33:34 +08:00 · 2016-01-08 06:42:22 +01:00
parent 5aa5f7f8c3
commit 2a88fc6cfd
2 changed files with 9 additions and 1 deletions
@@ -172,6 +172,14 @@ bool Ekf::initialiseFilter(void)
    resetVelocity();
 	resetPosition();

+    // initialize vertical position with newest baro measurement
+    baroSample baro_init = _baro_buffer.get_newest();
+    if (baro_init.time_us == 0) {
+		return false;
+    }
+
+	_state.pos(2) = -baro_init.hgt;
+
 	initialiseCovariance();

 	return true;