Chun Feng Huang / FILTER_LIB

A combination of some frequently used filters

FILTER_LIB.h@7:10df955a92d9, 2017-02-10 (annotated)

Committer:
benson516
Date:
Fri Feb 10 18:26:47 2017 +0000
Revision:
7:10df955a92d9
Parent:
6:18dd3f9ac217 
Add the first-order Kalman filter

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 7:10df955a92d9 264 //-----------First-Order Kalman Filter--------//
benson516 7:10df955a92d9 265 class FirstOrder_KalmanFilter{ // 1st-order Kalman filter
benson516 7:10df955a92d9 266 public:
benson516 7:10df955a92d9 267
benson516 7:10df955a92d9 268 // Parameters
benson516 7:10df955a92d9 269 float A;
benson516 7:10df955a92d9 270 float B;
benson516 7:10df955a92d9 271 float C;
benson516 7:10df955a92d9 272 //
benson516 7:10df955a92d9 273 float R;
benson516 7:10df955a92d9 274 float Q;
benson516 7:10df955a92d9 275
benson516 7:10df955a92d9 276 // States
benson516 7:10df955a92d9 277 float mu_est;
benson516 7:10df955a92d9 278 float Sigma_est;
benson516 7:10df955a92d9 279 // Kalman gain
benson516 7:10df955a92d9 280 float K;
benson516 7:10df955a92d9 281
benson516 7:10df955a92d9 282 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 283 float filter(float u, float z);
benson516 7:10df955a92d9 284 void reset(float z);
benson516 7:10df955a92d9 285
benson516 7:10df955a92d9 286 private:
benson516 7:10df955a92d9 287 float Ts;
benson516 7:10df955a92d9 288
benson516 7:10df955a92d9 289
benson516 7:10df955a92d9 290 // Flag
benson516 7:10df955a92d9 291 bool Flag_Init;
benson516 7:10df955a92d9 292 };
benson516 7:10df955a92d9 293
benson516 7:10df955a92d9 294 //-----------------Saturation---------------//
benson516 7:10df955a92d9 295 class Saturation{ // Saturation
benson516 7:10df955a92d9 296 public:
benson516 7:10df955a92d9 297
benson516 7:10df955a92d9 298 // States
benson516 7:10df955a92d9 299 float output;
benson516 7:10df955a92d9 300
benson516 7:10df955a92d9 301 Saturation(float bound_up_in, float bound_low_in); // If is_continuousTime -> continuous time system
benson516 7:10df955a92d9 302 float filter(float input);
benson516 7:10df955a92d9 303 void reset(float input);
benson516 7:10df955a92d9 304
benson516 7:10df955a92d9 305 private:
benson516 7:10df955a92d9 306 float Ts;
benson516 7:10df955a92d9 307
benson516 7:10df955a92d9 308 //
benson516 7:10df955a92d9 309 float bound_up;
benson516 7:10df955a92d9 310 float bound_low;
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_vector---------------//
benson516 7:10df955a92d9 317 class Saturation_vector{ // Saturation
benson516 7:10df955a92d9 318 public:
benson516 7:10df955a92d9 319
benson516 7:10df955a92d9 320 // States
benson516 7:10df955a92d9 321 vector<float> output;
benson516 7:10df955a92d9 322
benson516 7:10df955a92d9 323 Saturation_vector(size_t dimension, float bound_up_in, float bound_low_in); // If is_continuousTime -> continuous time system
benson516 7:10df955a92d9 324 vector<float> filter(vector<float> input);
benson516 7:10df955a92d9 325 void reset(vector<float> input);
benson516 7:10df955a92d9 326
benson516 7:10df955a92d9 327 private:
benson516 7:10df955a92d9 328 size_t n;
benson516 7:10df955a92d9 329
benson516 7:10df955a92d9 330 //
benson516 7:10df955a92d9 331 float bound_up;
benson516 7:10df955a92d9 332 float bound_low;
benson516 7:10df955a92d9 333
benson516 7:10df955a92d9 334 // Flag
benson516 7:10df955a92d9 335 bool Flag_Init;
benson516 7:10df955a92d9 336 };
benson516 7:10df955a92d9 337
benson516 0:63eb74dddbc9 338 #endif