Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : /// @file       Derivative.cpp
      17             : /// @brief      A class to implement a derivative (slope) filter
      18             : /// See http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
      19             : //
      20             : #include <inttypes.h>
      21             : #include <AP_Math/AP_Math.h>
      22             : #include "Filter.h"
      23             : #include "DerivativeFilter.h"
      24             : 
      25             : 
      26             : template <class T,  uint8_t FILTER_SIZE>
      27    20748956 : void DerivativeFilter<T,FILTER_SIZE>::update(T sample, uint32_t timestamp)
      28             : {
      29    20748956 :     uint8_t i = FilterWithBuffer<T,FILTER_SIZE>::sample_index;
      30             :     uint8_t i1;
      31    20748956 :     if (i == 0) {
      32     2965199 :         i1 = FILTER_SIZE-1;
      33             :     } else {
      34    17783757 :         i1 = i-1;
      35             :     }
      36    20748956 :     if (_timestamps[i1] == timestamp) {
      37             :         // this is not a new timestamp - ignore
      38       73440 :         return;
      39             :     }
      40             : 
      41             :     // add timestamp before we apply to FilterWithBuffer
      42    20675516 :     _timestamps[i] = timestamp;
      43             : 
      44             :     // call parent's apply function to get the sample into the array
      45    20675516 :     FilterWithBuffer<T,FILTER_SIZE>::apply(sample);
      46             : 
      47    20675516 :     _new_data = true;
      48             : }
      49             : 
      50             : 
      51             : template <class T,  uint8_t FILTER_SIZE>
      52    44779706 : float DerivativeFilter<T,FILTER_SIZE>::slope(void)
      53             : {
      54    44779706 :     if (!_new_data) {
      55    23866916 :         return _last_slope;
      56             :     }
      57             : 
      58    20912790 :     float result = 0;
      59             : 
      60             :     // use f() to make the code match the maths a bit better. Note
      61             :     // that unlike an average filter, we care about the order of the elements
      62             : #define f(i) FilterWithBuffer<T,FILTER_SIZE>::samples[(((FilterWithBuffer<T,FILTER_SIZE>::sample_index-1)+i+1)+3*FILTER_SIZE/2) % FILTER_SIZE]
      63             : #define x(i) _timestamps[(((FilterWithBuffer<T,FILTER_SIZE>::sample_index-1)+i+1)+3*FILTER_SIZE/2) % FILTER_SIZE]
      64             : 
      65    20912790 :     if (_timestamps[FILTER_SIZE-1] == _timestamps[FILTER_SIZE-2]) {
      66             :         // we haven't filled the buffer yet - assume zero derivative
      67      282587 :         return 0;
      68             :     }
      69             : 
      70             :     // N in the paper is FILTER_SIZE
      71             :     switch (FILTER_SIZE) {
      72           0 :     case 5:
      73           0 :         result = 2*2*(f(1) - f(-1)) / (x(1) - x(-1))
      74           0 :                  + 4*1*(f(2) - f(-2)) / (x(2) - x(-2));
      75           0 :         result /= 8;
      76           0 :         break;
      77    20630203 :     case 7:
      78    20630203 :         result = 2*5*(f(1) - f(-1)) / (x(1) - x(-1))
      79    20630203 :                  + 4*4*(f(2) - f(-2)) / (x(2) - x(-2))
      80    20630203 :                  + 6*1*(f(3) - f(-3)) / (x(3) - x(-3));
      81    20630203 :         result /= 32;
      82    20630203 :         break;
      83           0 :     case 9:
      84           0 :         result = 2*14*(f(1) - f(-1)) / (x(1) - x(-1))
      85           0 :                  + 4*14*(f(2) - f(-2)) / (x(2) - x(-2))
      86           0 :                  + 6* 6*(f(3) - f(-3)) / (x(3) - x(-3))
      87           0 :                  + 8* 1*(f(4) - f(-4)) / (x(4) - x(-4));
      88           0 :         result /= 128;
      89           0 :         break;
      90           0 :     case 11:
      91           0 :         result =  2*42*(f(1) - f(-1)) / (x(1) - x(-1))
      92           0 :                  +  4*48*(f(2) - f(-2)) / (x(2) - x(-2))
      93           0 :                  +  6*27*(f(3) - f(-3)) / (x(3) - x(-3))
      94           0 :                  +  8* 8*(f(4) - f(-4)) / (x(4) - x(-4))
      95           0 :                  + 10* 1*(f(5) - f(-5)) / (x(5) - x(-5));
      96           0 :         result /= 512;
      97           0 :         break;
      98             :     default:
      99             :         result = 0;
     100             :         break;
     101             :     }
     102             : 
     103             :     // cope with numerical errors
     104    20630203 :     if (isnan(result) || isinf(result)) {
     105           0 :         result = 0;
     106             :     }
     107             : 
     108    20630203 :     _new_data = false;
     109    20630203 :     _last_slope = result;
     110             : 
     111    20630203 :     return result;
     112             : }
     113             : 
     114             : // reset - clear all samples
     115             : template <class T, uint8_t FILTER_SIZE>
     116        1285 : void DerivativeFilter<T,FILTER_SIZE>::reset(void)
     117             : {
     118             :     // call parent's apply function to get the sample into the array
     119        1285 :     FilterWithBuffer<T,FILTER_SIZE>::reset();
     120        1285 : }
     121             : 
     122             : // add new instances as needed here
     123             : template void DerivativeFilter<float,5>::update(float sample, uint32_t timestamp);
     124             : template float DerivativeFilter<float,5>::slope(void);
     125             : template void DerivativeFilter<float,5>::reset(void);
     126             : 
     127             : template void DerivativeFilter<float,7>::update(float sample, uint32_t timestamp);
     128             : template float DerivativeFilter<float,7>::slope(void);
     129             : template void DerivativeFilter<float,7>::reset(void);
     130             : 
     131             : template void DerivativeFilter<float,9>::update(float sample, uint32_t timestamp);
     132             : template float DerivativeFilter<float,9>::slope(void);
     133             : template void DerivativeFilter<float,9>::reset(void);
     134             : 
     135             : template void DerivativeFilter<float,11>::update(float sample, uint32_t timestamp);
     136             : template float DerivativeFilter<float,11>::slope(void);
     137             : template void DerivativeFilter<float,11>::reset(void);
     138             : 
     139             : 
     140             :