diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
index 62ffed5eec..419b97a9c0 100644
--- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
@@ -335,8 +335,10 @@ void VehicleAngularVelocity::DisableDynamicNotchEscRpm()
 
 	// device id mismatch, disable all
 	for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
-		for (int axis = 0; axis < 3; axis++) {
-			dnf[axis].setParameters(0, 0, 0);
+		for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
+			for (int axis = 0; axis < 3; axis++) {
+				dnf[harmonic][axis].setParameters(0, 0, 0);
+			}
 		}
 	}
 
@@ -376,20 +378,27 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
 				if ((esc_status.esc[i].timestamp != 0) && ((_timestamp_sample_last - esc_status.esc[i].timestamp) < 1_s)
 				    && (esc_status.esc[i].esc_rpm > MIN_ESC_RPM)) {
 
-					// TODO: rotor frequency, blade pass frequency, harmonics
-					const float frequency_hz = static_cast<float>(esc_status.esc[i].esc_rpm) / 60.f;
+					const float esc_hz = static_cast<float>(esc_status.esc[i].esc_rpm) / 60.f;
 
-					for (int axis = 0; axis < 3; axis++) {
-						auto &dnf = _dynamic_notch_filter_esc_rpm[i][axis];
-						const float change_percent = fabsf(dnf.getNotchFreq() - frequency_hz) / frequency_hz;
+					for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
+						const float frequency_hz = esc_hz * (harmonic + 1);
+
+						auto &dnf0 = _dynamic_notch_filter_esc_rpm[i][harmonic][0];
+						const float change_percent = fabsf(dnf0.getNotchFreq() - frequency_hz) / frequency_hz;
 
 						if (change_percent > 0.001f) {
 							// peak frequency changed by at least 0.1%
-							dnf.setParameters(_filter_sample_rate_hz, frequency_hz, 1.f); // TODO: configurable bandwidth
+							for (int axis = 0; axis < 3; axis++) {
+								auto &dnf = _dynamic_notch_filter_esc_rpm[i][harmonic][axis];
+								dnf.setParameters(_filter_sample_rate_hz, frequency_hz, 1.f); // TODO: configurable bandwidth
+							}
 
 							// only reset if there's sufficient change (> 1%)
 							if ((change_percent > 0.01f) && (_last_scale > 0.f)) {
-								dnf.reset(_angular_velocity(axis) / _last_scale);
+								for (int axis = 0; axis < 3; axis++) {
+									auto &dnf = _dynamic_notch_filter_esc_rpm[i][harmonic][axis];
+									dnf.reset(_angular_velocity(axis) / _last_scale);
+								}
 							}
 
 							perf_count(_dynamic_notch_filter_esc_rpm_update_perf);
@@ -399,10 +408,11 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
 					_dynamic_notch_esc_rpm_available = true;
 
 				} else {
-					// disable this notch filter
-					for (int axis = 0; axis < 3; axis++) {
-						auto &dnf = _dynamic_notch_filter_esc_rpm[i][axis];
-						dnf.setParameters(0, 0, 0);
+					// disable all notch filters for this ESC
+					for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
+						for (int axis = 0; axis < 3; axis++) {
+							_dynamic_notch_filter_esc_rpm[i][harmonic][axis].setParameters(0, 0, 0);
+						}
 					}
 				}
 			}
@@ -525,8 +535,13 @@ void VehicleAngularVelocity::Run()
 						perf_begin(_dynamic_notch_filter_esc_rpm_perf);
 
 						for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
-							if (dnf[axis].getNotchFreq() > 0.f) {
-								dnf[axis].applyDF1(data, N);
+							for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
+								if (dnf[harmonic][axis].getNotchFreq() > 0.f) {
+									dnf[harmonic][axis].applyDF1(data, N);
+
+								} else {
+									break;
+								}
 							}
 						}
 
@@ -621,8 +636,13 @@ void VehicleAngularVelocity::Run()
 					perf_begin(_dynamic_notch_filter_esc_rpm_perf);
 
 					for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
-						if (dnf[axis].getNotchFreq() > 0.f) {
-							dnf[axis].applyDF1(&angular_velocity(axis), 1);
+						for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
+							if (dnf[harmonic][axis].getNotchFreq() > 0.f) {
+								dnf[harmonic][axis].applyDF1(&angular_velocity(axis), 1);
+
+							} else {
+								break;
+							}
 						}
 					}
 
diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
index 39ca082d01..8bd7636e0e 100644
--- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
+++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
@@ -133,10 +133,12 @@ private:
 	};
 
 	static constexpr int MAX_NUM_ESC_RPM = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]);
+	static constexpr int MAX_NUM_ESC_RPM_HARMONICS = 3;
+
 	static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(
 				sensor_gyro_fft_s::peak_frequencies_x[0]);
 
-	math::NotchFilterArray<float> _dynamic_notch_filter_esc_rpm[MAX_NUM_ESC_RPM][3] {};
+	math::NotchFilterArray<float> _dynamic_notch_filter_esc_rpm[MAX_NUM_ESC_RPM][MAX_NUM_ESC_RPM_HARMONICS][3] {};
 	math::NotchFilterArray<float> _dynamic_notch_filter_fft[MAX_NUM_FFT_PEAKS][3] {};
 
 	perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};