Added innovation logging to LPE. (#5124)

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -50,6 +50,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()),
 	_pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()),
 	_pub_est_status(ORB_ID(estimator_status), -1, &getPublications()),
+	_pub_innov(ORB_ID(ekf2_innovations), -1, &getPublications()),
 
 	// map projection
 	_map_ref(),
@@ -479,6 +480,7 @@ void BlockLocalPositionEstimator::update()
 		// update all publications if possible
 		publishLocalPos();
 		publishEstimatorStatus();
+		_pub_innov.update();
 
 		if (_validXY) {
 			publishGlobalPos();

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -28,6 +28,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>
 
 using namespace matrix;
 using namespace control;
@@ -235,6 +236,7 @@ private:
 	uORB::Publication<vehicle_local_position_s> _pub_lpos;
 	uORB::Publication<vehicle_global_position_s> _pub_gpos;
 	uORB::Publication<estimator_status_s> _pub_est_status;
+	uORB::Publication<ekf2_innovations_s> _pub_innov;
 
 	// map projection
 	struct map_projection_reference_s _map_ref;

src/modules/local_position_estimator/sensors/flow.cpp

@@ -122,7 +122,7 @@ void BlockLocalPositionEstimator::flowCorrect()
 	C(Y_flow_x, X_x) = 1;
 	C(Y_flow_y, X_y) = 1;
 
-	Matrix<float, n_y_flow, n_y_flow> R;
+	SquareMatrix<float, n_y_flow> R;
 	R.setZero();
 	R(Y_flow_x, Y_flow_x) =
 		_flow_xy_stddev.get() * _flow_xy_stddev.get();
@@ -131,6 +131,10 @@ void BlockLocalPositionEstimator::flowCorrect()
 
 	// residual
 	Vector<float, 2> r = y - C * _x;
+	_pub_innov.get().flow_innov[0] = r(0);
+	_pub_innov.get().flow_innov[1] = r(1);
+	_pub_innov.get().flow_innov_var[0] = R(0, 0);
+	_pub_innov.get().flow_innov_var[1] = R(1, 1);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_flow, n_y_flow> S_I =

src/modules/local_position_estimator/sensors/gps.cpp

@@ -114,7 +114,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	C(Y_gps_vz, X_vz) = 1;
 
 	// gps covariance matrix
-	Matrix<float, n_y_gps, n_y_gps> R;
+	SquareMatrix<float, n_y_gps> R;
 	R.setZero();
 
 	// default to parameter, use gps cov if provided
@@ -141,10 +141,10 @@ void BlockLocalPositionEstimator::gpsCorrect()
 
 	// get delayed x and P
 	float t_delay = 0;
-	int i = 0;
+	int i_hist = 0;
 
-	for (i = 1; i < HIST_LEN; i++) {
-		t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i)(0, 0));
+	for (i_hist = 1; i_hist < HIST_LEN; i_hist++) {
+		t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i_hist)(0, 0));
 
 		if (t_delay > _gps_delay.get()) {
 			break;
@@ -158,10 +158,16 @@ void BlockLocalPositionEstimator::gpsCorrect()
 		return;
 	}
 
-	Vector<float, n_x> x0 = _xDelay.get(i);
+	Vector<float, n_x> x0 = _xDelay.get(i_hist);
 
 	// residual
 	Vector<float, n_y_gps> r = y - C * x0;
+
+	for (int i = 0; i < 6; i ++) {
+		_pub_innov.get().vel_pos_innov[i] = r(i);
+		_pub_innov.get().vel_pos_innov_var[i] = R(i, i);
+	}
+
 	Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, 6>(C * _P * C.transpose() + R);
 
 	// fault detection

src/modules/local_position_estimator/sensors/lidar.cpp

@@ -77,7 +77,7 @@ void BlockLocalPositionEstimator::lidarCorrect()
 	C(Y_lidar_z, X_tz) = 1; // measured altitude, negative down dir.
 
 	// use parameter covariance unless sensor provides reasonable value
-	Matrix<float, n_y_lidar, n_y_lidar> R;
+	SquareMatrix<float, n_y_lidar> R;
 	R.setZero();
 	float cov = _sub_lidar->get().covariance;
 
@@ -91,6 +91,8 @@ void BlockLocalPositionEstimator::lidarCorrect()
 	// residual
 	Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>((C * _P * C.transpose()) + R);
 	Vector<float, n_y_lidar> r = y - C * _x;
+	_pub_innov.get().hagl_innov = r(0);
+	_pub_innov.get().hagl_innov_var = R(0, 0);
 
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);

src/modules/local_position_estimator/sensors/sonar.cpp

@@ -99,12 +99,14 @@ void BlockLocalPositionEstimator::sonarCorrect()
 	C(Y_sonar_z, X_tz) = 1; // measured altitude, negative down dir.
 
 	// covariance matrix
-	Matrix<float, n_y_sonar, n_y_sonar> R;
+	SquareMatrix<float, n_y_sonar> R;
 	R.setZero();
 	R(0, 0) = cov;
 
 	// residual
 	Vector<float, n_y_sonar> r = y - C * _x;
+	_pub_innov.get().hagl_innov = r(0);
+	_pub_innov.get().hagl_innov_var = R(0, 0);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_sonar, n_y_sonar> S_I =

src/modules/uORB/Publication.cpp

@@ -50,6 +50,7 @@
 #include "topics/tecs_status.h"
 #include "topics/rc_channels.h"
 #include "topics/filtered_bottom_flow.h"
+#include "topics/ekf2_innovations.h"
 
 #include <px4_defines.h>
 
@@ -120,5 +121,6 @@ template class __EXPORT Publication<actuator_direct_s>;
 template class __EXPORT Publication<tecs_status_s>;
 template class __EXPORT Publication<rc_channels_s>;
 template class __EXPORT Publication<filtered_bottom_flow_s>;
+template class __EXPORT Publication<ekf2_innovations_s>;
 
 }