MavlinkSonarArray/sonarFilter.cpp at master · devbender/MavlinkSonarArray · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
/*  ======================================================================
 *  *** Mavlink Sonar Array ***
 *
 *  File: sonarFilter.cpp
 *  Description: Qwiic Sonar filtering class file
 *
 *  MIT License
 *  Copyright (c) 2021 Juan Benitez <juan.a.benitez(a)gmail.com>
 *
 *  ======================================================================
 */

#include "sonarFilter.h"

sonarFilter::sonarFilter(uint8_t _samples) {
      samples = _samples;
}


void sonarFilter::setSamples(uint8_t _samples) {
  samples = _samples;
}


void sonarFilter::setKFParams(float mea_e, float est_e, float q) {
  _err_measure=mea_e;
  _err_estimate=est_e;
  _q = q;
}


void sonarFilter::setWAParams(float _wp1, float _wp2, float _wp3) {
  wp1=_wp1;
  wp2=_wp2;
  wp3=_wp3;
}


float sonarFilter::updateKFEstimate(float mea) {

  _kalman_gain = _err_estimate/(_err_estimate + _err_measure);
  _current_estimate = _last_estimate + _kalman_gain * (mea - _last_estimate);
  _err_estimate =  (1.0 - _kalman_gain)*_err_estimate + fabs(_last_estimate-_current_estimate)*_q;
  _last_estimate=_current_estimate;

  return _current_estimate;
}


void sonarFilter::add(uint16_t newSample) {

  // store raw sample
  d_raw = newSample;

  // add sample to pool
  data.push_back( newSample );

  // delete oldest
  if( data.size() > samples )
    data.erase( data.begin() );


  // Average ----------------------------------
  uint16_t sumSamples = 0;

  for(auto& sample : data)
    sumSamples+=sample;

  d_maverage = round( sumSamples / data.size() );

  // Median -----------------------------------
  if(data.size() >= samples) {

    dataCopy = data;
    std::sort(dataCopy.begin(), dataCopy.end());
    d_median = dataCopy[ round(dataCopy.size()/2) ];
  }

  // Kalman -----------------------------------
  d_kalmanFilter = updateKFEstimate( newSample );

  // Weighted Avg -----------------------------
  d_wavg = round( (wp1)*d_maverage + (wp2)*d_median + (wp3)*d_kalmanFilter  );
}


uint16_t sonarFilter::raw() {
  return d_raw;
}


uint16_t sonarFilter::mavg() {
  return d_maverage;
}


uint16_t sonarFilter::wavg() {
  return d_wavg;
}


uint16_t sonarFilter::median() {
  return d_median;
}


uint16_t sonarFilter::kalman() {
  return d_kalmanFilter;
}