EKF2: Adding gravity fusion observation hooks into the EKF.

Signed-off-by: Daniel M. Sahu <danielmohansahu@gmail.com>

src/modules/ekf2/EKF/common.h

@@ -386,6 +386,9 @@ struct parameters {
 	float ev_pos_innov_gate{5.0f};          ///< vision position fusion innovation consistency gate size (STD)
 	float ev_hgt_bias_nsd{0.13f};           ///< process noise for vision height bias estimation (m/s/sqrt(Hz))
 
+	// gravity fusion
+	float gravity_noise{1.0f};		///< accelerometer measurement gaussian noise (m/s**2)
+
 	// optical flow fusion
 	float flow_noise{0.15f};                ///< observation noise for optical flow LOS rate measurements (rad/sec)
 	float flow_noise_qual_min{0.5f};        ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)

src/modules/ekf2/EKF/control.cpp

@@ -198,6 +198,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	controlBetaFusion(imu_delayed);
 	controlDragFusion();
 	controlHeightFusion(imu_delayed);
+	controlGravityFusion();
 
 	// Additional data odometry data from an external estimator can be fused.
 	controlExternalVisionFusion();
@@ -428,3 +429,12 @@ void Ekf::controlAuxVelFusion()
 		}
 	}
 }
+
+void Ekf::controlGravityFusion()
+{
+	// fuse gravity observation if our overall acceleration isn't too big
+	const float gravity_scale = _accel_vec_filt.norm() / CONSTANTS_ONE_G;
+	if (gravity_scale >= 0.9f && gravity_scale <= 1.1f) {
+		fuseGravity();
+	}
+}

src/modules/ekf2/EKF/ekf.h

@@ -540,6 +540,8 @@ private:
 	float _hagl_innov{0.0f};		///< innovation of the last height above terrain measurement (m)
 	float _hagl_innov_var{0.0f};		///< innovation variance for the last height above terrain measurement (m**2)
 
+	Vector3f _gravity_innov{};	///< innovation of the last gravity fusion observation (m/s**2)
+
 	// optical flow processing
 	Vector3f _flow_gyro_bias{};	///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
 	Vector2f _flow_vel_body{};	///< velocity from corrected flow measurement (body frame)(m/s)
@@ -918,6 +920,11 @@ private:
 
 	void updateGroundEffect();
 
+	// gravity fusion
+	void fuseGravity();
+	// heuristically enable / disable gravity fusion
+	void controlGravityFusion();
+
 	// calculate the measurement variance for the optical flow sensor
 	float calcOptFlowMeasVar(const flowSample &flow_sample);
 

src/modules/ekf2/EKF/gravity_fusion.cpp

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 Estimation and Control Library (ECL). 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 ECL 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 gravity_fusion.cpp
+ * Fuse observations from the gravity vector to constrain roll
+ * and pitch (a la complementary filter).
+ * 
+ * @author Daniel M. Sahu <danielmohansahu@gmail.com>
+ */
+
+#include "ekf.h"
+#include "python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h"
+
+#include <mathlib/mathlib.h>
+
+void Ekf::fuseGravity()
+{
+	// get raw accelerometer reading at delayed horizon and expected measurement noise (gaussian)
+	const imuSample imu = get_imu_sample_delayed();
+	const Vector3f measurement = imu.delta_vel / imu.delta_vel_dt - getAccelBias();
+	const float acc_measurement_noise = sq(_params.gravity_noise);
+
+	// initialize fusion variables
+	Vector24f Kx, Ky, Kz; // Kalman gain vectors
+	Vector3f innovation_variance;
+
+	// calculate kalman gains and innovation variances
+	sym::ComputyGravityInnovVarAndKAndH(
+		getStateAtFusionHorizonAsVector(), P, measurement, acc_measurement_noise, FLT_EPSILON,
+		&_gravity_innov, &innovation_variance, &Kx, &Ky, &Kz);
+
+	// perform fusion for each axis
+	if (!measurementUpdate(Kx, innovation_variance(0), _gravity_innov(0)))
+		PX4_ERR("Gravity fusion (X axis) unsuccessful.");
+	if (!measurementUpdate(Ky, innovation_variance(1), _gravity_innov(1)))
+		PX4_ERR("Gravity fusion (y axis) unsuccessful.");
+	if (!measurementUpdate(Kz, innovation_variance(2), _gravity_innov(2)))
+		PX4_ERR("Gravity fusion (Z axis) unsuccessful.");
+}

src/modules/ekf2/EKF2.cpp

@@ -130,6 +130,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_eva_noise(_params->ev_att_noise),
 	_param_ekf2_evv_gate(_params->ev_vel_innov_gate),
 	_param_ekf2_evp_gate(_params->ev_pos_innov_gate),
+	_param_ekf2_grav_noise(_params->gravity_noise),
 	_param_ekf2_of_n_min(_params->flow_noise),
 	_param_ekf2_of_n_max(_params->flow_noise_qual_min),
 	_param_ekf2_of_qmin(_params->flow_qual_min),

src/modules/ekf2/EKF2.hpp

@@ -538,6 +538,9 @@ private:
 		(ParamExtFloat<px4::params::EKF2_EVP_GATE>)
 		_param_ekf2_evp_gate,	///< external vision position innovation consistency gate size (STD)
 
+		(ParamExtFloat<px4::params::EKF2_GRAV_NOISE>)
+		_param_ekf2_grav_noise,	///< default accelerometer noise for gravity fusion measurements (m/s**2)
+
 		// optical flow fusion
 		(ParamExtFloat<px4::params::EKF2_OF_N_MIN>)
 		_param_ekf2_of_n_min,	///< best quality observation noise for optical flow LOS rate measurements (rad/sec)

src/modules/ekf2/ekf2_params.c

@@ -843,6 +843,17 @@ PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f);
  */
 PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.1f);
 
+/**
+ * Accelerometer measurement noise for gravity based observations.
+ *
+ * @group EKF2
+ * @min 0.1
+ * @max 10.0
+ * @unit m/s^2
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_GRAV_NOISE, 1.0f);
+
 /**
  * Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum
  *