LPE: Use euler angles derived from quaternion

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -180,7 +180,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_err_perf(),
 
 	// kf matrices
-	_x(), _u(), _P()
+	_x(), _u(), _P(), _R_att(), _eul()
 {
 	// assign distance subs to array
 	_dist_subs[0] = &_sub_dist0;
@@ -731,7 +731,8 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		_pub_lpos.get().vx = xLP(X_vx); // north
 		_pub_lpos.get().vy = xLP(X_vy); // east
 		_pub_lpos.get().vz = xLP(X_vz); // down
-		_pub_lpos.get().yaw = _sub_att.get().yaw;
+
+		_pub_lpos.get().yaw = _eul(2);
 		_pub_lpos.get().xy_global = _validXY;
 		_pub_lpos.get().z_global = _baroInitialized;
 		_pub_lpos.get().ref_timestamp = _timeStamp;
@@ -822,7 +823,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 		_pub_gpos.get().vel_n = xLP(X_vx);
 		_pub_gpos.get().vel_e = xLP(X_vy);
 		_pub_gpos.get().vel_d = xLP(X_vz);
-		_pub_gpos.get().yaw = _sub_att.get().yaw;
+		_pub_gpos.get().yaw = _eul(2);
 		_pub_gpos.get().eph = eph;
 		_pub_gpos.get().epv = epv;
 		_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);
@@ -874,18 +875,17 @@ void BlockLocalPositionEstimator::updateSSStates()
 {
 	// derivative of velocity is accelerometer acceleration
 	// (in input matrix) - bias (in body frame)
-	Matrix3f R_att(_sub_att.get().R);
-	_A(X_vx, X_bx) = -R_att(0, 0);
-	_A(X_vx, X_by) = -R_att(0, 1);
-	_A(X_vx, X_bz) = -R_att(0, 2);
-
-	_A(X_vy, X_bx) = -R_att(1, 0);
-	_A(X_vy, X_by) = -R_att(1, 1);
-	_A(X_vy, X_bz) = -R_att(1, 2);
-
-	_A(X_vz, X_bx) = -R_att(2, 0);
-	_A(X_vz, X_by) = -R_att(2, 1);
-	_A(X_vz, X_bz) = -R_att(2, 2);
+	_A(X_vx, X_bx) = -_R_att(0, 0);
+	_A(X_vx, X_by) = -_R_att(0, 1);
+	_A(X_vx, X_bz) = -_R_att(0, 2);
+
+	_A(X_vy, X_bx) = -_R_att(1, 0);
+	_A(X_vy, X_by) = -_R_att(1, 1);
+	_A(X_vy, X_bz) = -_R_att(1, 2);
+
+	_A(X_vz, X_bx) = -_R_att(2, 0);
+	_A(X_vz, X_by) = -_R_att(2, 1);
+	_A(X_vz, X_bz) = -_R_att(2, 2);
 }
 
 void BlockLocalPositionEstimator::updateSSParams()
@@ -929,11 +929,13 @@ void BlockLocalPositionEstimator::predict()
 	// state or covariance
 	if (!_validXY && !_validZ) { return; }
 
-	if (integrate && _sub_att.get().R_valid) {
-		Matrix3f R_att(_sub_att.get().R);
+	if (integrate && _sub_att.get().q_valid) {
+		matrix::Quaternion<float> q(&_sub_att.get().q[0]);
+		_eul = matrix::Euler<float>(q);
+		_R_att = matrix::Dcm<float>(q);
 		Vector3f a(_sub_sensor.get().accelerometer_m_s2);
 		// note, bias is removed in dynamics function
-		_u = R_att * a;
+		_u = _R_att * a;
 		_u(U_az) += 9.81f; // add g
 
 	} else {

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -401,6 +401,10 @@ private:
 	Vector<float, n_x>  _x; // state vector
 	Vector<float, n_u>  _u; // input vector
 	Matrix<float, n_x, n_x>  _P; // state covariance matrix
+
+	matrix::Dcm<float> _R_att;
+	Vector3f _eul;
+
 	Matrix<float, n_x, n_x>  _A; // dynamics matrix
 	Matrix<float, n_x, n_u>  _B; // input matrix
 	Matrix<float, n_u, n_u>  _R; // input covariance

src/modules/local_position_estimator/sensors/flow.cpp

@@ -64,7 +64,10 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 		return -1;
 	}
 
-	float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
+	matrix::Quaternion<float> q(&_sub_att.get().q[0]);
+	matrix::Euler<float> euler(q);
+
+	float d = agl() * cosf(euler(0)) * cosf(euler(1));
 
 	// optical flow in x, y axis
 	// TODO consider making flow scale a states of the kalman filter
@@ -98,8 +101,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 		0);
 
 	// rotation of flow from body to nav frame
-	Matrix3f R_nb(_sub_att.get().R);
-	Vector3f delta_n = R_nb * delta_b;
+	Vector3f delta_n = _R_att * delta_b;
 
 	// imporant to timestamp flow even if distance is bad
 	_time_last_flow = _timeStamp;

src/modules/local_position_estimator/sensors/lidar.cpp

@@ -65,6 +65,11 @@ void BlockLocalPositionEstimator::lidarCorrect()
 
 	if (lidarMeasure(y) != OK) { return; }
 
+	// account for leaning
+	y(0) = y(0) *
+	       cosf(_eul.phi()) *
+	       cosf(_eul.theta());
+
 	// measurement matrix
 	Matrix<float, n_y_lidar, n_x> C;
 	C.setZero();

src/modules/local_position_estimator/sensors/sonar.cpp

@@ -67,8 +67,8 @@ int BlockLocalPositionEstimator::sonarMeasure(Vector<float, n_y_sonar> &y)
 	_time_last_sonar = _timeStamp;
 	y.setZero();
 	y(0) = (d + _sonar_z_offset.get()) *
-	       cosf(_sub_att.get().roll) *
-	       cosf(_sub_att.get().pitch);
+	       cosf(_eul(0)) *
+	       cosf(_eul(1));
 	return OK;
 }