lpe: small fixes

src/modules/local_position_estimator/sensors/flow.cpp

@@ -170,7 +170,7 @@ void BlockLocalPositionEstimator::flowCorrect()
 	Vector<float, 2> r = y - C * _x;
 
 	// residual covariance
-	Matrix<float, n_y_flow, n_y_flow> S = C * _P * C.transpose()) + R;
+	Matrix<float, n_y_flow, n_y_flow> S = C * _P * C.transpose() + R;
 
 	// publish innovations
 	_pub_innov.get().flow_innov[0] = r(0);
@@ -179,7 +179,7 @@ void BlockLocalPositionEstimator::flowCorrect()
 	_pub_innov.get().flow_innov_var[1] = S(1, 1);
 
 	// residual covariance, (inverse)
-	Matrix<float, n_y_flow, n_y_flow> S_I = inv(C);
+	Matrix<float, n_y_flow, n_y_flow> S_I = inv<float, n_y_flow>(S);
 
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);

src/modules/local_position_estimator/sensors/gps.cpp

@@ -185,7 +185,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	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;
+	Matrix<float, n_y_gps, n_y_gps> S = C * _P * C.transpose() + R;
 
 	// publish innovations
 	for (int i = 0; i < 6; i ++) {
@@ -194,7 +194,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	}
 
 	// residual covariance, (inverse)
-	Matrix<float, n_y_gps, n_y_gps> S_I = inv(C);
+	Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, n_y_gps>(S);
 
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);

src/modules/local_position_estimator/sensors/lidar.cpp

@@ -85,15 +85,18 @@ void BlockLocalPositionEstimator::lidarCorrect()
 		R(0, 0) = cov;
 	}
 
+	// residual
+	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;
+	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 = S(0, 0);
 
+	// residual covariance, (inverse)
+	Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>(S);
+
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);
 

src/modules/local_position_estimator/sensors/mocap.cpp

@@ -76,7 +76,7 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	// residual
 	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;
+	Matrix<float, n_y_mocap, n_y_mocap> S = C * _P * C.transpose() + R;
 
 	// publish innovations
 	for (int i = 0; i < 3; i ++) {
@@ -89,7 +89,7 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	}
 
 	// residual covariance, (inverse)
-	Matrix<float, n_y_mocap, n_y_mocap> S_I = inv(C);
+	Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>(S);
 
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);

src/modules/local_position_estimator/sensors/sonar.cpp

@@ -106,14 +106,14 @@ 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;
+	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 = S(0, 0);
 
 	// residual covariance, (inverse)
-	Matrix<float, n_y_sonar, n_y_sonar> S_I = inv(C);
+	Matrix<float, n_y_sonar, n_y_sonar> S_I = inv<float, n_y_sonar>(S);
 
 	// fault detection
 	float beta = (r.transpose()  * (S_I * r))(0, 0);