From a07e280eda81cdb718d56b613267ade9da4dab08 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Jul 2012 13:00:08 +1000 Subject: [PATCH] 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 | 91 ++++++++++--------- libraries/Filter/DerivativeFilter.h | 18 ++-- libraries/Filter/FilterWithBuffer.h | 3 +- .../Filter/examples/Derivative/Derivative.pde | 69 ++++++++++++++ libraries/Filter/examples/Derivative/Makefile | 4 + 5 files changed, 133 insertions(+), 52 deletions(-) create mode 100644 libraries/Filter/examples/Derivative/Derivative.pde create mode 100644 libraries/Filter/examples/Derivative/Makefile diff --git a/libraries/Filter/DerivativeFilter.cpp b/libraries/Filter/DerivativeFilter.cpp index 810e84833b..351ee7f0cd 100644 --- a/libraries/Filter/DerivativeFilter.cpp +++ b/libraries/Filter/DerivativeFilter.cpp @@ -15,75 +15,80 @@ #include #include -template -float DerivativeFilter::apply(T sample) +template +float DerivativeFilter::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::sample_index] = timestamp; + // call parent's apply function to get the sample into the array FilterWithBuffer::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::samples[(((FilterWithBuffer::sample_index-1)-i)+FILTER_SIZE) % FILTER_SIZE] + #define f(i) FilterWithBuffer::samples[(((FilterWithBuffer::sample_index-1)+i)+3*FILTER_SIZE/2) % FILTER_SIZE] + #define x(i) _timestamps[(((FilterWithBuffer::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 -void DerivativeFilter::reset(void) +template +void DerivativeFilter::reset(void) { // call parent's apply function to get the sample into the array FilterWithBuffer::reset(); - - // clear our variable - _num_samples = 0; } // add new instances as needed here -template float DerivativeFilter::apply(float sample); -template void DerivativeFilter::reset(void); +template float DerivativeFilter::apply(float sample, uint32_t timestamp); +template void DerivativeFilter::reset(void); + +template float DerivativeFilter::apply(float sample, uint32_t timestamp); +template void DerivativeFilter::reset(void); + +template float DerivativeFilter::apply(float sample, uint32_t timestamp); +template void DerivativeFilter::reset(void); + + + diff --git a/libraries/Filter/DerivativeFilter.h b/libraries/Filter/DerivativeFilter.h index 1705e3f5b2..5a6ce1171e 100644 --- a/libraries/Filter/DerivativeFilter.h +++ b/libraries/Filter/DerivativeFilter.h @@ -18,27 +18,31 @@ #include // 1st parameter is the type of data being filtered. -// 2nd parameter is a larger data type used during summation to prevent overflows -// 3rd parameter is the number of elements in the filter -template +// 2nd parameter is the number of elements in the filter +template class DerivativeFilter : public FilterWithBuffer { public: // constructor - DerivativeFilter() : FilterWithBuffer(), _num_samples(0) {}; + DerivativeFilter() : FilterWithBuffer() {}; // 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 DerivativeFilterFloat_Size9; +typedef DerivativeFilter DerivativeFilterFloat_Size5; +typedef DerivativeFilter DerivativeFilterFloat_Size7; +typedef DerivativeFilter DerivativeFilterFloat_Size9; #endif // Derivative_h diff --git a/libraries/Filter/FilterWithBuffer.h b/libraries/Filter/FilterWithBuffer.h index a39f5e109e..df2fcdd8a0 100644 --- a/libraries/Filter/FilterWithBuffer.h +++ b/libraries/Filter/FilterWithBuffer.h @@ -35,10 +35,9 @@ class FilterWithBuffer : public Filter // 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 diff --git a/libraries/Filter/examples/Derivative/Derivative.pde b/libraries/Filter/examples/Derivative/Derivative.pde new file mode 100644 index 0000000000..ffe94ee161 --- /dev/null +++ b/libraries/Filter/examples/Derivative/Derivative.pde @@ -0,0 +1,69 @@ +/* + Example sketch to demonstrate use of DerivativeFilter library. +*/ + +#include +#include +#include +#include +#include + +#ifdef DESKTOP_BUILD +// all of this is needed to build with SITL +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +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 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); +} + + diff --git a/libraries/Filter/examples/Derivative/Makefile b/libraries/Filter/examples/Derivative/Makefile new file mode 100644 index 0000000000..fcdc8ff8fe --- /dev/null +++ b/libraries/Filter/examples/Derivative/Makefile @@ -0,0 +1,4 @@ +include ../../../AP_Common/Arduino.mk + +sitl: + make -f ../../../../libraries/Desktop/Desktop.mk