diff --git a/src/lib/matrix b/src/lib/matrix
index f4243160e2..41a1cc7583 160000
--- a/src/lib/matrix
+++ b/src/lib/matrix
@@ -1 +1 @@
-Subproject commit f4243160e2c77eb8034a35fe42924d59a39319da
+Subproject commit 41a1cc7583253f3063ab4c626c40e7bc2a0bc868
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index c9aa58e797..89a8b20e57 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -849,7 +849,20 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	q.normalize();
 	qd.normalize();
 
-	/* full quaternion attitude control, qe is rotation from q to qd */
+
+	/* calculate reduced attitude which we would command if we about the vehicle's yaw */
+	Vector3f e_z = q.dcm_z();
+	Vector3f e_z_d = qd.dcm_z();
+	Quatf qd_red(e_z, e_z_d);
+	qd_red *= q;
+
+	/* mix full and reduced desired attitude */
+	Quatf q_mix = qd_red.inversed() * qd;
+	q_mix *= math::sign(q_mix(0));
+	qd = qd_red * Quatf(cosf(yaw_w * acosf(q_mix(0))), 0, 0, sinf(yaw_w * asinf(q_mix(3))));
+
+
+	/* quaternion attitude control law, qe is rotation from q to qd */
 	Quatf qe = q.inversed() * qd;
 
 	/* using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)