ekf2: add simple zero velocity update when vehicle at rest (#19149)

- further decreases initial tilt alignment time (now down to 2.5 seconds if still) and improves initial bias estimates

src/modules/ekf2/EKF/CMakeLists.txt

@@ -56,6 +56,7 @@ add_library(ecl_EKF
 	terrain_estimator.cpp
 	utils.cpp
 	vel_pos_fusion.cpp
+	zero_velocity_update.cpp
 )
 
 add_dependencies(ecl_EKF prebuild_targets)

src/modules/ekf2/EKF/control.cpp

@@ -181,6 +181,8 @@ void Ekf::controlFusionModes()
 	// Additional horizontal velocity data from an auxiliary sensor can be fused
 	controlAuxVelFusion();
 
+	controlZeroVelocityUpdate();
+
 	// Fake position measurement for constraining drift when no other velocity or position measurements
 	controlFakePosFusion();
 
@@ -476,6 +478,7 @@ void Ekf::controlOpticalFlowFusion()
 			// If the heading is valid and use is not inhibited , start using optical flow aiding
 			if (_control_status.flags.yaw_align || _params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
 				// set the flag and reset the fusion timeout
+				ECL_INFO("starting optical flow fusion");
 				_control_status.flags.opt_flow = true;
 				_time_last_of_fuse = _time_last_imu;
 
@@ -512,7 +515,7 @@ void Ekf::controlOpticalFlowFusion()
 		}
 
 	} else if (_control_status.flags.opt_flow
-		   && (_imu_sample_delayed.time_us >  _flow_sample_delayed.time_us + (uint64_t)10e6)) {
+		   && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us + (uint64_t)10e6)) {
 
 		stopFlowFusion();
 	}

src/modules/ekf2/EKF/ekf.cpp

@@ -75,6 +75,9 @@ void Ekf::reset()
 	_control_status.value = 0;
 	_control_status_prev.value = 0;
 
+	_control_status.flags.in_air = true;
+	_control_status_prev.flags.in_air = true;
+
 	_ang_rate_delayed_raw.zero();
 
 	_fault_status.value = 0;

src/modules/ekf2/EKF/ekf.h

@@ -61,7 +61,11 @@ public:
 
 	using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
 
-	Ekf() = default;
+	Ekf()
+	{
+		reset();
+	};
+
 	virtual ~Ekf() = default;
 
 	// initialise variables to sane values (also interface class)
@@ -379,6 +383,7 @@ private:
 	uint64_t _time_last_arsp_fuse{0};	///< time the last fusion of airspeed measurements were performed (uSec)
 	uint64_t _time_last_beta_fuse{0};	///< time the last fusion of synthetic sideslip measurements were performed (uSec)
 	uint64_t _time_last_fake_pos_fuse{0};	///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec)
+	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
 	uint64_t _time_last_gps_yaw_fuse{0};	///< time the last fusion of GPS yaw measurements were performed (uSec)
 	uint64_t _time_last_gps_yaw_data{0};	///< time the last GPS yaw measurement was available (uSec)
 	uint64_t _time_last_healthy_rng_data{0};
@@ -870,6 +875,8 @@ private:
 	// control fusion of fake position observations to constrain drift
 	void controlFakePosFusion();
 
+	void controlZeroVelocityUpdate();
+
 	// control fusion of auxiliary velocity observations
 	void controlAuxVelFusion();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1011,12 +1011,21 @@ void Ekf::update_deadreckoning_status()
 	_control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
 
 	if (!_control_status.flags.inertial_dead_reckoning) {
-		_time_last_aiding = _time_last_imu - _params.no_aid_timeout_max;
+		if (_time_last_imu > _params.no_aid_timeout_max) {
+			_time_last_aiding = _time_last_imu - _params.no_aid_timeout_max;
+		}
 	}
 
 	// report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present
-	_deadreckon_time_exceeded = (_time_last_aiding == 0)
+	bool deadreckon_time_exceeded = (_time_last_aiding == 0)
 				    || isTimedOut(_time_last_aiding, (uint64_t)_params.valid_timeout_max);
+
+	if (!_deadreckon_time_exceeded && deadreckon_time_exceeded) {
+		// deadreckon time now exceeded
+		ECL_WARN("dead reckon time exceeded");
+	}
+
+	_deadreckon_time_exceeded = deadreckon_time_exceeded;
 }
 
 // calculate the variances for the rotation vector equivalent
@@ -1652,10 +1661,13 @@ void Ekf::stopAuxVelFusion()
 
 void Ekf::stopFlowFusion()
 {
-	_control_status.flags.opt_flow = false;
-	_flow_innov.setZero();
-	_flow_innov_var.setZero();
-	_optflow_test_ratio = 0.0f;
+	if (_control_status.flags.opt_flow) {
+		ECL_INFO("stopping optical flow fusion");
+		_control_status.flags.opt_flow = false;
+		_flow_innov.setZero();
+		_flow_innov_var.setZero();
+		_optflow_test_ratio = 0.0f;
+	}
 }
 
 void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)

src/modules/ekf2/EKF/estimator_interface.h

@@ -258,12 +258,7 @@ public:
 
 protected:
 
-	EstimatorInterface()
-	{
-		_control_status.flags.in_air = true;
-		_control_status.flags.vehicle_at_rest = false;
-	};
-
+	EstimatorInterface() = default;
 	virtual ~EstimatorInterface();
 
 	virtual bool init(uint64_t timestamp) = 0;

src/modules/ekf2/EKF/zero_velocity_update.cpp

@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file zero_velocity_update.cpp
+ * Control function for ekf zero velocity update
+ */
+
+#include "ekf.h"
+
+void Ekf::controlZeroVelocityUpdate()
+{
+	// Fuse zero velocity at a limited rate (every 200 milliseconds)
+	const bool zero_velocity_update_data_ready = isTimedOut(_time_last_zero_velocity_fuse, (uint64_t)2e5);
+
+	if (zero_velocity_update_data_ready) {
+		const bool continuing_conditions_passing = _control_status.flags.vehicle_at_rest
+				&& _control_status_prev.flags.vehicle_at_rest;
+
+		if (continuing_conditions_passing) {
+			Vector3f vel_obs{0, 0, 0};
+			Vector3f innovation = _state.vel - vel_obs;
+
+			const float obs_var = sq(0.001f);
+			Vector3f innov_var{
+				P(4, 4) + obs_var,
+				P(5, 5) + obs_var,
+				P(6, 6) + obs_var};
+
+			fuseVelPosHeight(innovation(0), innov_var(0), 0);
+			fuseVelPosHeight(innovation(1), innov_var(1), 1);
+			fuseVelPosHeight(innovation(2), innov_var(2), 2);
+
+			_time_last_zero_velocity_fuse = _time_last_imu;
+		}
+	}
+}