Filter: cope with non-uniform time steps in the DerivativeFilter

this helps with the barometer a lot, as the timing is quite variable

libraries/Filter/DerivativeFilter.cpp

@@ -15,75 +15,80 @@
 #include <Filter.h>
 #include <DerivativeFilter.h>
 
-template <class T, class U, uint8_t FILTER_SIZE>
-float DerivativeFilter<T,U,FILTER_SIZE>::apply(T sample)
+template <class T,  uint8_t FILTER_SIZE>
+float DerivativeFilter<T,FILTER_SIZE>::apply(T sample, uint32_t timestamp)
 {
 	float result = 0;
-    uint32_t tnow = millis();
-    float deltat = (tnow - _last_time) * 1.0e-3;
-    _last_time = tnow;
+
+    // add timestamp before we apply to FilterWithBuffer
+    _timestamps[FilterWithBuffer<T,FILTER_SIZE>::sample_index] = timestamp;
+
 	// call parent's apply function to get the sample into the array
 	FilterWithBuffer<T,FILTER_SIZE>::apply(sample);
 
-	// increment the number of samples so far
-	_num_samples++;
-	if( _num_samples > FILTER_SIZE || _num_samples == 0 )
-		_num_samples = FILTER_SIZE;
-
     // use f() to make the code match the maths a bit better. Note
     // that unlike an average filter, we care about the order of the elements
-    #define f(i) FilterWithBuffer<T,FILTER_SIZE>::samples[(((FilterWithBuffer<T,FILTER_SIZE>::sample_index-1)-i)+FILTER_SIZE) % FILTER_SIZE]
+    #define f(i) FilterWithBuffer<T,FILTER_SIZE>::samples[(((FilterWithBuffer<T,FILTER_SIZE>::sample_index-1)+i)+3*FILTER_SIZE/2) % FILTER_SIZE]
+    #define x(i) _timestamps[(((FilterWithBuffer<T,FILTER_SIZE>::sample_index-1)+i)+3*FILTER_SIZE/2) % FILTER_SIZE]
 
-    // N in the paper is _num_samples-1
-    switch (FILTER_SIZE-1) {
-    case 1:
-        result = f(0) - f(1);
-        break;
-    case 2:
-        result = f(0) - f(2);
-        break;
-    case 3:
-        result = f(0) + f(1) - f(2) - f(3);
-        break;
-    case 4:
-        result = f(0) + 2*f(1) - 2*f(3) - f(4);
-        break;
+    if (_timestamps[FILTER_SIZE-1] == _timestamps[FILTER_SIZE-2]) {
+        // we haven't filled the buffer yet - assume zero derivative
+        return 0;
+    }
+
+    // N in the paper is FILTER_SIZE
+    switch (FILTER_SIZE) {
     case 5:
-        result = f(0) + 3*f(1) + 2*f(2) - 2*f(3) - 3*f(4) - f(5);
-        break;
-    case 6:
-        result = f(0) + 4*f(1) + 5*f(2) - 5*f(4) - 4*f(5) - f(6);
+        result = 2*2*(f(1) - f(-1)) / (x(1) - x(-1))
+               + 4*1*(f(2) - f(-2)) / (x(2) - x(-2));
+        result /= 8;
         break;
     case 7:
-        result = f(0) + 5*f(1) + 9*f(2) + 5*f(3) - 5*f(4) - 9*f(5) - 5*f(6) - f(7);
-        break;
-    case 8:
-        result = f(0) + 6*f(1) + 14*f(2) + 14*f(3) - 14*f(5) - 14*f(6) - 6*f(7) - f(8);
+        result = 2*5*(f(1) - f(-1)) / (x(1) - x(-1))
+               + 4*4*(f(2) - f(-2)) / (x(2) - x(-2)) 
+               + 6*1*(f(3) - f(-3)) / (x(3) - x(-3));
+        result /= 32;
         break;
     case 9:
-        result = f(0) + 7*f(1) + 20*f(2) + 28*f(3) + 14*f(4) - 14*f(5) - 28*f(6) - 20*f(7) - 7*f(8) - f(9);
+        result = 2*14*(f(1) - f(-1)) / (x(1) - x(-1))
+               + 4*14*(f(2) - f(-2)) / (x(2) - x(-2)) 
+               + 6* 6*(f(3) - f(-3)) / (x(3) - x(-3));
+               + 8* 1*(f(4) - f(-4)) / (x(4) - x(-4));
+        result /= 128;
+        break;
+    case 11:
+        result =  2*42*(f(1) - f(-1)) / (x(1) - x(-1))
+               +  4*48*(f(2) - f(-2)) / (x(2) - x(-2)) 
+               +  6*27*(f(3) - f(-3)) / (x(3) - x(-3))
+               +  8* 8*(f(4) - f(-4)) / (x(4) - x(-4))
+               + 10* 1*(f(5) - f(-5)) / (x(5) - x(-5));
+        result /= 512;
         break;
     default:
         result = 0;
         break;
     }
 
-    result /= deltat * (1<<(uint16_t)(FILTER_SIZE-2));
-
     return result;
 }
 
 // reset - clear all samples
-template <class T, class U, uint8_t FILTER_SIZE>
-void DerivativeFilter<T,U,FILTER_SIZE>::reset(void)
+template <class T, uint8_t FILTER_SIZE>
+void DerivativeFilter<T,FILTER_SIZE>::reset(void)
 {
 	// call parent's apply function to get the sample into the array
 	FilterWithBuffer<T,FILTER_SIZE>::reset();
-
-	// clear our variable
-	_num_samples = 0;
 }
 
 // add new instances as needed here
-template float DerivativeFilter<float,float,9>::apply(float sample);
-template void DerivativeFilter<float,float,9>::reset(void);
+template float DerivativeFilter<float,5>::apply(float sample, uint32_t timestamp);
+template void DerivativeFilter<float,5>::reset(void);
+
+template float DerivativeFilter<float,7>::apply(float sample, uint32_t timestamp);
+template void DerivativeFilter<float,7>::reset(void);
+
+template float DerivativeFilter<float,9>::apply(float sample, uint32_t timestamp);
+template void DerivativeFilter<float,9>::reset(void);
+
+
+

libraries/Filter/DerivativeFilter.h

@@ -18,27 +18,31 @@
 #include <FilterWithBuffer.h>
 
 // 1st parameter <T> is the type of data being filtered.  
-// 2nd parameter <U> is a larger data type used during summation to prevent overflows
-// 3rd parameter <FILTER_SIZE> is the number of elements in the filter
-template <class T, class U, uint8_t FILTER_SIZE>
+// 2nd parameter <FILTER_SIZE> is the number of elements in the filter
+template <class T, uint8_t FILTER_SIZE>
 class DerivativeFilter : public FilterWithBuffer<T,FILTER_SIZE>
 {
   public:
 	// constructor
-	DerivativeFilter() : FilterWithBuffer<T,FILTER_SIZE>(), _num_samples(0) {};
+	DerivativeFilter() : FilterWithBuffer<T,FILTER_SIZE>() {};
 
 	// apply - Add a new raw value to the filter, retrieve the filtered result
-	virtual float apply(T sample);
+	virtual float apply(T sample, uint32_t timestamp);
 
 	// reset - clear the filter
 	virtual void reset();
 
   private:
     uint32_t _last_time;
-	uint8_t	_num_samples;	// the number of samples in the filter, maxes out at size of the filter
+
+    // microsecond timestamps for samples. This is needed
+    // to cope with non-uniform time spacing of the data
+    uint32_t _timestamps[FILTER_SIZE];
 };
 
-typedef DerivativeFilter<float,float,9> DerivativeFilterFloat_Size9;
+typedef DerivativeFilter<float,5> DerivativeFilterFloat_Size5;
+typedef DerivativeFilter<float,7> DerivativeFilterFloat_Size7;
+typedef DerivativeFilter<float,9> DerivativeFilterFloat_Size9;
 
 
 #endif // Derivative_h

libraries/Filter/FilterWithBuffer.h

@@ -35,10 +35,9 @@ class FilterWithBuffer : public Filter<T>
 	// get filter size
 	virtual uint8_t get_filter_size() { return FILTER_SIZE; };
 
+  protected:
 	T		samples[FILTER_SIZE];	// buffer of samples
 	uint8_t	sample_index;			// pointer to the next empty slot in the buffer
-
-  private:
 };
 
 // Typedef for convenience

libraries/Filter/examples/Derivative/Derivative.pde

@@ -0,0 +1,69 @@
+/*
+	Example sketch to demonstrate use of DerivativeFilter library.
+*/
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>
+#include <Filter.h>
+#include <DerivativeFilter.h>
+
+#ifdef DESKTOP_BUILD
+// all of this is needed to build with SITL
+#include <DataFlash.h>
+#include <APM_RC.h>
+#include <GCS_MAVLink.h>
+#include <Arduino_Mega_ISR_Registry.h>
+#include <AP_PeriodicProcess.h>
+#include <AP_ADC.h>
+#include <AP_Baro.h>
+#include <AP_Compass.h>
+#include <AP_GPS.h>
+#include <Filter.h>
+#include <SITL.h>
+#include <I2C.h>
+#include <SPI.h>
+#include <AP_Declination.h>
+Arduino_Mega_ISR_Registry isr_registry;
+AP_Baro_BMP085_HIL      barometer;
+AP_Compass_HIL     compass;
+SITL sitl;
+BetterStream *mavlink_comm_0_port;
+BetterStream *mavlink_comm_1_port;
+mavlink_system_t mavlink_system;
+#endif
+
+FastSerialPort0(Serial);        // FTDI/console
+
+DerivativeFilter<float,float,7> derivative;
+
+// setup routine
+void setup()
+{
+	// Open up a serial connection
+	Serial.begin(115200);
+	
+	// introduction
+	Serial.printf("ArduPilot DerivativeFilter test\n");
+}
+
+static float noise(void)
+{
+	return ((random() % 100)-50) * 0.001;
+}
+
+//Main loop where the action takes place
+void loop()
+{
+	delay(50);
+	float t = millis()*1.0e-3;
+	float s = sin(t);
+	//s += noise();
+	uint32_t t1 = micros();
+	float output = derivative.apply(s, t1) * 1.0e6;
+	uint32_t t2 = micros();
+	Serial.printf("cos(t)=%.2f filter=%.2f tdiff=%u\n",
+		      cos(t), output, t2-t1);
+}
+
+

libraries/Filter/examples/Derivative/Makefile

@@ -0,0 +1,4 @@
+include ../../../AP_Common/Arduino.mk
+
+sitl:
+	make -f ../../../../libraries/Desktop/Desktop.mk