local_position_estimator: move to WQ and delete unused SubscriptionPollable

src/modules/local_position_estimator/sensors/baro.cpp

@@ -68,7 +68,7 @@ void BlockLocalPositionEstimator::baroCorrect()
 
 	// residual
 	Matrix<float, n_y_baro, n_y_baro> S_I =
-		inv<float, n_y_baro>((C * _P * C.transpose()) + R);
+		inv<float, n_y_baro>((C * m_P * C.transpose()) + R);
 	Vector<float, n_y_baro> r = y - (C * _x);
 
 	// fault detection
@@ -87,10 +87,10 @@ void BlockLocalPositionEstimator::baroCorrect()
 	}
 
 	// kalman filter correction always
-	Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
+	Matrix<float, n_x, n_y_baro> K = m_P * C.transpose() * S_I;
 	Vector<float, n_x> dx = K * r;
 	_x += dx;
-	_P -= K * C * _P;
+	m_P -= K * C * m_P;
 }
 
 void BlockLocalPositionEstimator::baroCheckTimeout()

src/modules/local_position_estimator/sensors/flow.cpp

@@ -169,7 +169,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 * m_P * C.transpose() + R;
 
 	// publish innovations
 	_pub_innov.get().flow_innov[0] = r(0);
@@ -196,10 +196,10 @@ void BlockLocalPositionEstimator::flowCorrect()
 
 	if (!(_sensorFault & SENSOR_FLOW)) {
 		Matrix<float, n_x, n_y_flow> K =
-			_P * C.transpose() * S_I;
+			m_P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
 		_x += dx;
-		_P -= K * C * _P;
+		m_P -= K * C * m_P;
 	}
 }
 

src/modules/local_position_estimator/sensors/gps.cpp

@@ -186,7 +186,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 * m_P * C.transpose() + R;
 
 	// publish innovations
 	for (size_t i = 0; i < 6; i++) {
@@ -217,10 +217,10 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	}
 
 	// kalman filter correction always for GPS
-	Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
+	Matrix<float, n_x, n_y_gps> K = m_P * C.transpose() * S_I;
 	Vector<float, n_x> dx = K * r;
 	_x += dx;
-	_P -= K * C * _P;
+	m_P -= K * C * m_P;
 }
 
 void BlockLocalPositionEstimator::gpsCheckTimeout()

src/modules/local_position_estimator/sensors/land.cpp

@@ -59,7 +59,7 @@ void BlockLocalPositionEstimator::landCorrect()
 	R(Y_land_agl, Y_land_agl) = _param_lpe_land_z.get() * _param_lpe_land_z.get();
 
 	// residual
-	Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * _P * C.transpose()) + R);
+	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);
@@ -85,10 +85,10 @@ void BlockLocalPositionEstimator::landCorrect()
 	}
 
 	// kalman filter correction always for land detector
-	Matrix<float, n_x, n_y_land> K = _P * C.transpose() * S_I;
+	Matrix<float, n_x, n_y_land> K = m_P * C.transpose() * S_I;
 	Vector<float, n_x> dx = K * r;
 	_x += dx;
-	_P -= K * C * _P;
+	m_P -= K * C * m_P;
 }
 
 void BlockLocalPositionEstimator::landCheckTimeout()

src/modules/local_position_estimator/sensors/landing_target.cpp

@@ -82,7 +82,7 @@ void BlockLocalPositionEstimator::landingTargetCorrect()
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_target, n_y_target> S_I =
-		inv<float, n_y_target>(C * _P * C.transpose() + R);
+		inv<float, n_y_target>(C * m_P * C.transpose() + R);
 
 	// fault detection
 	float beta = (r.transpose()  * (S_I * r))(0, 0);
@@ -103,10 +103,10 @@ void BlockLocalPositionEstimator::landingTargetCorrect()
 
 	// kalman filter correction
 	Matrix<float, n_x, n_y_target> K =
-		_P * C.transpose() * S_I;
+		m_P * C.transpose() * S_I;
 	Vector<float, n_x> dx = K * r;
 	_x += dx;
-	_P -= K * C * _P;
+	m_P -= K * C * m_P;
 
 }
 

src/modules/local_position_estimator/sensors/lidar.cpp

@@ -89,7 +89,7 @@ void BlockLocalPositionEstimator::lidarCorrect()
 	// 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 * m_P * C.transpose() + R;
 
 	// publish innovations
 	_pub_innov.get().hagl_innov = r(0);
@@ -116,10 +116,10 @@ void BlockLocalPositionEstimator::lidarCorrect()
 	}
 
 	// kalman filter correction always
-	Matrix<float, n_x, n_y_lidar> K = _P * C.transpose() * S_I;
+	Matrix<float, n_x, n_y_lidar> K = m_P * C.transpose() * S_I;
 	Vector<float, n_x> dx = K * r;
 	_x += dx;
-	_P -= K * C * _P;
+	m_P -= K * C * m_P;
 }
 
 void BlockLocalPositionEstimator::lidarCheckTimeout()

src/modules/local_position_estimator/sensors/mocap.cpp

@@ -106,7 +106,8 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	Vector<float, n_y_mocap> y;
 
 	if (mocapMeasure(y) != OK) {
-		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", _mocap_eph, _mocap_epv);
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", (double)_mocap_eph,
+					     (double)_mocap_epv);
 		return;
 	}
 
@@ -141,7 +142,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 * m_P * C.transpose() + R;
 
 	// publish innovations
 	for (size_t i = 0; i < 3; i++) {
@@ -172,10 +173,10 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	}
 
 	// kalman filter correction always
-	Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
+	Matrix<float, n_x, n_y_mocap> K = m_P * C.transpose() * S_I;
 	Vector<float, n_x> dx = K * r;
 	_x += dx;
-	_P -= K * C * _P;
+	m_P -= K * C * m_P;
 }
 
 void BlockLocalPositionEstimator::mocapCheckTimeout()

src/modules/local_position_estimator/sensors/sonar.cpp

@@ -109,7 +109,7 @@ 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 * m_P * C.transpose() + R;
 
 	// publish innovations
 	_pub_innov.get().hagl_innov = r(0);
@@ -138,10 +138,10 @@ void BlockLocalPositionEstimator::sonarCorrect()
 	// kalman filter correction if no fault
 	if (!(_sensorFault & SENSOR_SONAR)) {
 		Matrix<float, n_x, n_y_sonar> K =
-			_P * C.transpose() * S_I;
+			m_P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
 		_x += dx;
-		_P -= K * C * _P;
+		m_P -= K * C * m_P;
 	}
 }
 

src/modules/local_position_estimator/sensors/vision.cpp

@@ -111,7 +111,8 @@ void BlockLocalPositionEstimator::visionCorrect()
 	Vector<float, n_y_vision> y;
 
 	if (visionMeasure(y) != OK) {
-		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision data invalid. eph: %f epv: %f", _vision_eph, _vision_epv);
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision data invalid. eph: %f epv: %f", (double)_vision_eph,
+					     (double)_vision_epv);
 		return;
 	}
 
@@ -158,7 +159,7 @@ void BlockLocalPositionEstimator::visionCorrect()
 	// residual
 	Matrix<float, n_y_vision, 1> r = y - C * x0;
 	// residual covariance
-	Matrix<float, n_y_vision, n_y_vision> S = C * _P * C.transpose() + R;
+	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++) {
@@ -190,10 +191,10 @@ void BlockLocalPositionEstimator::visionCorrect()
 
 	// kalman filter correction if no fault
 	if (!(_sensorFault & SENSOR_VISION)) {
-		Matrix<float, n_x, n_y_vision> K = _P * C.transpose() * S_I;
+		Matrix<float, n_x, n_y_vision> K = m_P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
 		_x += dx;
-		_P -= K * C * _P;
+		m_P -= K * C * m_P;
 	}
 }