diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index a30e483703..727407e857 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -799,6 +799,11 @@ float Aircraft::filtered_idx(float v, uint8_t idx)
     }
     const float cutoff = 1.0f / (2 * M_PI * sitl->servo_speed);
     servo_filter[idx].set_cutoff_frequency(cutoff);
+
+    if (idx >= ARRAY_SIZE(servo_filter)) {
+        AP_HAL::panic("Attempt to filter invalid servo at offset %u", (unsigned)idx);
+    }
+
     return servo_filter[idx].apply(v, frame_time_us * 1.0e-6f);
 }