Reduce angle drift by adding level correction to the estimator

Leverage a priori knowledge that the drone's average attitude is level.
Explanation: https://t.me/opensourcequadcopter/158.

flix/estimate.ino

@@ -13,11 +13,13 @@ Quaternion attitude; // estimated attitude
 bool landed;
 
 float accWeight = 0.003;
+float levelWeight = 0.0002;
 LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
 
 void estimate() {
 	applyGyro();
 	applyAcc();
+	applyLevel();
 }
 
 void applyGyro() {
@@ -42,3 +44,12 @@ void applyAcc() {
 	// apply correction
 	attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
 }
+
+void applyLevel() {
+	if (landed) return;
+
+	// assume the pilot keeps the drone more or less level in flight
+	Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
+	Vector correction = Vector::rotationVectorBetween(Vector(0, 0, 1), up) * levelWeight;
+	attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
+}

flix/parameters.ino

@@ -73,6 +73,7 @@ Parameter parameters[] = {
 	{"IMU_GYRO_BIAS_A", &gyroBiasFilter.alpha},
 	// estimate
 	{"EST_ACC_WEIGHT", &accWeight},
+	{"EST_LVL_WEIGHT", &levelWeight},
 	{"EST_RATES_LPF_A", &ratesFilter.alpha},
 	// motors
 	{"MOT_PIN_FL", &motorPins[MOTOR_FRONT_LEFT], setupMotors},

gazebo/flix.h

@@ -27,6 +27,7 @@ void step();
 void computeLoopRate();
 void applyGyro();
 void applyAcc();
+void applyLevel();
 void control();
 void interpretControls();
 void controlAttitude();