ECL: Clean velPos logging, deprecate ekf2_innovations msg

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -493,6 +493,8 @@ void BlockLocalPositionEstimator::Run()
 		publishEstimatorStatus();
 		_pub_innov.get().timestamp = _timeStamp;
 		_pub_innov.update();
+		_pub_innov_var.get().timestamp = _timeStamp;
+		_pub_innov_var.update();
 
 		if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) {
 			publishGlobalPos();

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -34,7 +34,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/estimator_status.h>
-#include <uORB/topics/ekf2_innovations.h>
+#include <uORB/topics/estimator_innovations.h>
 
 using namespace matrix;
 using namespace control;
@@ -285,7 +285,8 @@ private:
 	uORB::PublicationData<vehicle_global_position_s> _pub_gpos{ORB_ID(vehicle_global_position)};
 	uORB::PublicationData<vehicle_odometry_s> _pub_odom{ORB_ID(vehicle_odometry)};
 	uORB::PublicationData<estimator_status_s> _pub_est_status{ORB_ID(estimator_status)};
-	uORB::PublicationData<ekf2_innovations_s> _pub_innov{ORB_ID(ekf2_innovations)};
+	uORB::PublicationData<estimator_innovations_s> _pub_innov{ORB_ID(estimator_innovations)};
+	uORB::PublicationData<estimator_innovations_s> _pub_innov_var{ORB_ID(estimator_innovation_variances)};
 
 	// map projection
 	struct map_projection_reference_s _map_ref;

src/modules/local_position_estimator/sensors/flow.cpp

@@ -172,10 +172,10 @@ void BlockLocalPositionEstimator::flowCorrect()
 	Matrix<float, n_y_flow, n_y_flow> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
-	_pub_innov.get().flow_innov[0] = r(0);
-	_pub_innov.get().flow_innov[1] = r(1);
-	_pub_innov.get().flow_innov_var[0] = S(0, 0);
-	_pub_innov.get().flow_innov_var[1] = S(1, 1);
+	_pub_innov.get().flow[0] = r(0);
+	_pub_innov.get().flow[1] = r(1);
+	_pub_innov_var.get().flow[0] = S(0, 0);
+	_pub_innov_var.get().flow[1] = S(1, 1);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_flow, n_y_flow> S_I = inv<float, n_y_flow>(S);

src/modules/local_position_estimator/sensors/gps.cpp

@@ -189,10 +189,20 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	Matrix<float, n_y_gps, n_y_gps> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
-	for (size_t i = 0; i < 6; i++) {
-		_pub_innov.get().vel_pos_innov[i] = r(i);
-		_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
-	}
+	_pub_innov.get().gps_hvel[0] = r(3);
+	_pub_innov.get().gps_hvel[1] = r(4);
+	_pub_innov.get().gps_vvel    = r(5);
+	_pub_innov.get().gps_hpos[0] = r(0);
+	_pub_innov.get().gps_hpos[1] = r(1);
+	_pub_innov.get().gps_vpos    = r(2);
+
+	// publish innovation variances
+	_pub_innov_var.get().gps_hvel[0] = S(3, 3);
+	_pub_innov_var.get().gps_hvel[1] = S(4, 4);
+	_pub_innov_var.get().gps_vvel    = S(5, 5);
+	_pub_innov_var.get().gps_hpos[0] = S(0, 0);
+	_pub_innov_var.get().gps_hpos[1] = S(1, 1);
+	_pub_innov_var.get().gps_vpos    = S(2, 2);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, n_y_gps>(S);

src/modules/local_position_estimator/sensors/land.cpp

@@ -61,8 +61,8 @@ void BlockLocalPositionEstimator::landCorrect()
 	// residual
 	Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * m_P * C.transpose()) + R);
 	Vector<float, n_y_land> r = y - C * _x;
-	_pub_innov.get().hagl_innov = r(Y_land_agl);
-	_pub_innov.get().hagl_innov_var = R(Y_land_agl, Y_land_agl);
+	_pub_innov.get().hagl = r(Y_land_agl);
+	_pub_innov_var.get().hagl = R(Y_land_agl, Y_land_agl);
 
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);

src/modules/local_position_estimator/sensors/lidar.cpp

@@ -92,8 +92,8 @@ void BlockLocalPositionEstimator::lidarCorrect()
 	Matrix<float, n_y_lidar, n_y_lidar> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
-	_pub_innov.get().hagl_innov = r(0);
-	_pub_innov.get().hagl_innov_var = S(0, 0);
+	_pub_innov.get().hagl = r(0);
+	_pub_innov_var.get().hagl = S(0, 0);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>(S);

src/modules/local_position_estimator/sensors/mocap.cpp

@@ -145,15 +145,20 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	Matrix<float, n_y_mocap, n_y_mocap> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
-	for (size_t i = 0; i < 3; i++) {
-		_pub_innov.get().vel_pos_innov[i] = r(i);
-		_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
-	}
+	_pub_innov.get().ev_hvel[0] = NAN;
+	_pub_innov.get().ev_hvel[1] = NAN;
+	_pub_innov.get().ev_vvel    = NAN;
+	_pub_innov.get().ev_hpos[0] = r(0);
+	_pub_innov.get().ev_hpos[1] = r(1);
+	_pub_innov.get().ev_vpos    = r(2);
 
-	for (size_t i = 3; i < 6; i++) {
-		_pub_innov.get().vel_pos_innov[i] = 0;
-		_pub_innov.get().vel_pos_innov_var[i] = 1;
-	}
+	// publish innovation variances
+	_pub_innov_var.get().ev_hvel[0] = NAN;
+	_pub_innov_var.get().ev_hvel[1] = NAN;
+	_pub_innov_var.get().ev_vvel    = NAN;
+	_pub_innov_var.get().ev_hpos[0] = S(0, 0);
+	_pub_innov_var.get().ev_hpos[1] = S(1, 1);
+	_pub_innov_var.get().ev_vpos    = S(2, 2);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>(S);

src/modules/local_position_estimator/sensors/sonar.cpp

@@ -112,8 +112,8 @@ void BlockLocalPositionEstimator::sonarCorrect()
 	Matrix<float, n_y_sonar, n_y_sonar> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
-	_pub_innov.get().hagl_innov = r(0);
-	_pub_innov.get().hagl_innov_var = S(0, 0);
+	_pub_innov.get().hagl = r(0);
+	_pub_innov_var.get().hagl = S(0, 0);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_sonar, n_y_sonar> S_I = inv<float, n_y_sonar>(S);

src/modules/local_position_estimator/sensors/vision.cpp

@@ -162,15 +162,20 @@ void BlockLocalPositionEstimator::visionCorrect()
 	Matrix<float, n_y_vision, n_y_vision> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
-	for (size_t i = 0; i < 3; i++) {
-		_pub_innov.get().vel_pos_innov[i] = r(i, 0);
-		_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
-	}
+	_pub_innov.get().ev_hvel[0] = NAN;
+	_pub_innov.get().ev_hvel[1] = NAN;
+	_pub_innov.get().ev_vvel    = NAN;
+	_pub_innov.get().ev_hpos[0] = r(0, 0);
+	_pub_innov.get().ev_hpos[1] = r(1, 0);
+	_pub_innov.get().ev_vpos    = r(2, 0);
 
-	for (size_t i = 3; i < 6; i++) {
-		_pub_innov.get().vel_pos_innov[i] = 0;
-		_pub_innov.get().vel_pos_innov_var[i] = 1;
-	}
+	// publish innovation variances
+	_pub_innov_var.get().ev_hvel[0] = NAN;
+	_pub_innov_var.get().ev_hvel[1] = NAN;
+	_pub_innov_var.get().ev_vvel    = NAN;
+	_pub_innov_var.get().ev_hpos[0] = S(0, 0);
+	_pub_innov_var.get().ev_hpos[1] = S(1, 1);
+	_pub_innov_var.get().ev_vpos    = S(2, 2);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>(S);