Joseph Roberts / filter

a floating median filter to filter floating point data such as analog inputs

Dependents:   Quadcopter_mk2

Fork of filter by Ad van der Weiden

filter.cpp@1:9ce370b360ba, 2015-04-01 (annotated)

Committer:
joe4465
Date:
Wed Apr 01 11:18:55 2015 +0000
Revision:
1:9ce370b360ba
Parent:
0:46a72e790df8 
...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
networker 0:46a72e790df8 1 #include <float.h>
networker 0:46a72e790df8 2 #include "filter.h"
networker 0:46a72e790df8 3
joe4465 1:9ce370b360ba 4 filter::filter(int window): N(window) {
networker 0:46a72e790df8 5 big = new bool[N];
joe4465 1:9ce370b360ba 6 val = new double[N];
networker 0:46a72e790df8 7 big = new bool[N];
networker 0:46a72e790df8 8 i = 0;
networker 0:46a72e790df8 9 for (int j = 0; j < N; j++) {
networker 0:46a72e790df8 10 val[j] = 0;
networker 0:46a72e790df8 11 big[j] = j > N/2;
networker 0:46a72e790df8 12 }
networker 0:46a72e790df8 13 med = 0;
networker 0:46a72e790df8 14 median=0;
networker 0:46a72e790df8 15 }
networker 0:46a72e790df8 16
joe4465 1:9ce370b360ba 17 int filter::findmax() {
joe4465 1:9ce370b360ba 18 double m = -FLT_MAX;
networker 0:46a72e790df8 19 int n = -1;
networker 0:46a72e790df8 20 for (int j = 0; j < N; j++) {
networker 0:46a72e790df8 21 if (j == med) continue;
networker 0:46a72e790df8 22 if (!big[j]) { //find max
networker 0:46a72e790df8 23 if (val[j] > m) {
networker 0:46a72e790df8 24 m = val[j];
networker 0:46a72e790df8 25 n = j;
networker 0:46a72e790df8 26 }
networker 0:46a72e790df8 27 }
networker 0:46a72e790df8 28 }
networker 0:46a72e790df8 29 return n;
networker 0:46a72e790df8 30 }
networker 0:46a72e790df8 31
joe4465 1:9ce370b360ba 32 int filter::findmin() {
joe4465 1:9ce370b360ba 33 double m = FLT_MAX;
networker 0:46a72e790df8 34 int n = -1;
networker 0:46a72e790df8 35 for (int j = 0; j < N; j++) {
networker 0:46a72e790df8 36 if (big[j]) { //find min
networker 0:46a72e790df8 37 if (val[j] < m) {
networker 0:46a72e790df8 38 m = val[j];
networker 0:46a72e790df8 39 n = j;
networker 0:46a72e790df8 40 }
networker 0:46a72e790df8 41 }
networker 0:46a72e790df8 42 }
networker 0:46a72e790df8 43 return n;
networker 0:46a72e790df8 44 }
networker 0:46a72e790df8 45
joe4465 1:9ce370b360ba 46 double filter::process(double in) {
networker 0:46a72e790df8 47 //the value at position 'i' is to be replaced by 'in' and the new median is computed
networker 0:46a72e790df8 48 //var 'median' refers to the old median
networker 0:46a72e790df8 49 // val[j] <= median <= val[k]
networker 0:46a72e790df8 50 //by convention the mediam is considered small
networker 0:46a72e790df8 51 val[i] = in;
networker 0:46a72e790df8 52 if (i == med) { //the median itself is removed (not the value but the actual sample)
networker 0:46a72e790df8 53 if (in <= median) { //the new value is smaller than or equal to the old median and may be the new median
networker 0:46a72e790df8 54 med = -1; //hack to include the median cell in the comparison
networker 0:46a72e790df8 55 med = findmax(); //the largest small value is the new median
networker 0:46a72e790df8 56 } else { //the new value is larger than the old median and may be the new median
networker 0:46a72e790df8 57 big[i] = true; //add the new val to the big set, which is now 1 too large
networker 0:46a72e790df8 58 med = findmin();
networker 0:46a72e790df8 59 big[med] = false;
networker 0:46a72e790df8 60 }
networker 0:46a72e790df8 61 } else if (!big[i]) {//old value is removed from small values
networker 0:46a72e790df8 62 if (in <= median) {
networker 0:46a72e790df8 63 //replace small with small, median not affected
networker 0:46a72e790df8 64 } else { //the new value is large
networker 0:46a72e790df8 65 big[i] = true;
networker 0:46a72e790df8 66 med = findmin();
networker 0:46a72e790df8 67 big[med] = false;
networker 0:46a72e790df8 68 }
networker 0:46a72e790df8 69 } else { //old value is large
networker 0:46a72e790df8 70 if (in <= median) { //but the new value is small
networker 0:46a72e790df8 71 big[i] = false;
networker 0:46a72e790df8 72 big[med] = true;
networker 0:46a72e790df8 73 med = findmax();
networker 0:46a72e790df8 74 } else {//new value is also large
networker 0:46a72e790df8 75 //replace large with large, median not affected
networker 0:46a72e790df8 76 }
networker 0:46a72e790df8 77 }
networker 0:46a72e790df8 78 if (++i >= N) i = 0;
networker 0:46a72e790df8 79 median = val[med];
networker 0:46a72e790df8 80 return median;
networker 0:46a72e790df8 81 }
joe4465 1:9ce370b360ba 82
joe4465 1:9ce370b360ba 83 double filter::getMedian()
joe4465 1:9ce370b360ba 84 {
joe4465 1:9ce370b360ba 85 return median;
joe4465 1:9ce370b360ba 86 }