Add notch filter for the gyro

flix/cli.ino

@@ -6,7 +6,7 @@
 #include "pid.h"
 #include "vector.h"
 #include "util.h"
-#include "lpf.h"
+#include "filter.h"
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
 extern const int RAW, ACRO, STAB, AUTO;

flix/control.ino

@@ -6,7 +6,7 @@
 #include "vector.h"
 #include "quaternion.h"
 #include "pid.h"
-#include "lpf.h"
+#include "filter.h"
 #include "util.h"
 
 #define PITCHRATE_P 0.05

flix/estimate.ino

@@ -5,7 +5,7 @@
 
 #include "quaternion.h"
 #include "vector.h"
-#include "lpf.h"
+#include "filter.h"
 #include "util.h"
 
 Vector rates; // estimated angular rates, rad/s
@@ -15,6 +15,12 @@ bool landed;
 float accWeight = 0.003;
 float levelWeight = 0.0002;
 LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
+NotchFilter<Vector> ratesNotch(382, 40);
+
+void setupEstimate() {
+	print("Setup estimation\n");
+	ratesNotch.reset();
+}
 
 void estimate() {
 	applyGyro();
@@ -25,6 +31,7 @@ void estimate() {
 void applyGyro() {
 	// filter gyro to get angular rates
 	rates = ratesFilter.update(gyro);
+	rates = ratesNotch.update(rates);
 
 	// apply rates to attitude
 	attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(rates * dt));

flix/filter.h

@@ -0,0 +1,98 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+// Low pass and notch filters
+
+#pragma once
+
+template <typename T> // Using template to make the filter usable for scalar and vector values
+class LowPassFilter {
+public:
+	float alpha; // smoothing constant, 1 means filter disabled
+	T output;
+
+	LowPassFilter(float alpha): alpha(alpha) {};
+
+	T update(const T input) {
+		if (!init) {
+			init = true;
+			return output = input;
+		}
+		return output += alpha * (input - output);
+	}
+
+	void setCutOffFrequency(float cutOffFreq, float dt) {
+		alpha = 1 - exp(-2 * PI * cutOffFreq * dt);
+	}
+
+	void reset() {
+		init = false;
+	}
+
+private:
+	bool init = false;
+};
+
+template <typename T>
+class NotchFilter {
+public:
+	float frequency;
+	float bandwidth;
+	T output;
+
+	NotchFilter(float frequency, float bandwidth): frequency(frequency), bandwidth(bandwidth) {
+		reset();
+	};
+
+	T update(const T input) {
+		if (frequency <= 0 || bandwidth <= 0) return input;
+
+		if (!init) {
+			init = true;
+			x1 = x2 = input;
+			y1 = y2 = input;
+			return output = input;
+		}
+
+		output = b0 * input + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2;
+
+		x2 = x1;
+		x1 = input;
+		y2 = y1;
+		y1 = output;
+
+		return output;
+	}
+
+	void reset() {
+		const float dt = 0.001f;
+		float f = frequency;
+		float bw = bandwidth;
+		if (f < 0) f = 0;
+		if (bw < 1e-6f) bw = 1e-6f;
+
+		float q = f / bw;
+		if (q < 1e-3f) q = 1e-3f;
+
+		const float w0 = 2.0f * PI * f * dt;
+		const float c = cos(w0);
+		const float s = sin(w0);
+		const float alpha = s / (2.0f * q);
+
+		const float a0 = 1.0f + alpha;
+		const float invA0 = 1.0f / a0;
+
+		b0 = 1.0f * invA0;
+		b1 = -2.0f * c * invA0;
+		b2 = 1.0f * invA0;
+		a1 = -2.0f * c * invA0;
+		a2 = (1.0f - alpha) * invA0;
+
+		init = false;
+	}
+
+private:
+	float b0, b1, b2, a1, a2;
+	T x1, x2, y1, y2;
+	bool init = false;
+};

flix/flix.ino

@@ -26,6 +26,7 @@ void setup() {
 	setupWiFi();
 	setupIMU();
 	setupRC();
+	setupEstimate();
 	setupLog();
 	setLED(false);
 	print("Initializing complete\n");

flix/imu.ino

@@ -6,7 +6,7 @@
 #include <SPI.h>
 #include <FlixPeriph.h>
 #include "vector.h"
-#include "lpf.h"
+#include "filter.h"
 #include "util.h"
 
 MPU9250 imu(SPI);

flix/lpf.h

@@ -1,34 +0,0 @@
-// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
-// Repository: https://github.com/okalachev/flix
-
-// Low pass filter implementation
-
-#pragma once
-
-template <typename T> // Using template to make the filter usable for scalar and vector values
-class LowPassFilter {
-public:
-	float alpha; // smoothing constant, 1 means filter disabled
-	T output;
-
-	LowPassFilter(float alpha): alpha(alpha) {};
-
-	T update(const T input) {
-		if (!init) {
-			init = true;
-			return output = input;
-		}
-		return output += alpha * (input - output);
-	}
-
-	void setCutOffFrequency(float cutOffFreq, float dt) {
-		alpha = 1 - exp(-2 * PI * cutOffFreq * dt);
-	}
-
-	void reset() {
-		init = false;
-	}
-
-private:
-	bool init = false;
-};

flix/parameters.ino

@@ -74,6 +74,8 @@ Parameter parameters[] = {
 	{"EST_ACC_WEIGHT", &accWeight},
 	{"EST_LVL_WEIGHT", &levelWeight},
 	{"EST_RATES_LPF_A", &ratesFilter.alpha},
+	{"EST_RATES_NF_F", &ratesNotch.frequency, setupEstimate},
+	{"EST_RATES_NF_BW", &ratesNotch.bandwidth, setupEstimate},
 	// motors
 	{"MOT_PIN_FL", &motorPins[MOTOR_FRONT_LEFT], setupMotors},
 	{"MOT_PIN_FR", &motorPins[MOTOR_FRONT_RIGHT], setupMotors},

flix/pid.h

@@ -5,7 +5,7 @@
 
 #pragma once
 
-#include "lpf.h"
+#include "filter.h"
 
 class PID {
 public:

flix/power.ino

@@ -5,7 +5,7 @@
 
 #include <soc/soc.h>
 #include <soc/rtc_cntl_reg.h>
-#include "lpf.h"
+#include "filter.h"
 #include "util.h"
 
 float voltage = NAN;

gazebo/flix.h

@@ -9,7 +9,7 @@
 #include "quaternion.h"
 #include "Arduino.h"
 #include "wifi.h"
-#include "lpf.h"
+#include "filter.h"
 
 extern float t, dt;
 extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;

gazebo/simulator.cpp

@@ -23,7 +23,7 @@
 #include "estimate.ino"
 #include "safety.ino"
 #include "log.ino"
-#include "lpf.h"
+#include "filter.h"
 #include "mavlink.ino"
 #include "motors.ino"
 #include "parameters.ino"