EKF2: Added gravity innovations for logging.

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

msg/EstimatorInnovations.msg

@@ -28,6 +28,7 @@ float32[2] terr_flow	# flow innvoation (rad/sec) and innovation variance compute
 # Various
 float32 heading		# heading innovation (rad) and innovation variance (rad**2)
 float32[3] mag_field	# earth magnetic field innovation (Gauss) and innovation variance (Gauss**2)
+float32[3] gravity	# gravity innovation from accelerometerr vector (m/s**2)
 float32[2] drag		# drag specific force innovation (m/sec**2) and innovation variance ((m/sec)**2)
 float32 airspeed	# airspeed innovation (m/sec) and innovation variance ((m/sec)**2)
 float32 beta		# synthetic sideslip innovation (rad) and innovation variance (rad**2)

src/modules/ekf2/EKF/ekf.h

@@ -191,6 +191,8 @@ public:
 	void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
 	void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
 
+	void getGravInnov(float grav_innov[3]) const { _gravity_innov.copyTo(grav_innov); }
+
 	// get the state vector at the delayed time horizon
 	matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const;
 

src/modules/ekf2/EKF2.cpp

@@ -1077,6 +1077,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	_ekf.getHaglInnov(innovations.hagl);
 	_ekf.getHaglRateInnov(innovations.hagl_rate);
 	_ekf.getTerrainFlowInnov(innovations.terr_flow);
+	_ekf.getGravInnov(innovations.gravity);
 	// Not yet supported
 	innovations.aux_vvel = NAN;