lpe: fix innovation covariance publishing

src/modules/local_position_estimator/sensors/flow.cpp

@@ -168,14 +168,18 @@ void BlockLocalPositionEstimator::flowCorrect()
 
 	// residual
 	Vector<float, 2> r = y - C * _x;
+
+	// residual covariance
+	Matrix<float, n_y_flow, n_y_flow> S = C * _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] = R(0, 0);
-	_pub_innov.get().flow_innov_var[1] = R(1, 1);
+	_pub_innov.get().flow_innov_var[0] = S(0, 0);
+	_pub_innov.get().flow_innov_var[1] = S(1, 1);
 
 	// residual covariance, (inverse)
-	Matrix<float, n_y_flow, n_y_flow> S_I =
-		inv<float, n_y_flow>(C * _P * C.transpose() + R);
+	Matrix<float, n_y_flow, n_y_flow> S_I = inv(C);
 
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);

src/modules/local_position_estimator/sensors/gps.cpp

@@ -167,7 +167,6 @@ void BlockLocalPositionEstimator::gpsCorrect()
 		var_vz = gps_s_stddev * gps_s_stddev;
 	}
 
-
 	R(0, 0) = var_xy;
 	R(1, 1) = var_xy;
 	R(2, 2) = var_z;
@@ -185,12 +184,17 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	// residual
 	Vector<float, n_y_gps> r = y - C * x0;
 
+	// residual covariance
+	Matrix<float, n_y_gps, n_y_gps> S = C * _P * C.transpose()) + R;
+
+	// publish innovations
 	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);
+		_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
 	}
 
-	Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, 6>(C * _P * C.transpose() + R);
+	// residual covariance, (inverse)
+	Matrix<float, n_y_gps, n_y_gps> S_I = inv(C);
 
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);

src/modules/local_position_estimator/sensors/lidar.cpp

@@ -85,11 +85,14 @@ void BlockLocalPositionEstimator::lidarCorrect()
 		R(0, 0) = cov;
 	}
 
-	// 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;
+	// residual covariance
+	Matrix<float, n_y_lidar, n_y_lidar> S = C * _P * C.transpose()) + R;
+	// residual covariance, (inverse)
+	Matrix<float, n_y_lidar, n_y_lidar> S_I = inv(C);
+
+	// publish innovations
 	_pub_innov.get().hagl_innov = r(0);
-	_pub_innov.get().hagl_innov_var = R(0, 0);
+	_pub_innov.get().hagl_innov_var = S(0, 0);
 
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);

src/modules/local_position_estimator/sensors/mocap.cpp

@@ -74,8 +74,22 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	R(Y_mocap_z, Y_mocap_z) = mocap_p_var;
 
 	// residual
-	Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R);
-	Matrix<float, n_y_mocap, 1> r = y - C * _x;
+	Vector<float, n_y_mocap> r = y - C * _x;
+	// residual covariance
+	Matrix<float, n_y_mocap, n_y_mocap> S = C * _P * C.transpose()) + R;
+
+	// publish innovations
+	for (int 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);
+	}
+	for (int i = 3; i < 6; i ++) {
+		_pub_innov.get().vel_pos_innov[i] = 0;
+		_pub_innov.get().vel_pos_innov_var[i] = 1;
+	}
+
+	// residual covariance, (inverse)
+	Matrix<float, n_y_mocap, n_y_mocap> S_I = inv(C);
 
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);

src/modules/local_position_estimator/sensors/sonar.cpp

@@ -105,12 +105,15 @@ void BlockLocalPositionEstimator::sonarCorrect()
 
 	// residual
 	Vector<float, n_y_sonar> r = y - C * _x;
+	// residual covariance
+	Matrix<float, n_y_sonar, n_y_sonar> S = C * _P * C.transpose()) + R;
+
+	// publish innovations
 	_pub_innov.get().hagl_innov = r(0);
-	_pub_innov.get().hagl_innov_var = R(0, 0);
+	_pub_innov.get().hagl_innov_var = S(0, 0);
 
 	// residual covariance, (inverse)
-	Matrix<float, n_y_sonar, n_y_sonar> S_I =
-		inv<float, n_y_sonar>(C * _P * C.transpose() + R);
+	Matrix<float, n_y_sonar, n_y_sonar> S_I = inv(C);
 
 	// fault detection
 	float beta = (r.transpose()  * (S_I * r))(0, 0);
@@ -149,5 +152,3 @@ void BlockLocalPositionEstimator::sonarCheckTimeout()
 		}
 	}
 }
-
-