Chun Feng Huang / FILTER_LIB

A combination of some frequently used filters

CIRCULAR_BUFFER_VECTOR.cpp@7:10df955a92d9, 2017-02-10 (annotated)

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 4:f1a174f1c63b 1 #include "CIRCULAR_BUFFER_VECTOR.h"
benson516 4:f1a174f1c63b 2
benson516 4:f1a174f1c63b 3 // First type of constructer
benson516 4:f1a174f1c63b 4 CIRCULAR_BUFFER_VECTOR::CIRCULAR_BUFFER_VECTOR(void):
benson516 4:f1a174f1c63b 5 buffer(0,vector<float>(0))
benson516 4:f1a174f1c63b 6 {
benson516 4:f1a174f1c63b 7 //
benson516 4:f1a174f1c63b 8 buffer_size = 0;
benson516 4:f1a174f1c63b 9 vector_size = 0;
benson516 4:f1a174f1c63b 10 idx = 0;
benson516 4:f1a174f1c63b 11 }
benson516 4:f1a174f1c63b 12 // Second type of constructer
benson516 4:f1a174f1c63b 13 CIRCULAR_BUFFER_VECTOR::CIRCULAR_BUFFER_VECTOR(size_t buffer_size_in, size_t vector_size_in):
benson516 4:f1a174f1c63b 14 buffer_size(buffer_size_in),
benson516 4:f1a174f1c63b 15 vector_size(vector_size_in),
benson516 4:f1a174f1c63b 16 buffer(buffer_size_in,vector<float>(vector_size_in,0.0))
benson516 4:f1a174f1c63b 17 {
benson516 4:f1a174f1c63b 18 idx = 0;
benson516 4:f1a174f1c63b 19 }
benson516 4:f1a174f1c63b 20 //
benson516 4:f1a174f1c63b 21 void CIRCULAR_BUFFER_VECTOR::Init(size_t buffer_size_in, const vector<float> &initial_value){ // If using the first type of constructer, this function helps
benson516 4:f1a174f1c63b 22 //
benson516 4:f1a174f1c63b 23 buffer_size = buffer_size_in;
benson516 4:f1a174f1c63b 24 vector_size = initial_value.size();
benson516 4:f1a174f1c63b 25 idx = 0;
benson516 4:f1a174f1c63b 26 //
benson516 4:f1a174f1c63b 27 buffer.assign(buffer_size_in, initial_value);
benson516 4:f1a174f1c63b 28 }
benson516 4:f1a174f1c63b 29 void CIRCULAR_BUFFER_VECTOR::Reset(const vector<float> &value){ // Reset all the element without changeing the buffer size
benson516 4:f1a174f1c63b 30 //
benson516 4:f1a174f1c63b 31 buffer.assign(buffer_size, value);
benson516 4:f1a174f1c63b 32 idx = 0;
benson516 4:f1a174f1c63b 33 }
benson516 4:f1a174f1c63b 34 // Element-wise operation
benson516 4:f1a174f1c63b 35 vector<float> CIRCULAR_BUFFER_VECTOR::Get(int i){ // Get the element that is i samples ago
benson516 4:f1a174f1c63b 36 int idx_e = idx - i;
benson516 4:f1a174f1c63b 37 while (idx_e < 0){
benson516 4:f1a174f1c63b 38 idx_e += buffer_size;
benson516 4:f1a174f1c63b 39 }
benson516 4:f1a174f1c63b 40 return buffer[idx_e];
benson516 4:f1a174f1c63b 41 }
benson516 4:f1a174f1c63b 42 void CIRCULAR_BUFFER_VECTOR::Set(int i, const vector<float> &value){ // Set the element that is i samples ago to the "value"
benson516 4:f1a174f1c63b 43 //
benson516 4:f1a174f1c63b 44 int idx_e = idx - i;
benson516 4:f1a174f1c63b 45 while (idx_e < 0){
benson516 4:f1a174f1c63b 46 idx_e += buffer_size;
benson516 4:f1a174f1c63b 47 }
benson516 4:f1a174f1c63b 48 //
benson516 4:f1a174f1c63b 49 buffer[idx_e] = value;
benson516 4:f1a174f1c63b 50 }
benson516 4:f1a174f1c63b 51 void CIRCULAR_BUFFER_VECTOR::Increase(int i, const vector<float> &increase_value, bool is_minus){ // vi += (or -=) increase_value
benson516 4:f1a174f1c63b 52 //
benson516 4:f1a174f1c63b 53 int idx_e = idx - i;
benson516 4:f1a174f1c63b 54 while (idx_e < 0){
benson516 4:f1a174f1c63b 55 idx_e += buffer_size;
benson516 4:f1a174f1c63b 56 }
benson516 4:f1a174f1c63b 57 //
benson516 4:f1a174f1c63b 58 Get_VectorIncrement(buffer[idx_e], increase_value, is_minus);
benson516 4:f1a174f1c63b 59 }
benson516 4:f1a174f1c63b 60 // Iterate the buffer
benson516 4:f1a174f1c63b 61 void CIRCULAR_BUFFER_VECTOR::Insert(const vector<float> &x_new){ // Pop the oldest element and push a new element
benson516 4:f1a174f1c63b 62 //
benson516 4:f1a174f1c63b 63 idx++;
benson516 4:f1a174f1c63b 64 while (idx >= buffer_size){
benson516 4:f1a174f1c63b 65 idx -= (int)buffer_size;
benson516 4:f1a174f1c63b 66 }
benson516 4:f1a174f1c63b 67 buffer[idx] = x_new;
benson516 4:f1a174f1c63b 68 }
benson516 4:f1a174f1c63b 69 // Utilities
benson516 4:f1a174f1c63b 70 // Increment
benson516 4:f1a174f1c63b 71 void CIRCULAR_BUFFER_VECTOR::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
benson516 4:f1a174f1c63b 72 // Size check
benson516 4:f1a174f1c63b 73 if (v_a.size() != v_b.size()){
benson516 4:f1a174f1c63b 74 v_a.resize(v_b.size());
benson516 4:f1a174f1c63b 75 }
benson516 4:f1a174f1c63b 76 //
benson516 4:f1a174f1c63b 77 if (is_minus){ // -=
benson516 4:f1a174f1c63b 78 for (size_t i = 0; i < v_b.size(); ++i){
benson516 4:f1a174f1c63b 79 v_a[i] -= v_b[i];
benson516 4:f1a174f1c63b 80 }
benson516 4:f1a174f1c63b 81 }else{ // +=
benson516 4:f1a174f1c63b 82 for (size_t i = 0; i < v_b.size(); ++i){
benson516 4:f1a174f1c63b 83 v_a[i] += v_b[i];
benson516 4:f1a174f1c63b 84 }
benson516 4:f1a174f1c63b 85 }
benson516 4:f1a174f1c63b 86
benson516 4:f1a174f1c63b 87 }