EKF/local position acceleration: use mean value between two publications

To avoid aliasing on the ned acceleration, add an accumulation of
acceleration to improve the downsampling

src/modules/ekf2/EKF/estimator_interface.h

@@ -240,7 +240,9 @@ class EstimatorInterface
 	const matrix::Quatf &getQuaternion() const { return _output_predictor.getQuaternion(); }
 	float getUnaidedYaw() const { return _output_predictor.getUnaidedYaw(); }
 	Vector3f getVelocity() const { return _output_predictor.getVelocity(); }
-	const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); }
+
+	// get the mean velocity derivative in earth frame since last call
+	Vector3f getVelocityDerivative() { return _output_predictor.getVelocityDerivative(); }
 	float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); }
 	Vector3f getPosition() const;
 	LatLonAlt getLatLonAlt() const { return _output_predictor.getLatLonAlt(); }

src/modules/ekf2/EKF/output_predictor/output_predictor.cpp

@@ -109,7 +109,8 @@ void OutputPredictor::reset()
 
 	_R_to_earth_now.setIdentity();
 	_vel_imu_rel_body_ned.setZero();
-	_vel_deriv.setZero();
+	_delta_vel_sum.setZero();
+	_delta_vel_dt = 0.f;
 
 	_delta_angle_corr.setZero();
 
@@ -210,9 +211,8 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector
 	delta_vel_earth(2) += CONSTANTS_ONE_G * delta_velocity_dt;
 
 	// calculate the earth frame velocity derivatives
-	if (delta_velocity_dt > 0.001f) {
-		_vel_deriv = delta_vel_earth / delta_velocity_dt;
-	}
+	_delta_vel_sum += delta_vel_earth;
+	_delta_vel_dt += delta_velocity_dt;
 
 	// save the previous velocity so we can use trapezoidal integration
 	const Vector3f vel_last(_output_new.vel);
@@ -389,3 +389,17 @@ void OutputPredictor::applyCorrectionToOutputBuffer(const Vector3f &vel_correcti
 	// update output state to corrected values
 	_output_new = _output_buffer.get_newest();
 }
+
+matrix::Vector3f OutputPredictor::getVelocityDerivative()
+{
+	matrix::Vector3f vel_deriv(0.f, 0.f, 0.f);
+
+	if (_delta_vel_dt > FLT_EPSILON) {
+		vel_deriv = _delta_vel_sum / _delta_vel_dt;
+	}
+
+	_delta_vel_dt = 0.f;
+	_delta_vel_sum.setZero();
+
+	return vel_deriv;
+}

src/modules/ekf2/EKF/output_predictor/output_predictor.h

@@ -101,8 +101,8 @@ class OutputPredictor
 	// get the velocity of the body frame origin in local NED earth frame
 	matrix::Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; }
 
-	// get the velocity derivative in earth frame
-	const matrix::Vector3f &getVelocityDerivative() const { return _vel_deriv; }
+	// get the mean velocity derivative in earth frame since last call
+	matrix::Vector3f getVelocityDerivative();
 
 	// get the derivative of the vertical position of the body frame origin in local NED earth frame
 	float getVerticalPositionDerivative() const { return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); }
@@ -178,7 +178,8 @@ class OutputPredictor
 	outputVert _output_vert_new{};		// vertical filter output on the non-delayed time horizon
 	matrix::Matrix3f _R_to_earth_now{};		// rotation matrix from body to earth frame at current time
 	matrix::Vector3f _vel_imu_rel_body_ned{};		// velocity of IMU relative to body origin in NED earth frame
-	matrix::Vector3f _vel_deriv{};		// velocity derivative at the IMU in NED earth frame (m/s/s)
+	matrix::Vector3f _delta_vel_sum{};	// accumulation of delta velocity at the IMU in NED earth frame (m/s/s)
+	float _delta_vel_dt{};			// duration of delta velocity integration (s)
 
 	// output predictor states
 	matrix::Vector3f _delta_angle_corr{};	///< delta angle correction vector (rad)