Project_WIPV_antiSlip / FILTER_LIB

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of FILTER_LIB by Project_WIPV_antiSlip

FILTER_LIB.h@9:5d6b06b34127, 2017-05-08 (annotated)

Committer:
benson516
Date:
Mon May 08 17:29:26 2017 +0000
Revision:
9:5d6b06b34127
Parent:
7:10df955a92d9 
Add MovingAverage

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 2:7275567b9c13 1 //
benson516 2:7275567b9c13 2 #ifndef PI
benson516 2:7275567b9c13 3 #define PI 3.1415926
benson516 2:7275567b9c13 4 #endif
benson516 2:7275567b9c13 5 //
benson516 0:63eb74dddbc9 6 #ifndef FILTER_LIB_H
benson516 0:63eb74dddbc9 7 #define FILTER_LIB_H
benson516 0:63eb74dddbc9 8 //
benson516 2:7275567b9c13 9 #include "IIR.h"
benson516 6:18dd3f9ac217 10 #include <math.h>
benson516 0:63eb74dddbc9 11 #include <vector>
benson516 2:7275567b9c13 12
benson516 0:63eb74dddbc9 13 using std::vector;
benson516 0:63eb74dddbc9 14
benson516 2:7275567b9c13 15 //--------------------LPF---------------------//
benson516 2:7275567b9c13 16 class LPF{ // Low-pass filter
benson516 2:7275567b9c13 17 public:
benson516 0:63eb74dddbc9 18 float output;
benson516 0:63eb74dddbc9 19
benson516 0:63eb74dddbc9 20 LPF(float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz"
benson516 0:63eb74dddbc9 21 float filter(float input);
benson516 0:63eb74dddbc9 22 void reset(float input);
benson516 0:63eb74dddbc9 23
benson516 0:63eb74dddbc9 24 private:
benson516 0:63eb74dddbc9 25 float Ts;
benson516 0:63eb74dddbc9 26 float cutOff_freq_Hz; // Hz
benson516 0:63eb74dddbc9 27 float alpha_Ts;
benson516 0:63eb74dddbc9 28 float One_alpha_Ts;
benson516 0:63eb74dddbc9 29
benson516 0:63eb74dddbc9 30 // Flag
benson516 0:63eb74dddbc9 31 bool Flag_Init;
benson516 0:63eb74dddbc9 32 };
benson516 0:63eb74dddbc9 33
benson516 2:7275567b9c13 34 //--------------------LPF_vector---------------------//
benson516 2:7275567b9c13 35 class LPF_vector{ // Vectorized low-pass filter
benson516 2:7275567b9c13 36 public:
benson516 1:ec51b9f84eee 37 vector<float> output;
benson516 1:ec51b9f84eee 38
benson516 1:ec51b9f84eee 39 LPF_vector(size_t dimension, float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz"
benson516 6:18dd3f9ac217 40 vector<float> filter(const vector<float> &input);
benson516 6:18dd3f9ac217 41 void reset(const vector<float> &input);
benson516 1:ec51b9f84eee 42
benson516 1:ec51b9f84eee 43 private:
benson516 1:ec51b9f84eee 44 size_t n;
benson516 1:ec51b9f84eee 45 float Ts;
benson516 1:ec51b9f84eee 46 float cutOff_freq_Hz; // Hz
benson516 1:ec51b9f84eee 47 float alpha_Ts;
benson516 1:ec51b9f84eee 48 float One_alpha_Ts;
benson516 1:ec51b9f84eee 49
benson516 1:ec51b9f84eee 50 // Flag
benson516 1:ec51b9f84eee 51 bool Flag_Init;
benson516 1:ec51b9f84eee 52
benson516 1:ec51b9f84eee 53 //
benson516 1:ec51b9f84eee 54 vector<float> zeros; // Zero vector [0;0;0]
benson516 1:ec51b9f84eee 55 };
benson516 1:ec51b9f84eee 56
benson516 6:18dd3f9ac217 57 //--------------------LPF_nthOrderCritical---------------------//
benson516 6:18dd3f9ac217 58 class LPF_nthOrderCritical{ // nth-order critical-damped Low-pass filter (all the poles are at the same place)
benson516 6:18dd3f9ac217 59 public:
benson516 6:18dd3f9ac217 60 float output;
benson516 6:18dd3f9ac217 61
benson516 6:18dd3f9ac217 62 LPF_nthOrderCritical(float samplingTime, float cutOff_freq_Hz_in, size_t order_in); // cutOff_freq_Hz_in is in "Hz"
benson516 6:18dd3f9ac217 63 float filter(float input);
benson516 6:18dd3f9ac217 64 void reset(float input);
benson516 6:18dd3f9ac217 65
benson516 6:18dd3f9ac217 66 private:
benson516 6:18dd3f9ac217 67 float Ts;
benson516 6:18dd3f9ac217 68 size_t order;
benson516 6:18dd3f9ac217 69 float cutOff_freq_Hz; // Hz
benson516 6:18dd3f9ac217 70
benson516 6:18dd3f9ac217 71 // Layers of 1st-order LPF
benson516 6:18dd3f9ac217 72 vector<LPF> filter_layers;
benson516 6:18dd3f9ac217 73
benson516 6:18dd3f9ac217 74 // Flag
benson516 6:18dd3f9ac217 75 bool Flag_Init;
benson516 6:18dd3f9ac217 76 };
benson516 6:18dd3f9ac217 77
benson516 6:18dd3f9ac217 78 //--------------------LPF_vector_nthOrderCritical---------------------//
benson516 6:18dd3f9ac217 79 class LPF_vector_nthOrderCritical{ // Vectorized nth-order critical-damped Low-pass filter (all the poles are at the same place)
benson516 6:18dd3f9ac217 80 public:
benson516 6:18dd3f9ac217 81 vector<float> output;
benson516 6:18dd3f9ac217 82
benson516 6:18dd3f9ac217 83 LPF_vector_nthOrderCritical(size_t dimension, float samplingTime, float cutOff_freq_Hz_in, size_t order_in); // cutOff_freq_Hz_in is in "Hz"
benson516 6:18dd3f9ac217 84 vector<float> filter(const vector<float> &input);
benson516 6:18dd3f9ac217 85 void reset(const vector<float> &input);
benson516 6:18dd3f9ac217 86
benson516 6:18dd3f9ac217 87 private:
benson516 6:18dd3f9ac217 88 size_t n;
benson516 6:18dd3f9ac217 89 float Ts;
benson516 6:18dd3f9ac217 90 size_t order;
benson516 6:18dd3f9ac217 91
benson516 6:18dd3f9ac217 92 float cutOff_freq_Hz; // Hz
benson516 6:18dd3f9ac217 93
benson516 6:18dd3f9ac217 94 // Flag
benson516 6:18dd3f9ac217 95 bool Flag_Init;
benson516 6:18dd3f9ac217 96
benson516 6:18dd3f9ac217 97 // Layers of vectorized 1st-order LPF
benson516 6:18dd3f9ac217 98 vector<LPF_vector> filter_layers;
benson516 6:18dd3f9ac217 99
benson516 6:18dd3f9ac217 100 //
benson516 6:18dd3f9ac217 101 vector<float> zeros; // Zero vector [0;0;0]
benson516 6:18dd3f9ac217 102 };
benson516 6:18dd3f9ac217 103
benson516 2:7275567b9c13 104
benson516 2:7275567b9c13 105 //--------------------HPF---------------------//
benson516 2:7275567b9c13 106 class HPF{ // High-pass filter
benson516 2:7275567b9c13 107 public:
benson516 2:7275567b9c13 108 float output;
benson516 2:7275567b9c13 109
benson516 2:7275567b9c13 110 HPF(float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz"
benson516 2:7275567b9c13 111 float filter(float input);
benson516 2:7275567b9c13 112 void reset(float input);
benson516 2:7275567b9c13 113
benson516 2:7275567b9c13 114 private:
benson516 2:7275567b9c13 115 float Ts;
benson516 2:7275567b9c13 116 float cutOff_freq_Hz; // Hz
benson516 2:7275567b9c13 117 // float alpha_Ts;
benson516 2:7275567b9c13 118 // float One_alpha_Ts;
benson516 2:7275567b9c13 119
benson516 2:7275567b9c13 120 // Flag
benson516 2:7275567b9c13 121 bool Flag_Init;
benson516 2:7275567b9c13 122
benson516 2:7275567b9c13 123 //
benson516 2:7275567b9c13 124 LPF lpf;
benson516 2:7275567b9c13 125 };
benson516 2:7275567b9c13 126
benson516 3:4fc5e159fdf7 127 //--------------------HPF_vector---------------------//
benson516 3:4fc5e159fdf7 128 class HPF_vector{ // Vectorized high-pass filter
benson516 3:4fc5e159fdf7 129 public:
benson516 3:4fc5e159fdf7 130 vector<float> output;
benson516 3:4fc5e159fdf7 131
benson516 3:4fc5e159fdf7 132 HPF_vector(size_t dimension, float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz"
benson516 6:18dd3f9ac217 133 vector<float> filter(const vector<float> &input);
benson516 6:18dd3f9ac217 134 void reset(const vector<float> &input);
benson516 3:4fc5e159fdf7 135
benson516 3:4fc5e159fdf7 136 private:
benson516 3:4fc5e159fdf7 137 size_t n;
benson516 3:4fc5e159fdf7 138 float Ts;
benson516 3:4fc5e159fdf7 139 float cutOff_freq_Hz; // Hz
benson516 3:4fc5e159fdf7 140 // float alpha_Ts;
benson516 3:4fc5e159fdf7 141 // float One_alpha_Ts;
benson516 3:4fc5e159fdf7 142
benson516 3:4fc5e159fdf7 143 // Flag
benson516 3:4fc5e159fdf7 144 bool Flag_Init;
benson516 3:4fc5e159fdf7 145
benson516 3:4fc5e159fdf7 146 //
benson516 3:4fc5e159fdf7 147 LPF_vector lpf_v;
benson516 3:4fc5e159fdf7 148 };
benson516 3:4fc5e159fdf7 149
benson516 6:18dd3f9ac217 150 //--------------------HPF_nthOrderCritical---------------------//
benson516 6:18dd3f9ac217 151 class HPF_nthOrderCritical{ // nth-order critical-damped High-pass filter (all the poles are at the same place)
benson516 6:18dd3f9ac217 152 public:
benson516 6:18dd3f9ac217 153 float output;
benson516 6:18dd3f9ac217 154
benson516 6:18dd3f9ac217 155 HPF_nthOrderCritical(float samplingTime, float cutOff_freq_Hz_in, size_t order_in); // cutOff_freq_Hz_in is in "Hz"
benson516 6:18dd3f9ac217 156 float filter(float input);
benson516 6:18dd3f9ac217 157 void reset(float input);
benson516 6:18dd3f9ac217 158
benson516 6:18dd3f9ac217 159 private:
benson516 6:18dd3f9ac217 160 float Ts;
benson516 6:18dd3f9ac217 161 size_t order;
benson516 6:18dd3f9ac217 162 float cutOff_freq_Hz; // Hz
benson516 6:18dd3f9ac217 163
benson516 6:18dd3f9ac217 164 // Layers of 1st-order HPF
benson516 6:18dd3f9ac217 165 vector<HPF> filter_layers;
benson516 6:18dd3f9ac217 166
benson516 6:18dd3f9ac217 167 // Flag
benson516 6:18dd3f9ac217 168 bool Flag_Init;
benson516 6:18dd3f9ac217 169 };
benson516 6:18dd3f9ac217 170
benson516 6:18dd3f9ac217 171
benson516 6:18dd3f9ac217 172 //--------------------HPF_vector_nthOrderCritical---------------------//
benson516 6:18dd3f9ac217 173 class HPF_vector_nthOrderCritical{ // Vectorized nth-order critical-damped High-pass filter (all the poles are at the same place)
benson516 6:18dd3f9ac217 174 public:
benson516 6:18dd3f9ac217 175 vector<float> output;
benson516 6:18dd3f9ac217 176
benson516 6:18dd3f9ac217 177 HPF_vector_nthOrderCritical(size_t dimension, float samplingTime, float cutOff_freq_Hz_in, size_t order_in); // cutOff_freq_Hz_in is in "Hz"
benson516 6:18dd3f9ac217 178 vector<float> filter(const vector<float> &input);
benson516 6:18dd3f9ac217 179 void reset(const vector<float> &input);
benson516 6:18dd3f9ac217 180
benson516 6:18dd3f9ac217 181 private:
benson516 6:18dd3f9ac217 182 size_t n;
benson516 6:18dd3f9ac217 183 float Ts;
benson516 6:18dd3f9ac217 184 size_t order;
benson516 6:18dd3f9ac217 185
benson516 6:18dd3f9ac217 186 float cutOff_freq_Hz; // Hz
benson516 6:18dd3f9ac217 187
benson516 6:18dd3f9ac217 188 // Flag
benson516 6:18dd3f9ac217 189 bool Flag_Init;
benson516 6:18dd3f9ac217 190
benson516 6:18dd3f9ac217 191 // Layers of vectorized 1st-order LPF
benson516 6:18dd3f9ac217 192 vector<HPF_vector> filter_layers;
benson516 6:18dd3f9ac217 193
benson516 6:18dd3f9ac217 194 //
benson516 6:18dd3f9ac217 195 vector<float> zeros; // Zero vector [0;0;0]
benson516 6:18dd3f9ac217 196 };
benson516 6:18dd3f9ac217 197
benson516 6:18dd3f9ac217 198 //--------------------HPF_vector_1minusLPF_nthOrderCritical---------------------//
benson516 6:18dd3f9ac217 199 class HPF_vector_1minusLPF_nthOrderCritical{ // Vectorized nth-order critical-damped High-pass filter ( the version of (1 - nth-order LPF), all the poles are at the same place)
benson516 6:18dd3f9ac217 200 public:
benson516 6:18dd3f9ac217 201 vector<float> output;
benson516 6:18dd3f9ac217 202
benson516 6:18dd3f9ac217 203 HPF_vector_1minusLPF_nthOrderCritical(size_t dimension, float samplingTime, float cutOff_freq_Hz_in, size_t order_in); // cutOff_freq_Hz_in is in "Hz"
benson516 6:18dd3f9ac217 204 vector<float> filter(const vector<float> &input);
benson516 6:18dd3f9ac217 205 void reset(const vector<float> &input);
benson516 6:18dd3f9ac217 206
benson516 6:18dd3f9ac217 207 private:
benson516 6:18dd3f9ac217 208 size_t n;
benson516 6:18dd3f9ac217 209 float Ts;
benson516 6:18dd3f9ac217 210 size_t order;
benson516 6:18dd3f9ac217 211
benson516 6:18dd3f9ac217 212 float cutOff_freq_Hz; // Hz
benson516 6:18dd3f9ac217 213
benson516 6:18dd3f9ac217 214 // Flag
benson516 6:18dd3f9ac217 215 bool Flag_Init;
benson516 6:18dd3f9ac217 216
benson516 6:18dd3f9ac217 217 // Layers of vectorized 1st-order LPF
benson516 6:18dd3f9ac217 218 vector<LPF_vector> filter_layers;
benson516 6:18dd3f9ac217 219
benson516 6:18dd3f9ac217 220 //
benson516 6:18dd3f9ac217 221 vector<float> zeros; // Zero vector [0;0;0]
benson516 6:18dd3f9ac217 222 };
benson516 6:18dd3f9ac217 223
benson516 3:4fc5e159fdf7 224
benson516 2:7275567b9c13 225 //--------------------Derivative_appr---------------------//
benson516 2:7275567b9c13 226 class Derivative_appr{ // Approximated Derivative, cut-off at 10% of sampling frequency
benson516 2:7275567b9c13 227 public:
benson516 2:7275567b9c13 228 float output;
benson516 2:7275567b9c13 229
benson516 2:7275567b9c13 230 Derivative_appr(float samplingTime);
benson516 2:7275567b9c13 231 float filter(float input);
benson516 2:7275567b9c13 232 void reset(float input);
benson516 2:7275567b9c13 233
benson516 2:7275567b9c13 234 private:
benson516 2:7275567b9c13 235 float Ts;
benson516 2:7275567b9c13 236 float cutOff_freq_Hz; // Hz
benson516 2:7275567b9c13 237
benson516 2:7275567b9c13 238 // Flag
benson516 2:7275567b9c13 239 bool Flag_Init;
benson516 2:7275567b9c13 240
benson516 2:7275567b9c13 241 //
benson516 2:7275567b9c13 242 IIR derivative_LPF2;
benson516 2:7275567b9c13 243 };
benson516 2:7275567b9c13 244
benson516 5:2291a3ccc64a 245 //--------------------Rate-saturation Filter---------------------//
benson516 5:2291a3ccc64a 246 class RateSaturation_Filter{ // Rate-saturation Filter
benson516 5:2291a3ccc64a 247 public:
benson516 5:2291a3ccc64a 248 float output;
benson516 5:2291a3ccc64a 249 float error;
benson516 5:2291a3ccc64a 250
benson516 5:2291a3ccc64a 251 RateSaturation_Filter(float samplingTime, float limit_rate_in); // limit_rate is in the unit of "value/s"
benson516 5:2291a3ccc64a 252 float filter(float input);
benson516 5:2291a3ccc64a 253 void reset(float input);
benson516 5:2291a3ccc64a 254
benson516 5:2291a3ccc64a 255 private:
benson516 5:2291a3ccc64a 256 float Ts;
benson516 5:2291a3ccc64a 257 float limit_rate;
benson516 5:2291a3ccc64a 258 float limit_increment;
benson516 5:2291a3ccc64a 259
benson516 5:2291a3ccc64a 260 // Flag
benson516 5:2291a3ccc64a 261 bool Flag_Init;
benson516 5:2291a3ccc64a 262 };
benson516 2:7275567b9c13 263
benson516 9:5d6b06b34127 264 //--------------------Moving average Filter---------------------//
benson516 9:5d6b06b34127 265 class MovingAverage{ // Moving average Filter
benson516 9:5d6b06b34127 266 public:
benson516 9:5d6b06b34127 267 float output;
benson516 9:5d6b06b34127 268
benson516 9:5d6b06b34127 269 MovingAverage(size_t windowSize_in); // windowSize is in the unit of "samples"
benson516 9:5d6b06b34127 270 float filter(float input);
benson516 9:5d6b06b34127 271 void reset(float input);
benson516 9:5d6b06b34127 272
benson516 9:5d6b06b34127 273 private:
benson516 9:5d6b06b34127 274 size_t windowSize;
benson516 9:5d6b06b34127 275 float windowSize_inv;
benson516 9:5d6b06b34127 276
benson516 9:5d6b06b34127 277 // Flag
benson516 9:5d6b06b34127 278 bool Flag_Init;
benson516 9:5d6b06b34127 279
benson516 9:5d6b06b34127 280 //
benson516 9:5d6b06b34127 281 vector<float> buffer;
benson516 9:5d6b06b34127 282 size_t idx_head;
benson516 9:5d6b06b34127 283 float buffer_sum;
benson516 9:5d6b06b34127 284 };
benson516 9:5d6b06b34127 285
benson516 7:10df955a92d9 286 //-----------First-Order Kalman Filter--------//
benson516 7:10df955a92d9 287 class FirstOrder_KalmanFilter{ // 1st-order Kalman filter
benson516 7:10df955a92d9 288 public:
benson516 7:10df955a92d9 289
benson516 7:10df955a92d9 290 // Parameters
benson516 7:10df955a92d9 291 float A;
benson516 7:10df955a92d9 292 float B;
benson516 7:10df955a92d9 293 float C;
benson516 7:10df955a92d9 294 //
benson516 7:10df955a92d9 295 float R;
benson516 7:10df955a92d9 296 float Q;
benson516 7:10df955a92d9 297
benson516 7:10df955a92d9 298 // States
benson516 7:10df955a92d9 299 float mu_est;
benson516 7:10df955a92d9 300 float Sigma_est;
benson516 7:10df955a92d9 301 // Kalman gain
benson516 7:10df955a92d9 302 float K;
benson516 7:10df955a92d9 303
benson516 7:10df955a92d9 304 FirstOrder_KalmanFilter(float samplingTime, float A_in, float B_in, float C_in, float R_in, float Q_in, bool is_continuousTime); // If is_continuousTime -> continuous time system
benson516 7:10df955a92d9 305 float filter(float u, float z);
benson516 7:10df955a92d9 306 void reset(float z);
benson516 7:10df955a92d9 307
benson516 7:10df955a92d9 308 private:
benson516 7:10df955a92d9 309 float Ts;
benson516 7:10df955a92d9 310
benson516 7:10df955a92d9 311
benson516 7:10df955a92d9 312 // Flag
benson516 7:10df955a92d9 313 bool Flag_Init;
benson516 7:10df955a92d9 314 };
benson516 7:10df955a92d9 315
benson516 7:10df955a92d9 316 //-----------------Saturation---------------//
benson516 7:10df955a92d9 317 class Saturation{ // Saturation
benson516 7:10df955a92d9 318 public:
benson516 7:10df955a92d9 319
benson516 7:10df955a92d9 320 // States
benson516 7:10df955a92d9 321 float output;
benson516 9:5d6b06b34127 322 float delta_out; // (original_out - limited_out)
benson516 9:5d6b06b34127 323 //
benson516 9:5d6b06b34127 324 bool is_saturated; // Indicate that if the signal is saturated.
benson516 7:10df955a92d9 325
benson516 7:10df955a92d9 326 Saturation(float bound_up_in, float bound_low_in); // If is_continuousTime -> continuous time system
benson516 9:5d6b06b34127 327 // Set the limitations
benson516 9:5d6b06b34127 328 void set_bound(float bound_up_in, float bound_low_in);
benson516 9:5d6b06b34127 329 //
benson516 7:10df955a92d9 330 float filter(float input);
benson516 7:10df955a92d9 331 void reset(float input);
benson516 7:10df955a92d9 332
benson516 7:10df955a92d9 333 private:
benson516 7:10df955a92d9 334 float Ts;
benson516 7:10df955a92d9 335
benson516 7:10df955a92d9 336 //
benson516 7:10df955a92d9 337 float bound_up;
benson516 7:10df955a92d9 338 float bound_low;
benson516 7:10df955a92d9 339
benson516 7:10df955a92d9 340 // Flag
benson516 7:10df955a92d9 341 bool Flag_Init;
benson516 7:10df955a92d9 342 };
benson516 7:10df955a92d9 343
benson516 7:10df955a92d9 344 //-----------------Saturation_vector---------------//
benson516 7:10df955a92d9 345 class Saturation_vector{ // Saturation
benson516 7:10df955a92d9 346 public:
benson516 7:10df955a92d9 347
benson516 7:10df955a92d9 348 // States
benson516 7:10df955a92d9 349 vector<float> output;
benson516 9:5d6b06b34127 350 vector<float> delta_out; // (original_out - limited_out)
benson516 9:5d6b06b34127 351 //
benson516 9:5d6b06b34127 352 vector<bool> is_saturated; // Indicate that if the signal is saturated.
benson516 7:10df955a92d9 353
benson516 7:10df955a92d9 354 Saturation_vector(size_t dimension, float bound_up_in, float bound_low_in); // If is_continuousTime -> continuous time system
benson516 9:5d6b06b34127 355 // Set the limitations
benson516 9:5d6b06b34127 356 void set_bound(float bound_up_in, float bound_low_in);
benson516 9:5d6b06b34127 357 //
benson516 7:10df955a92d9 358 vector<float> filter(vector<float> input);
benson516 7:10df955a92d9 359 void reset(vector<float> input);
benson516 7:10df955a92d9 360
benson516 7:10df955a92d9 361 private:
benson516 7:10df955a92d9 362 size_t n;
benson516 7:10df955a92d9 363
benson516 7:10df955a92d9 364 //
benson516 7:10df955a92d9 365 float bound_up;
benson516 7:10df955a92d9 366 float bound_low;
benson516 7:10df955a92d9 367
benson516 7:10df955a92d9 368 // Flag
benson516 7:10df955a92d9 369 bool Flag_Init;
benson516 7:10df955a92d9 370 };
benson516 7:10df955a92d9 371
benson516 0:63eb74dddbc9 372 #endif