Filter_Obs.cpp 55.3 KB
Newer Older
Achim Morschhauser's avatar
Achim Morschhauser committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
/*
 * Filter_Obs_Median.cpp
 *
 *  Created on: Jun 7, 2016
 *      Author: Achim Morschhauser, GFZ Potsdam
 */

// C++ Headers
#include <Filter_Obs.hpp>
#include <queue>
#include <vector>
#include <iostream>

// C Headers
#include <time.h>
#include <math.h>

#include <data_obs_vector.hpp>

20

Achim Morschhauser's avatar
Achim Morschhauser committed
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
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
//																			/
//						Constructors & Destructors							/
//																			/
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

Filter_Obs::Filter_Obs() {
	// TODO Auto-generated constructor stub

}

Filter_Obs::~Filter_Obs() {
	// TODO Auto-generated destructor stub
}

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
//																			/
//						Public Methods										/
//																			/
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////
//																		 	/
// Calculate running median for vector data:								/
// 																			/
// - Points is the number of points to use for calculation of the median.	/
// - Downsampling allows to only output each downsampling point.			/
// - If Points=Downsampling, a non-running mean is calculated.				/
// 																			/
 ////////////////////////////////////////////////////////////////////////////
55
int Filter_Obs::median_odd(buffer_obs* buffer_in,
Achim Morschhauser's avatar
Achim Morschhauser committed
56
57
58
59
60
61
62
		buffer_obs* buffer_out, int points, int downsample){

	if (points % 2 == 0){
		std::cerr << "Only odd number of points possible!" << std::endl;
		return(-1);
	}

63
64
	std::vector <data_obs*> tofilter;
	data_obs* data = new data_obs();
Achim Morschhauser's avatar
Achim Morschhauser committed
65
66
67
68
69
70
71

	int count=downsample;

	// Load initial number of points
	for (int i=0; i<points-1; i++){

		buffer_in->pop(data);
72
		tofilter.push_back(new data_obs(data));
Achim Morschhauser's avatar
Achim Morschhauser committed
73
74
75
76
77
78
79
80
81

	}


	// Running median
	while (1){

		buffer_in->pop(data);

82
		tofilter.push_back(new data_obs(data));
Achim Morschhauser's avatar
Achim Morschhauser committed
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
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127

		// Apply filter window
		if (count==downsample){
			data->median_odd(&tofilter);
			buffer_out->put(data);
			count=1;
		} else {
			count++;
		}

		delete(tofilter[0]);
		tofilter.erase(tofilter.begin());

	}

	return(0);

}

/////////////////////////////////////////////////////////////////////////////
//																		 	/
// Filter with Gauss window.												/
//																			/
/////////////////////////////////////////////////////////////////////////////
/**
// Geht leider nicht so einfach.... Window design constraints
// --> see MATLAB fir1
int Filter_Obs::gaussian_odd_vec(buffer_obs* buffer_in,
		buffer_obs* buffer_out, double cutoff, int points){

	// Find the STD from -3dB cutoff frequency
	double sigma_f=sqrt(pow(cutoff,2)/log(2));
	double sigma_t=1/2/M_PI/cutoff;

	return(0);

}

**/

/////////////////////////////////////////////////////////////////////////////
//																		 	/
// FIR-Filter with pre-defined window .										/
//																			/
/////////////////////////////////////////////////////////////////////////////
128
int Filter_Obs::fir_window(buffer_obs* buffer_in,
Achim Morschhauser's avatar
Achim Morschhauser committed
129
130
131
132
133
134
135
136
137
138
		buffer_obs* buffer_out, std::vector <double>* win, int downsample){


	//
	// Initialize
	// ==========
	//

	std::vector <data_obs*> tofilter;			// Values for filtering

139
	data_obs* data = 				         	// Data to hold the
Achim Morschhauser's avatar
Achim Morschhauser committed
140
141
142
143
144
145
146
147
148
149
150
151
			new data_obs_vector();				// filtered value

	int count=downsample;						// Counter for downsampling.
												// Start with first value.

	//
	// Load initial number of points.
	// ==============================
	//
	for (int i=0; i<win->size()-1; i++){

		buffer_in->pop(data);
152
		tofilter.push_back(new data_obs(data));
Achim Morschhauser's avatar
Achim Morschhauser committed
153
154
155
156
157
158
159
160
161
162
163
164
165

	}

	//
	// Run the filter continuously.
	// ============================
	//
	while (1){

		// Read data
		buffer_in->pop(data);

		// Add data to filter list
166
		tofilter.push_back(new data_obs(data));
Achim Morschhauser's avatar
Achim Morschhauser committed
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191

		// Apply filter window
		if (count==downsample){
			data->filter_fir_sym(&tofilter,win);
			buffer_out->put(data);
			count=1;
		} else {
			count++;
		}

		// Delete oldest data in filter last
		delete(tofilter[0]);
		tofilter.erase(tofilter.begin());

	}

	return(0);

}

/////////////////////////////////////////////////////////////////////////////
//																		 	/
// PLASMON filter with downsampling to 1Hz.									/
//																			/
/////////////////////////////////////////////////////////////////////////////
192

Achim Morschhauser's avatar
Achim Morschhauser committed
193
int Filter_Obs::lowpass_PLASMON_1Hz(buffer_obs* buffer_in,
194
        buffer_obs* buffer_out, double freq_in) {
Achim Morschhauser's avatar
Achim Morschhauser committed
195

196
    // =====================================================================
197
    // Initialize
198
    // =====================================================================
199

variomet's avatar
variomet committed
200
201
    std::cerr << "------------- INTITIALIZE PLASMON -------------" << std::endl;

202
203
    std::vector <data_obs*> tofilter1; // Filter1 queue
    std::vector <data_obs*> tofilter2; // Filter2 queue
204
    std::vector <data_obs*> filter1_slice; // Filter1 subslice queue
205
    
206
207
208
    // Possibly slower, array might be faster but less dynamic
    std::map<int,std::vector<double>> filter1_wins;
    std::vector<double>* filter1_win;
209
210

    data_obs_vector* data = // Data to hold the
Thomas Edwards's avatar
Thomas Edwards committed
211
    new data_obs_vector(5); // filtered value
212
    // Placeholder when filtering
213
    data_obs_vector* filter_data_temp = new data_obs_vector(5);
214
215
216
217
    
    // Time of current and previous samples
    struct timespec last_timespec;
    struct timespec this_timespec;
218
219
    struct timespec last_timeblock_edge;
    struct timespec this_timeblock_edge;
220
221
    // Timestamp as string for logging
    std::string timestamp;
222

223
224
225
226
    // Hard limiter for the que
    // currently set to 10 seconds at 128 Hz
    int que_limit = 128*10;

227
    // =====================================================================
228
    // Filter1 downsamples to 16 Hz with a cut-off frequency of 5 Hz
229
230
231
232
233
234
    // =====================================================================

    // Output frequency of filter1 in Hz      
    int filter1_freq = 16;
    // Number of output blocks required to fill filter1
    int filter1_block_len = filter1_freq;
235

236
237
    // Time window between two blocks
    long filter1_delta_t_msec = (1.0 / filter1_freq) * 1e9;
238
239
240
    int filter1_que_limit = (freq_in*1.5);
    int slip_count = 0;
    int slip_limit = 8;
241
242
    
    if (freq_in == 128) {
243

244
245
        // Created with MATLAB: f126=fir1(N-1,5/(N-1)*2,gausswin(N));
    	// 					    sprintf("%9.8f, ",f126)
246
247
248
249
250
251
    	// filter1_wins[121] = std::vector<double> ({0.00000000, 0.00006793, 0.00014776, 0.00023495, 0.00032308, 0.00040401, 0.00046829, 0.00050572, 0.00050624, 0.00046086, 0.00036286, 0.00020886, -0.00000000, -0.00025724, -0.00055044, -0.00086115, -0.00116530, -0.00143427, -0.00163663, -0.00174041, -0.00171592, -0.00153897, -0.00119406, -0.00067750, 0.00000000, 0.00081157, 0.00171357, 0.00264631, 0.00353635, 0.00430039, 0.00485066, 0.00510164, 0.00497766, 0.00442088, 0.00339910, 0.00191272, -0.00000000, -0.00225972, -0.00474567, -0.00729838, -0.00972564, -0.01181165, -0.01332897, -0.01405266, -0.01377574, -0.01232494, -0.00957560, -0.00546457, 0.00000000, 0.00673290, 0.01457086, 0.02327665, 0.03254824, 0.04203272, 0.05134420, 0.06008462, 0.06786613, 0.07433360, 0.07918563, 0.08219276, 0.08321142, 0.08219276, 0.07918563, 0.07433360, 0.06786613, 0.06008462, 0.05134420, 0.04203272, 0.03254824, 0.02327665, 0.01457086, 0.00673290, 0.00000000, -0.00546457, -0.00957560, -0.01232494, -0.01377574, -0.01405266, -0.01332897, -0.01181165, -0.00972564, -0.00729838, -0.00474567, -0.00225972, -0.00000000, 0.00191272, 0.00339910, 0.00442088, 0.00497766, 0.00510164, 0.00485066, 0.00430039, 0.00353635, 0.00264631, 0.00171357, 0.00081157, 0.00000000, -0.00067750, -0.00119406, -0.00153897, -0.00171592, -0.00174041, -0.00163663, -0.00143427, -0.00116530, -0.00086115, -0.00055044, -0.00025724, -0.00000000, 0.00020886, 0.00036286, 0.00046086, 0.00050624, 0.00050572, 0.00046829, 0.00040401, 0.00032308, 0.00023495, 0.00014776, 0.00006793, 0.00000000});
    	// filter1_wins[122] = std::vector<double> ({0.00000000, 0.00006676, 0.00014516, 0.00023082, 0.00031759, 0.00039762, 0.00046180, 0.00050030, 0.00050334, 0.00046213, 0.00036998, 0.00022332, 0.00002284, -0.00022573, -0.00051096, -0.00081565, -0.00111713, -0.00138820, -0.00159869, -0.00171758, -0.00171563, -0.00156836, -0.00125911, -0.00078210, -0.00014492, 0.00062955, 0.00150237, 0.00241894, 0.00331085, 0.00409926, 0.00469975, 0.00502839, 0.00500883, 0.00457986, 0.00370297, 0.00236919, 0.00060469, -0.00152563, -0.00391688, -0.00642593, -0.00887608, -0.01106485, -0.01277454, -0.01378506, -0.01388831, -0.01290319, -0.01069021, -0.00716459, -0.00230681, 0.00383034, 0.01111902, 0.01935818, 0.02828041, 0.03756333, 0.04684510, 0.05574323, 0.06387531, 0.07088059, 0.07644081, 0.08029896, 0.08227455, 0.08227455, 0.08029896, 0.07644081, 0.07088059, 0.06387531, 0.05574323, 0.04684510, 0.03756333, 0.02828041, 0.01935818, 0.01111902, 0.00383034, -0.00230681, -0.00716459, -0.01069021, -0.01290319, -0.01388831, -0.01378506, -0.01277454, -0.01106485, -0.00887608, -0.00642593, -0.00391688, -0.00152563, 0.00060469, 0.00236919, 0.00370297, 0.00457986, 0.00500883, 0.00502839, 0.00469975, 0.00409926, 0.00331085, 0.00241894, 0.00150237, 0.00062955, -0.00014492, -0.00078210, -0.00125911, -0.00156836, -0.00171563, -0.00171758, -0.00159869, -0.00138820, -0.00111713, -0.00081565, -0.00051096, -0.00022573, 0.00002284, 0.00022332, 0.00036998, 0.00046213, 0.00050334, 0.00050030, 0.00046180, 0.00039762, 0.00031759, 0.00023082, 0.00014516, 0.00006676, 0.00000000});
    	// filter1_wins[123] = std::vector<double> ({0.00000000, 0.00006562, 0.00014262, 0.00022680, 0.00031222, 0.00039135, 0.00045540, 0.00049487, 0.00050027, 0.00046302, 0.00037645, 0.00023684, 0.00004446, -0.00019563, -0.00047296, -0.00077148, -0.00106986, -0.00134226, -0.00155976, -0.00169232, -0.00171120, -0.00159175, -0.00131639, -0.00087746, -0.00027975, 0.00045754, 0.00129987, 0.00219747, 0.00308680, 0.00389351, 0.00453664, 0.00493428, 0.00501001, 0.00470011, 0.00396076, 0.00277482, 0.00115752, -0.00083946, -0.00312617, -0.00557513, -0.00802493, -0.01028671, -0.01215341, -0.01341131, -0.01385334, -0.01329327, -0.01157983, -0.00860988, -0.00433945, 0.00120820, 0.00793828, 0.01568445, 0.02421344, 0.03323411, 0.04241066, 0.05137934, 0.05976747, 0.06721384, 0.07338901, 0.07801424, 0.08087777, 0.08184727, 0.08087777, 0.07801424, 0.07338901, 0.06721384, 0.05976747, 0.05137934, 0.04241066, 0.03323411, 0.02421344, 0.01568445, 0.00793828, 0.00120820, -0.00433945, -0.00860988, -0.01157983, -0.01329327, -0.01385334, -0.01341131, -0.01215341, -0.01028671, -0.00802493, -0.00557513, -0.00312617, -0.00083946, 0.00115752, 0.00277482, 0.00396076, 0.00470011, 0.00501001, 0.00493428, 0.00453664, 0.00389351, 0.00308680, 0.00219747, 0.00129987, 0.00045754, -0.00027975, -0.00087746, -0.00131639, -0.00159175, -0.00171120, -0.00169232, -0.00155976, -0.00134226, -0.00106986, -0.00077148, -0.00047296, -0.00019563, 0.00004446, 0.00023684, 0.00037645, 0.00046302, 0.00050027, 0.00049487, 0.00045540, 0.00039135, 0.00031222, 0.00022680, 0.00014262, 0.00006562, 0.00000000});
    	// filter1_wins[124] = std::vector<double> ({0.00000000, 0.00006450, 0.00014015, 0.00022288, 0.00030698, 0.00038521, 0.00044908, 0.00048944, 0.00049706, 0.00046355, 0.00038230, 0.00024946, 0.00006491, -0.00016690, -0.00043639, -0.00072864, -0.00102354, -0.00129658, -0.00152006, -0.00166493, -0.00170301, -0.00160961, -0.00136639, -0.00096403, -0.00040484, 0.00029541, 0.00110625, 0.00198249, 0.00286532, 0.00368478, 0.00436356, 0.00482203, 0.00498428, 0.00478480, 0.00417539, 0.00313183, 0.00165960, -0.00020177, -0.00237626, -0.00475130, -0.00718040, -0.00948843, -0.01147961, -0.01294788, -0.01368905, -0.01351411, -0.01226285, -0.00981681, -0.00611068, -0.00114110, 0.00502771, 0.01226233, 0.02036276, 0.02906928, 0.03807353, 0.04703300, 0.05558840, 0.06338259, 0.07008017, 0.07538625, 0.07906324, 0.08094463, 0.08094463, 0.07906324, 0.07538625, 0.07008017, 0.06338259, 0.05558840, 0.04703300, 0.03807353, 0.02906928, 0.02036276, 0.01226233, 0.00502771, -0.00114110, -0.00611068, -0.00981681, -0.01226285, -0.01351411, -0.01368905, -0.01294788, -0.01147961, -0.00948843, -0.00718040, -0.00475130, -0.00237626, -0.00020177, 0.00165960, 0.00313183, 0.00417539, 0.00478480, 0.00498428, 0.00482203, 0.00436356, 0.00368478, 0.00286532, 0.00198249, 0.00110625, 0.00029541, -0.00040484, -0.00096403, -0.00136639, -0.00160961, -0.00170301, -0.00166493, -0.00152006, -0.00129658, -0.00102354, -0.00072864, -0.00043639, -0.00016690, 0.00006491, 0.00024946, 0.00038230, 0.00046355, 0.00049706, 0.00048944, 0.00044908, 0.00038521, 0.00030698, 0.00022288, 0.00014015, 0.00006450, 0.00000000});
    	// filter1_wins[125] = std::vector<double> ({0.00000000, 0.00006342, 0.00013774, 0.00021905, 0.00030186, 0.00037919, 0.00044286, 0.00048401, 0.00049371, 0.00046376, 0.00038759, 0.00026124, 0.00008426, -0.00013948, -0.00040124, -0.00068711, -0.00097821, -0.00125126, -0.00147978, -0.00163569, -0.00169142, -0.00162237, -0.00140956, -0.00104228, -0.00052053, 0.00014297, 0.00092160, 0.00177448, 0.00264734, 0.00347456, 0.00418253, 0.00469418, 0.00493454, 0.00483698, 0.00434980, 0.00344266, 0.00211237, 0.00038742, -0.00166909, -0.00395875, -0.00634949, -0.00867985, -0.01076583, -0.01240998, -0.01341258, -0.01358394, -0.01275744, -0.01080216, -0.00763439, -0.00322707, 0.00238362, 0.00909520, 0.01673971, 0.02508863, 0.03386179, 0.04273992, 0.05138008, 0.05943322, 0.06656267, 0.07246255, 0.07687477, 0.07960367, 0.08052713, 0.07960367, 0.07687477, 0.07246255, 0.06656267, 0.05943322, 0.05138008, 0.04273992, 0.03386179, 0.02508863, 0.01673971, 0.00909520, 0.00238362, -0.00322707, -0.00763439, -0.01080216, -0.01275744, -0.01358394, -0.01341258, -0.01240998, -0.01076583, -0.00867985, -0.00634949, -0.00395875, -0.00166909, 0.00038742, 0.00211237, 0.00344266, 0.00434980, 0.00483698, 0.00493454, 0.00469418, 0.00418253, 0.00347456, 0.00264734, 0.00177448, 0.00092160, 0.00014297, -0.00052053, -0.00104228, -0.00140956, -0.00162237, -0.00169142, -0.00163569, -0.00147978, -0.00125126, -0.00097821, -0.00068711, -0.00040124, -0.00013948, 0.00008426, 0.00026124, 0.00038759, 0.00046376, 0.00049371, 0.00048401, 0.00044286, 0.00037919, 0.00030186, 0.00021905, 0.00013774, 0.00006342, 0.00000000});
    	// filter1_wins[126] = std::vector<double> ({0.0000000, 0.00006236, 0.00013539, 0.00021532, 0.00029686, 0.00037329, 0.00043672, 0.00047860, 0.00049024, 0.00046365, 0.00039233, 0.00027222, 0.00010255, -0.00011334, -0.00036746, -0.00064690, -0.00093390, -0.00120640, -0.00143907, -0.00160484, -0.00167678, -0.00163044, -0.00144638, -0.00111264, -0.00062723, 0.00000000, 0.00074595, 0.00157382, 0.00243368, 0.00326415, 0.00399538, 0.00455305, 0.00486352, 0.00485961, 0.00448688, 0.00370981, 0.00251754, 0.00092858, -0.00100593, -0.00320088, -0.00553810, -0.00786962, -0.01002342, -0.01181151, -0.01303991, -0.01352011, -0.01308133, -0.01158282, -0.00892530, -0.00506079, 0.00000000, 0.00618338, 0.01335196, 0.02130775, 0.02979916, 0.03853149, 0.04718062, 0.05540894, 0.06288274, 0.06928990, 0.07435688, 0.07786383, 0.07965689, 0.07965689, 0.07786383, 0.07435688, 0.06928990, 0.06288274, 0.05540894, 0.04718062, 0.03853149, 0.02979916, 0.02130775, 0.01335196, 0.00618338, 0.00000000, -0.00506079, -0.00892530, -0.01158282, -0.01308133, -0.01352011, -0.01303991, -0.01181151, -0.01002342, -0.00786962, -0.00553810, -0.00320088, -0.00100593, 0.00092858, 0.00251754, 0.00370981, 0.00448688, 0.00485961, 0.00486352, 0.00455305, 0.00399538, 0.00326415, 0.00243368, 0.00157382, 0.00074595, 0.00000000, -0.00062723, -0.00111264, -0.00144638, -0.00163044, -0.00167678, -0.00160484, -0.00143907, -0.00120640, -0.00093390, -0.00064690, -0.00036746, -0.00011334, 0.00010255, 0.00027222, 0.00039233, 0.00046365, 0.00049024, 0.00047860, 0.00043672, 0.00037329, 0.00029686, 0.00021532, 0.00013539, 0.00006236, 0.00000000});
252
        // filter1_wins[127] = std::vector<double> ({0.0000000, 0.00006133, 0.00013310, 0.00021168, 0.00029198, 0.00036751, 0.00043068, 0.00047320, 0.00048667, 0.00046326, 0.00039657, 0.00028243, 0.00011983, -0.00008841, -0.00033502, -0.00060798, -0.00089063, -0.00116207, -0.00139811, -0.00157263, -0.00165940, -0.00163421, -0.00147726, -0.00117556, -0.00072530, -0.00013375, 0.00057927, 0.00138079, 0.00222502, 0.00305473, 0.00380378, 0.00440079, 0.00477377, 0.00485551, 0.00458950, 0.00393587, 0.00287705, 0.00142257, -0.00038743, -0.00248031, -0.00475115, -0.00706518, -0.00926251, -0.01116513, -0.01258589, -0.01333904, -0.01325168, -0.01217562, -0.00999864, -0.00665452, -0.00213109, 0.00352454, 0.01020386, 0.01773846, 0.02590522, 0.03443492, 0.04302413, 0.05134955, 0.05908407, 0.06591368, 0.07155408, 0.07576607, 0.07836861, 0.07924890, 0.07836861, 0.07576607, 0.07155408, 0.06591368, 0.05908407, 0.05134955, 0.04302413, 0.03443492, 0.02590522, 0.01773846, 0.01020386, 0.00352454, -0.00213109, -0.00665452, -0.00999864, -0.01217562, -0.01325168, -0.01333904, -0.01258589, -0.01116513, -0.00926251, -0.00706518, -0.00475115, -0.00248031, -0.00038743, 0.00142257, 0.00287705, 0.00393587, 0.00458950, 0.00485551, 0.00477377, 0.00440079, 0.00380378, 0.00305473, 0.00222502, 0.00138079, 0.00057927, -0.00013375, -0.00072530, -0.00117556, -0.00147726, -0.00163421, -0.00165940, -0.00157263, -0.00139811, -0.00116207, -0.00089063, -0.00060798, -0.00033502, -0.00008841, 0.00011983, 0.00028243, 0.00039657, 0.00046326, 0.00048667, 0.00047320, 0.00043068, 0.00036751, 0.00029198, 0.00021168, 0.00013310, 0.00006133, 0.00000000});
253
254
        filter1_wins[128] = std::vector<double> ({0.0000000, 0.00006032, 0.00013087, 0.00020813, 0.00028721, 0.00036184, 0.00042472, 0.00046782, 0.00048300, 0.00046261, 0.00040033, 0.00029193, 0.00013614, -0.00006466, -0.00030387, -0.00057035, -0.00084844, -0.00111836, -0.00135701, -0.00153925, -0.00163957, -0.00163404, -0.00150263, -0.00123147, -0.00081513, -0.00025856, 0.00042147, 0.00119559, 0.00202193, 0.00284727, 0.00360921, 0.00423937, 0.00466769, 0.00482736, 0.00466043, 0.00412343, 0.00319299, 0.00187056, 0.00018626, -0.00179892, -0.00399265, -0.00627295, -0.00849203, -0.01048226, -0.01206421, -0.01305618, -0.01328498, -0.01259712, -0.01086990, -0.00802144, -0.00401917, 0.00111407, 0.00729683, 0.01438906, 0.02219567, 0.03047337, 0.03894080, 0.04729127, 0.05520746, 0.06237732, 0.06851018, 0.07335203, 0.07669920, 0.07840938, 0.07840938, 0.07669920, 0.07335203, 0.06851018, 0.06237732, 0.05520746, 0.04729127, 0.03894080, 0.03047337, 0.02219567, 0.01438906, 0.00729683, 0.00111407, -0.00401917, -0.00802144, -0.01086990, -0.01259712, -0.01328498, -0.01305618, -0.01206421, -0.01048226, -0.00849203, -0.00627295, -0.00399265, -0.00179892, 0.00018626, 0.00187056, 0.00319299, 0.00412343, 0.00466043, 0.00482736, 0.00466769, 0.00423937, 0.00360921, 0.00284727, 0.00202193, 0.00119559, 0.00042147, -0.00025856, -0.00081513, -0.00123147, -0.00150263, -0.00163404, -0.00163957, -0.00153925, -0.00135701, -0.00111836, -0.00084844, -0.00057035, -0.00030387, -0.00006466, 0.00013614, 0.00029193, 0.00040033, 0.00046261, 0.00048300, 0.00046782, 0.00042472, 0.00036184, 0.00028721, 0.00020813, 0.00013087, 0.00006032, 0.00000000});
        filter1_wins[129] = std::vector<double> ({0.0000000, 0.00005934, 0.00012869, 0.00020467, 0.00028254, 0.00035629, 0.00041885, 0.00046247, 0.00047925, 0.00046172, 0.00040364, 0.00030074, 0.00015153, -0.00004204, -0.00027398, -0.00053397, -0.00080733, -0.00107531, -0.00131591, -0.00150492, -0.00161756, -0.00163027, -0.00152288, -0.00128078, -0.00089712, -0.00037471, 0.00027244, 0.00101835, 0.00182487, 0.00264267, 0.00341299, 0.00407058, 0.00454748, 0.00477771, 0.00470236, 0.00427512, 0.00346756, 0.00227400, 0.00071541, -0.00115799, -0.00326580, -0.00549837, -0.00771983, -0.00977320, -0.01148740, -0.01268595, -0.01319690, -0.01286345, -0.01155462, -0.00917531, -0.00567502, -0.00105456, 0.00462973, 0.01126474, 0.01868264, 0.02666616, 0.03495694, 0.04326671, 0.05129062, 0.05872201, 0.06526776, 0.07066341, 0.07468700, 0.07717085, 0.07801061, 0.07717085, 0.07468700, 0.07066341, 0.06526776, 0.05872201, 0.05129062, 0.04326671, 0.03495694, 0.02666616, 0.01868264, 0.01126474, 0.00462973, -0.00105456, -0.00567502, -0.00917531, -0.01155462, -0.01286345, -0.01319690, -0.01268595, -0.01148740, -0.00977320, -0.00771983, -0.00549837, -0.00326580, -0.00115799, 0.00071541, 0.00227400, 0.00346756, 0.00427512, 0.00470236, 0.00477771, 0.00454748, 0.00407058, 0.00341299, 0.00264267, 0.00182487, 0.00101835, 0.00027244, -0.00037471, -0.00089712, -0.00128078, -0.00152288, -0.00163027, -0.00161756, -0.00150492, -0.00131591, -0.00107531, -0.00080733, -0.00053397, -0.00027398, -0.00004204, 0.00015153, 0.00030074, 0.00040364, 0.00046172, 0.00047925, 0.00046247, 0.00041885, 0.00035629, 0.00028254, 0.00020467, 0.00012869, 0.00005934, 0.00000000});
255
        filter1_wins[130] = std::vector<double> ({0.0000000, 0.00005838, 0.00012656, 0.00020129, 0.00027798, 0.00035085, 0.00041307, 0.00045715, 0.00047542, 0.00046059, 0.00040653, 0.00030891, 0.00016604, -0.00002050, -0.00024531, -0.00049884, -0.00076730, -0.00103299, -0.00127490, -0.00146980, -0.00159362, -0.00162323, -0.00153839, -0.00132389, -0.00097164, -0.00048253, 0.00013202, 0.00084913, 0.00163423, 0.00244167, 0.00321631, 0.00389604, 0.00441521, 0.00470894, 0.00471789, 0.00439349, 0.00370304, 0.00263454, 0.00120069, -0.00055827, -0.00257310, -0.00474597, -0.00695272, -0.00904710, -0.01086688, -0.01224173, -0.01300229, -0.01299017, -0.01206820, -0.01013030, -0.00711036, -0.00298945, 0.00219920, 0.00836785, 0.01537498, 0.02302900, 0.03109511, 0.03930484, 0.04736802, 0.05498630, 0.06186771, 0.06774131, 0.07237101, 0.07556792, 0.07720023, 0.07720023, 0.07556792, 0.07237101, 0.06774131, 0.06186771, 0.05498630, 0.04736802, 0.03930484, 0.03109511, 0.02302900, 0.01537498, 0.00836785, 0.00219920, -0.00298945, -0.00711036, -0.01013030, -0.01206820, -0.01299017, -0.01300229, -0.01224173, -0.01086688, -0.00904710, -0.00695272, -0.00474597, -0.00257310, -0.00055827, 0.00120069, 0.00263454, 0.00370304, 0.00439349, 0.00471789, 0.00470894, 0.00441521, 0.00389604, 0.00321631, 0.00244167, 0.00163423, 0.00084913, 0.00013202, -0.00048253, -0.00097164, -0.00132389, -0.00153839, -0.00162323, -0.00159362, -0.00146980, -0.00127490, -0.00103299, -0.00076730, -0.00049884, -0.00024531, -0.00002050, 0.00016604, 0.00030891, 0.00040653, 0.00046059, 0.00047542, 0.00045715, 0.00041307, 0.00035085, 0.00027798, 0.00020129, 0.00012656, 0.00005838, 0.00000000});
256
257
258
259
260
261
        // filter1_wins[131] = std::vector<double> ({0.0000000, 0.00005745, 0.00012449, 0.00019798, 0.00027353, 0.00034552, 0.00040738, 0.00045187, 0.00047152, 0.00045926, 0.00040902, 0.00031647, 0.00017972, -0.00000000, -0.00021783, -0.00046492, -0.00072837, -0.00099144, -0.00123410, -0.00143406, -0.00156800, -0.00161321, -0.00154951, -0.00136119, -0.00103908, -0.00058233, 0.00000000, 0.00068795, 0.00145028, 0.00224491, 0.00302022, 0.00371723, 0.00427276, 0.00462330, 0.00470950, 0.00448108, 0.00390176, 0.00295400, 0.00164304, -0.00000000, -0.00191637, -0.00401950, -0.00619658, -0.00831209, -0.01021301, -0.01173589, -0.01271511, -0.01299223, -0.01242574, -0.01090070, -0.00833761, -0.00469999, 0.00000000, 0.00569833, 0.01227855, 0.01957425, 0.02737432, 0.03543109, 0.04347079, 0.05120593, 0.05834884, 0.06462572, 0.06979033, 0.07363650, 0.07600875, 0.07681043, 0.07600875, 0.07363650, 0.06979033, 0.06462572, 0.05834884, 0.05120593, 0.04347079, 0.03543109, 0.02737432, 0.01957425, 0.01227855, 0.00569833, 0.00000000, -0.00469999, -0.00833761, -0.01090070, -0.01242574, -0.01299223, -0.01271511, -0.01173589, -0.01021301, -0.00831209, -0.00619658, -0.00401950, -0.00191637, -0.00000000, 0.00164304, 0.00295400, 0.00390176, 0.00448108, 0.00470950, 0.00462330, 0.00427276, 0.00371723, 0.00302022, 0.00224491, 0.00145028, 0.00068795, 0.00000000, -0.00058233, -0.00103908, -0.00136119, -0.00154951, -0.00161321, -0.00156800, -0.00143406, -0.00123410, -0.00099144, -0.00072837, -0.00046492, -0.00021783, -0.00000000, 0.00017972, 0.00031647, 0.00040902, 0.00045926, 0.00047152, 0.00045187, 0.00040738, 0.00034552, 0.00027353, 0.00019798, 0.00012449, 0.00005745, 0.00000000});
        // filter1_wins[132] = std::vector<double> ({0.00000000, 0.00005653, 0.00012247, 0.00019476, 0.00026917, 0.00034030, 0.00040178, 0.00044662, 0.00046757, 0.00045774, 0.00041114, 0.00032344, 0.00019259, 0.00001950, -0.00019149, -0.00043219, -0.00069053, -0.00095068, -0.00119359, -0.00139785, -0.00154089, -0.00160050, -0.00155658, -0.00139306, -0.00109980, -0.00067442, -0.00012382, 0.00053476, 0.00127324, 0.00205293, 0.00282562, 0.00353548, 0.00412188, 0.00452290, 0.00467954, 0.00454033, 0.00406604, 0.00323430, 0.00204368, 0.00051694, -0.00129691, -0.00332196, -0.00545641, -0.00757531, -0.00953510, -0.01117976, -0.01234839, -0.01288384, -0.01264193, -0.01150079, -0.00936964, -0.00619658, -0.00197469, 0.00325391, 0.00939656, 0.01631117, 0.02381022, 0.03166741, 0.03962676, 0.04741369, 0.05474760, 0.06135514, 0.06698354, 0.07141316, 0.07446862, 0.07602771, 0.07602771, 0.07446862, 0.07141316, 0.06698354, 0.06135514, 0.05474760, 0.04741369, 0.03962676, 0.03166741, 0.02381022, 0.01631117, 0.00939656, 0.00325391, -0.00197469, -0.00619658, -0.00936964, -0.01150079, -0.01264193, -0.01288384, -0.01234839, -0.01117976, -0.00953510, -0.00757531, -0.00545641, -0.00332196, -0.00129691, 0.00051694, 0.00204368, 0.00323430, 0.00406604, 0.00454033, 0.00467954, 0.00452290, 0.00412188, 0.00353548, 0.00282562, 0.00205293, 0.00127324, 0.00053476, -0.00012382, -0.00067442, -0.00109980, -0.00139306, -0.00155658, -0.00160050, -0.00154089, -0.00139785, -0.00119359, -0.00095068, -0.00069053, -0.00043219, -0.00019149, 0.00001950, 0.00019259, 0.00032344, 0.00041114, 0.00045774, 0.00046757, 0.00044662, 0.00040178, 0.00034030, 0.00026917, 0.00019476, 0.00012247, 0.00005653, 0.00000000});
        // filter1_wins[133] = std::vector<double> ({0.00000000, 0.00005564, 0.00012049, 0.00019161, 0.00026491, 0.00033518, 0.00039627, 0.00044141, 0.00046357, 0.00045603, 0.00041292, 0.00032987, 0.00020471, 0.00003804, -0.00016626, -0.00040064, -0.00065379, -0.00091076, -0.00115344, -0.00136129, -0.00151250, -0.00158536, -0.00155993, -0.00141985, -0.00115418, -0.00075915, -0.00023968, 0.00038950, 0.00110327, 0.00186617, 0.00263331, 0.00335198, 0.00396414, 0.00440968, 0.00463025, 0.00457363, 0.00419820, 0.00347745, 0.00240402, 0.00099305, -0.00071549, -0.00265573, -0.00473642, -0.00684302, -0.00884148, -0.01058368, -0.01191423, -0.01267843, -0.01273093, -0.01194468, -0.01021959, -0.00749036, -0.00373300, 0.00103052, 0.00672983, 0.01324621, 0.02041531, 0.02803247, 0.03586045, 0.04363932, 0.05109788, 0.05796617, 0.06398824, 0.06893459, 0.07261353, 0.07488075, 0.07564662, 0.07488075, 0.07261353, 0.06893459, 0.06398824, 0.05796617, 0.05109788, 0.04363932, 0.03586045, 0.02803247, 0.02041531, 0.01324621, 0.00672983, 0.00103052, -0.00373300, -0.00749036, -0.01021959, -0.01194468, -0.01273093, -0.01267843, -0.01191423, -0.01058368, -0.00884148, -0.00684302, -0.00473642, -0.00265573, -0.00071549, 0.00099305, 0.00240402, 0.00347745, 0.00419820, 0.00457363, 0.00463025, 0.00440968, 0.00396414, 0.00335198, 0.00263331, 0.00186617, 0.00110327, 0.00038950, -0.00023968, -0.00075915, -0.00115418, -0.00141985, -0.00155993, -0.00158536, -0.00151250, -0.00136129, -0.00115344, -0.00091076, -0.00065379, -0.00040064, -0.00016626, 0.00003804, 0.00020471, 0.00032987, 0.00041292, 0.00045603, 0.00046357, 0.00044141, 0.00039627, 0.00033518, 0.00026491, 0.00019161, 0.00012049, 0.00005564, 0.00000000});
        // filter1_wins[134] = std::vector<double> ({0.00000000, 0.00005477, 0.00011856, 0.00018854, 0.00026075, 0.00033016, 0.00039084, 0.00043624, 0.00045953, 0.00045417, 0.00041436, 0.00033578, 0.00021610, 0.00005565, -0.00014210, -0.00037021, -0.00061812, -0.00087170, -0.00111373, -0.00132452, -0.00148301, -0.00156803, -0.00155985, -0.00144191, -0.00120258, -0.00083684, -0.00034783, 0.00025205, 0.00094045, 0.00168501, 0.00244401, 0.00316780, 0.00380101, 0.00428546, 0.00456373, 0.00458325, 0.00430053, 0.00368553, 0.00272562, 0.00142908, -0.00017247, -0.00202259, -0.00404010, -0.00612059, -0.00813954, -0.00995703, -0.01142382, -0.01238864, -0.01270630, -0.01224617, -0.01090066, -0.00859301, -0.00528407, -0.00097750, 0.00427712, 0.01038328, 0.01719918, 0.02454176, 0.03219319, 0.03990950, 0.04743087, 0.05449330, 0.06084069, 0.06623703, 0.07047781, 0.07340000, 0.07489018, 0.07489018, 0.07340000, 0.07047781, 0.06623703, 0.06084069, 0.05449330, 0.04743087, 0.03990950, 0.03219319, 0.02454176, 0.01719918, 0.01038328, 0.00427712, -0.00097750, -0.00528407, -0.00859301, -0.01090066, -0.01224617, -0.01270630, -0.01238864, -0.01142382, -0.00995703, -0.00813954, -0.00612059, -0.00404010, -0.00202259, -0.00017247, 0.00142908, 0.00272562, 0.00368553, 0.00430053, 0.00458325, 0.00456373, 0.00428546, 0.00380101, 0.00316780, 0.00244401, 0.00168501, 0.00094045, 0.00025205, -0.00034783, -0.00083684, -0.00120258, -0.00144191, -0.00155985, -0.00156803, -0.00148301, -0.00132452, -0.00111373, -0.00087170, -0.00061812, -0.00037021, -0.00014210, 0.00005565, 0.00021610, 0.00033578, 0.00041436, 0.00045417, 0.00045953, 0.00043624, 0.00039084, 0.00033016, 0.00026075, 0.00018854, 0.00011856, 0.00005477, 0.00000000});
        // filter1_wins[135] = std::vector<double> ({0.00000000, 0.00005392, 0.00011668, 0.00018554, 0.00025668, 0.00032524, 0.00038550, 0.00043111, 0.00045546, 0.00045215, 0.00041551, 0.00034120, 0.00022680, 0.00007239, -0.00011897, -0.00034091, -0.00058352, -0.00083352, -0.00107450, -0.00128764, -0.00145259, -0.00154874, -0.00155663, -0.00145956, -0.00124533, -0.00090782, -0.00044853, 0.00012227, 0.00078484, 0.00150973, 0.00225829, 0.00298389, 0.00363380, 0.00415191, 0.00448197, 0.00457138, 0.00437527, 0.00386060, 0.00301018, 0.00182603, 0.00033217, -0.00142380, -0.00337028, -0.00541264, -0.00743582, -0.00930827, -0.01088742, -0.01202627, -0.01258093, -0.01241865, -0.01142598, -0.00951653, -0.00663788, -0.00277710, 0.00203537, 0.00772402, 0.01416875, 0.02120787, 0.02864326, 0.03624789, 0.04377499, 0.05096868, 0.05757545, 0.06335588, 0.06809591, 0.07161711, 0.07378538, 0.07451754, 0.07378538, 0.07161711, 0.06809591, 0.06335588, 0.05757545, 0.05096868, 0.04377499, 0.03624789, 0.02864326, 0.02120787, 0.01416875, 0.00772402, 0.00203537, -0.00277710, -0.00663788, -0.00951653, -0.01142598, -0.01241865, -0.01258093, -0.01202627, -0.01088742, -0.00930827, -0.00743582, -0.00541264, -0.00337028, -0.00142380, 0.00033217, 0.00182603, 0.00301018, 0.00386060, 0.00437527, 0.00457138, 0.00448197, 0.00415191, 0.00363380, 0.00298389, 0.00225829, 0.00150973, 0.00078484, 0.00012227, -0.00044853, -0.00090782, -0.00124533, -0.00145956, -0.00155663, -0.00154874, -0.00145259, -0.00128764, -0.00107450, -0.00083352, -0.00058352, -0.00034091, -0.00011897, 0.00007239, 0.00022680, 0.00034120, 0.00041551, 0.00045215, 0.00045546, 0.00043111, 0.00038550, 0.00032524, 0.00025668, 0.00018554, 0.00011668, 0.00005392, 0.00000000});
        // filter1_wins[136] = std::vector<double> ({0.00000000, 0.00005309, 0.00011484, 0.00018260, 0.00025269, 0.00032042, 0.00038025, 0.00042603, 0.00045136, 0.00044999, 0.00041636, 0.00034615, 0.00023684, 0.00008828, -0.00009685, -0.00031268, -0.00054999, -0.00079623, -0.00103582, -0.00125074, -0.00142140, -0.00152772, -0.00155053, -0.00147312, -0.00128277, -0.00097241, -0.00054206, 0.00000000, 0.00063643, 0.00134056, 0.00207668, 0.00280107, 0.00346369, 0.00401057, 0.00438681, 0.00454010, 0.00442458, 0.00400477, 0.00325945, 0.00218512, 0.00079877, -0.00086019, -0.00272920, -0.00472308, -0.00673604, -0.00864500, -0.01031443, -0.01160231, -0.01236702, -0.01247500, -0.01180847, -0.01027311, -0.00780496, -0.00437630, 0.00000000, 0.00526806, 0.01132855, 0.01804062, 0.02522598, 0.03267520, 0.04015581, 0.04742198, 0.05422517, 0.06032532, 0.06550192, 0.06956431, 0.07236084, 0.07378611, 0.07378611, 0.07236084, 0.06956431, 0.06550192, 0.06032532, 0.05422517, 0.04742198, 0.04015581, 0.03267520, 0.02522598, 0.01804062, 0.01132855, 0.00526806, 0.00000000, -0.00437630, -0.00780496, -0.01027311, -0.01180847, -0.01247500, -0.01236702, -0.01160231, -0.01031443, -0.00864500, -0.00673604, -0.00472308, -0.00272920, -0.00086019, 0.00079877, 0.00218512, 0.00325945, 0.00400477, 0.00442458, 0.00454010, 0.00438681, 0.00401057, 0.00346369, 0.00280107, 0.00207668, 0.00134056, 0.00063643, 0.00000000, -0.00054206, -0.00097241, -0.00128277, -0.00147312, -0.00155053, -0.00152772, -0.00142140, -0.00125074, -0.00103582, -0.00079623, -0.00054999, -0.00031268, -0.00009685, 0.00008828, 0.00023684, 0.00034615, 0.00041636, 0.00044999, 0.00045136, 0.00042603, 0.00038025, 0.00032042, 0.00025269, 0.00018260, 0.00011484, 0.00005309, 0.00000000});
262

263
    } else if (freq_in == 640) {
264

265
        // Created with MATLAB: fir1(640,5/320,gausswin(641));
266
        filter1_wins[640] = std::vector<double> ({0.00000000, 0.00000219, 0.00000448, 0.00000685, 0.00000932, 0.00001187, 0.00001450, 0.00001721, 0.00001999, 0.00002283, 0.00002574, 0.00002870, 0.00003170, 0.00003475, 0.00003783, 0.00004093, 0.00004405, 0.00004718, 0.00005030, 0.00005342, 0.00005651, 0.00005957, 0.00006258, 0.00006554, 0.00006844, 0.00007126, 0.00007399, 0.00007662, 0.00007913, 0.00008152, 0.00008377, 0.00008587, 0.00008780, 0.00008956, 0.00009113, 0.00009249, 0.00009364, 0.00009457, 0.00009525, 0.00009568, 0.00009585, 0.00009574, 0.00009535, 0.00009465, 0.00009366, 0.00009234, 0.00009070, 0.00008873, 0.00008641, 0.00008375, 0.00008073, 0.00007735, 0.00007360, 0.00006949, 0.00006500, 0.00006015, 0.00005492, 0.00004932, 0.00004335, 0.00003701, 0.00003031, 0.00002325, 0.00001584, 0.00000809, -0.00000000, -0.00000841, -0.00001712, -0.00002614, -0.00003543, -0.00004499, -0.00005480, -0.00006483, -0.00007507, -0.00008549, -0.00009607, -0.00010679, -0.00011762, -0.00012853, -0.00013950, -0.00015049, -0.00016146, -0.00017240, -0.00018327, -0.00019402, -0.00020463, -0.00021506, -0.00022527, -0.00023523, -0.00024489, -0.00025422, -0.00026318, -0.00027172, -0.00027982, -0.00028742, -0.00029449, -0.00030098, -0.00030687, -0.00031210, -0.00031664, -0.00032046, -0.00032351, -0.00032576, -0.00032717, -0.00032771, -0.00032735, -0.00032605, -0.00032379, -0.00032054, -0.00031626, -0.00031095, -0.00030457, -0.00029711, -0.00028855, -0.00027888, -0.00026808, -0.00025615, -0.00024308, -0.00022887, -0.00021352, -0.00019704, -0.00017942, -0.00016069, -0.00014086, -0.00011994, -0.00009796, -0.00007494, -0.00005092, -0.00002593, 0.00000000, 0.00002682, 0.00005449, 0.00008295, 0.00011216, 0.00014206, 0.00017258, 0.00020367, 0.00023524, 0.00026724, 0.00029958, 0.00033218, 0.00036497, 0.00039786, 0.00043075, 0.00046355, 0.00049618, 0.00052853, 0.00056051, 0.00059200, 0.00062292, 0.00065315, 0.00068259, 0.00071113, 0.00073866, 0.00076508, 0.00079026, 0.00081411, 0.00083651, 0.00085736, 0.00087654, 0.00089395, 0.00090949, 0.00092305, 0.00093453, 0.00094383, 0.00095087, 0.00095553, 0.00095775, 0.00095743, 0.00095449, 0.00094886, 0.00094046, 0.00092924, 0.00091514, 0.00089810, 0.00087807, 0.00085502, 0.00082891, 0.00079971, 0.00076742, 0.00073202, 0.00069351, 0.00065189, 0.00060718, 0.00055940, 0.00050859, 0.00045480, 0.00039806, 0.00033844, 0.00027602, 0.00021087, 0.00014308, 0.00007275, -0.00000000, -0.00007506, -0.00015231, -0.00023160, -0.00031278, -0.00039570, -0.00048018, -0.00056607, -0.00065316, -0.00074126, -0.00083018, -0.00091970, -0.00100960, -0.00109966, -0.00118965, -0.00127932, -0.00136843, -0.00145673, -0.00154395, -0.00162984, -0.00171413, -0.00179653, -0.00187679, -0.00195462, -0.00202974, -0.00210187, -0.00217072, -0.00223601, -0.00229746, -0.00235478, -0.00240769, -0.00245591, -0.00249916, -0.00253717, -0.00256967, -0.00259639, -0.00261708, -0.00263149, -0.00263936, -0.00264046, -0.00263455, -0.00262143, -0.00260086, -0.00257267, -0.00253664, -0.00249262, -0.00244042, -0.00237989, -0.00231090, -0.00223332, -0.00214703, -0.00205195, -0.00194797, -0.00183505, -0.00171312, -0.00158216, -0.00144215, -0.00129309, -0.00113499, -0.00096790, -0.00079187, -0.00060696, -0.00041327, -0.00021091, 0.00000000, 0.00021931, 0.00044686, 0.00068247, 0.00092594, 0.00117705, 0.00143556, 0.00170123, 0.00197378, 0.00225293, 0.00253837, 0.00282979, 0.00312686, 0.00342921, 0.00373650, 0.00404833, 0.00436433, 0.00468408, 0.00500718, 0.00533319, 0.00566167, 0.00599219, 0.00632428, 0.00665748, 0.00699133, 0.00732533, 0.00765902, 0.00799190, 0.00832348, 0.00865327, 0.00898077, 0.00930550, 0.00962694, 0.00994462, 0.01025804, 0.01056670, 0.01087014, 0.01116786, 0.01145941, 0.01174430, 0.01202209, 0.01229233, 0.01255459, 0.01280843, 0.01305344, 0.01328923, 0.01351540, 0.01373158, 0.01393741, 0.01413256, 0.01431670, 0.01448952, 0.01465073, 0.01480006, 0.01493727, 0.01506212, 0.01517440, 0.01527392, 0.01536052, 0.01543404, 0.01549436, 0.01554139, 0.01557504, 0.01559525, 0.01560199, 0.01559525, 0.01557504, 0.01554139, 0.01549436, 0.01543404, 0.01536052, 0.01527392, 0.01517440, 0.01506212, 0.01493727, 0.01480006, 0.01465073, 0.01448952, 0.01431670, 0.01413256, 0.01393741, 0.01373158, 0.01351540, 0.01328923, 0.01305344, 0.01280843, 0.01255459, 0.01229233, 0.01202209, 0.01174430, 0.01145941, 0.01116786, 0.01087014, 0.01056670, 0.01025804, 0.00994462, 0.00962694, 0.00930550, 0.00898077, 0.00865327, 0.00832348, 0.00799190, 0.00765902, 0.00732533, 0.00699133, 0.00665748, 0.00632428, 0.00599219, 0.00566167, 0.00533319, 0.00500718, 0.00468408, 0.00436433, 0.00404833, 0.00373650, 0.00342921, 0.00312686, 0.00282979, 0.00253837, 0.00225293, 0.00197378, 0.00170123, 0.00143556, 0.00117705, 0.00092594, 0.00068247, 0.00044686, 0.00021931, 0.00000000, -0.00021091, -0.00041327, -0.00060696, -0.00079187, -0.00096790, -0.00113499, -0.00129309, -0.00144215, -0.00158216, -0.00171312, -0.00183505, -0.00194797, -0.00205195, -0.00214703, -0.00223332, -0.00231090, -0.00237989, -0.00244042, -0.00249262, -0.00253664, -0.00257267, -0.00260086, -0.00262143, -0.00263455, -0.00264046, -0.00263936, -0.00263149, -0.00261708, -0.00259639, -0.00256967, -0.00253717, -0.00249916, -0.00245591, -0.00240769, -0.00235478, -0.00229746, -0.00223601, -0.00217072, -0.00210187, -0.00202974, -0.00195462, -0.00187679, -0.00179653, -0.00171413, -0.00162984, -0.00154395, -0.00145673, -0.00136843, -0.00127932, -0.00118965, -0.00109966, -0.00100960, -0.00091970, -0.00083018, -0.00074126, -0.00065316, -0.00056607, -0.00048018, -0.00039570, -0.00031278, -0.00023160, -0.00015231, -0.00007506, -0.00000000, 0.00007275, 0.00014308, 0.00021087, 0.00027602, 0.00033844, 0.00039806, 0.00045480, 0.00050859, 0.00055940, 0.00060718, 0.00065189, 0.00069351, 0.00073202, 0.00076742, 0.00079971, 0.00082891, 0.00085502, 0.00087807, 0.00089810, 0.00091514, 0.00092924, 0.00094046, 0.00094886, 0.00095449, 0.00095743, 0.00095775, 0.00095553, 0.00095087, 0.00094383, 0.00093453, 0.00092305, 0.00090949, 0.00089395, 0.00087654, 0.00085736, 0.00083651, 0.00081411, 0.00079026, 0.00076508, 0.00073866, 0.00071113, 0.00068259, 0.00065315, 0.00062292, 0.00059200, 0.00056051, 0.00052853, 0.00049618, 0.00046355, 0.00043075, 0.00039786, 0.00036497, 0.00033218, 0.00029958, 0.00026724, 0.00023524, 0.00020367, 0.00017258, 0.00014206, 0.00011216, 0.00008295, 0.00005449, 0.00002682, 0.00000000, -0.00002593, -0.00005092, -0.00007494, -0.00009796, -0.00011994, -0.00014086, -0.00016069, -0.00017942, -0.00019704, -0.00021352, -0.00022887, -0.00024308, -0.00025615, -0.00026808, -0.00027888, -0.00028855, -0.00029711, -0.00030457, -0.00031095, -0.00031626, -0.00032054, -0.00032379, -0.00032605, -0.00032735, -0.00032771, -0.00032717, -0.00032576, -0.00032351, -0.00032046, -0.00031664, -0.00031210, -0.00030687, -0.00030098, -0.00029449, -0.00028742, -0.00027982, -0.00027172, -0.00026318, -0.00025422, -0.00024489, -0.00023523, -0.00022527, -0.00021506, -0.00020463, -0.00019402, -0.00018327, -0.00017240, -0.00016146, -0.00015049, -0.00013950, -0.00012853, -0.00011762, -0.00010679, -0.00009607, -0.00008549, -0.00007507, -0.00006483, -0.00005480, -0.00004499, -0.00003543, -0.00002614, -0.00001712, -0.00000841, -0.00000000, 0.00000809, 0.00001584, 0.00002325, 0.00003031, 0.00003701, 0.00004335, 0.00004932, 0.00005492, 0.00006015, 0.00006500, 0.00006949, 0.00007360, 0.00007735, 0.00008073, 0.00008375, 0.00008641, 0.00008873, 0.00009070, 0.00009234, 0.00009366, 0.00009465, 0.00009535, 0.00009574, 0.00009585, 0.00009568, 0.00009525, 0.00009457, 0.00009364, 0.00009249, 0.00009113, 0.00008956, 0.00008780, 0.00008587, 0.00008377, 0.00008152, 0.00007913, 0.00007662, 0.00007399, 0.00007126, 0.00006844, 0.00006554, 0.00006258, 0.00005957, 0.00005651, 0.00005342, 0.00005030, 0.00004718, 0.00004405, 0.00004093, 0.00003783, 0.00003475, 0.00003170, 0.00002870, 0.00002574, 0.00002283, 0.00001999, 0.00001721, 0.00001450, 0.00001187, 0.00000932, 0.00000685, 0.00000448, 0.00000219, 0.00000000});
267
268
269

    } else {

270
        return (-1);
271
272
273

    }

274
275
276
277
278
279
280
281
    // Record the minimum and maximum filter sizes
    int min_filter1 = filter1_wins.begin()->first;
    int max_filter1 = filter1_wins.rbegin()->first;

    // Debug printing for filter lengths
    std::cerr << "Min Filter 1 Len: " << min_filter1 << std::endl;    
    std::cerr << "Max Filter 1 Len: " << max_filter1 << std::endl;    

282
    // =====================================================================
283
    // Filter2 downsamples to 1 Hz with a cut-off frequency of XXX Hz
284
    // =====================================================================
285

286
    std::vector <double> filter2_win {0.00006417, 0.00007841, 0.00009291, 0.00010743, 0.00012169, 0.00013541, 0.00014827, 0.00015992, 0.00017001, 0.00017819, 0.00018409, 0.00018735, 0.00018762, 0.00018456, 0.00017787, 0.00016727, 0.00015255, 0.00013351, 0.00011006, 0.00008214, 0.00004980, 0.00001314, -0.00002761, -0.00007216, -0.00012009, -0.00017089, -0.00022395, -0.00027856, -0.00033390, -0.00038907, -0.00044307, -0.00049485, -0.00054327, -0.00058715, -0.00062530, -0.00065649, -0.00067952, -0.00069321, -0.00069646, -0.00068821, -0.00066755, -0.00063369, -0.00058598, -0.00052399, -0.00044747, -0.00035643, -0.00025112, -0.00013204, 0.00000000, 0.00014390, 0.00029827, 0.00046141, 0.00063134, 0.00080578, 0.00098218, 0.00115777, 0.00132951, 0.00149423, 0.00164858, 0.00178912, 0.00191235, 0.00201478, 0.00209301, 0.00214372, 0.00216381, 0.00215043, 0.00210107, 0.00201359, 0.00188631, 0.00171807, 0.00150829, 0.00125701, 0.00096496, 0.00063355, 0.00026496, -0.00013787, -0.00057125, -0.00103071, -0.00151101, -0.00200620, -0.00250963, -0.00301402, -0.00351151, -0.00399372, -0.00445189, -0.00487692, -0.00525952, -0.00559029, -0.00585988, -0.00605909, -0.00617901, -0.00621117, -0.00614765, -0.00598123, -0.00570551, -0.00531503, -0.00480541, -0.00417341, -0.00341705, -0.00253570, -0.00153009, -0.00040242, 0.00084364, 0.00220297, 0.00366896, 0.00523358, 0.00688740, 0.00861970, 0.01041852, 0.01227080, 0.01416249, 0.01607870, 0.01800386, 0.01992185, 0.02181626, 0.02367050, 0.02546803, 0.02719254, 0.02882815, 0.03035961, 0.03177248, 0.03305330, 0.03418976, 0.03517086, 0.03598706, 0.03663036, 0.03709444, 0.03737474, 0.03746848, 0.03737474, 0.03709444, 0.03663036, 0.03598706, 0.03517086, 0.03418976, 0.03305330, 0.03177248, 0.03035961, 0.02882815, 0.02719254, 0.02546803, 0.02367050, 0.02181626, 0.01992185, 0.01800386, 0.01607870, 0.01416249, 0.01227080, 0.01041852, 0.00861970, 0.00688740, 0.00523358, 0.00366896, 0.00220297, 0.00084364, -0.00040242, -0.00153009, -0.00253570, -0.00341705, -0.00417341, -0.00480541, -0.00531503, -0.00570551, -0.00598123, -0.00614765, -0.00621117, -0.00617901, -0.00605909, -0.00585988, -0.00559029, -0.00525952, -0.00487692, -0.00445189, -0.00399372, -0.00351151, -0.00301402, -0.00250963, -0.00200620, -0.00151101, -0.00103071, -0.00057125, -0.00013787, 0.00026496, 0.00063355, 0.00096496, 0.00125701, 0.00150829, 0.00171807, 0.00188631, 0.00201359, 0.00210107, 0.00215043, 0.00216381, 0.00214372, 0.00209301, 0.00201478, 0.00191235, 0.00178912, 0.00164858, 0.00149423, 0.00132951, 0.00115777, 0.00098218, 0.00080578, 0.00063134, 0.00046141, 0.00029827, 0.00014390, 0.00000000, -0.00013204, -0.00025112, -0.00035643, -0.00044747, -0.00052399, -0.00058598, -0.00063369, -0.00066755, -0.00068821, -0.00069646, -0.00069321, -0.00067952, -0.00065649, -0.00062530, -0.00058715, -0.00054327, -0.00049485, -0.00044307, -0.00038907, -0.00033390, -0.00027856, -0.00022395, -0.00017089, -0.00012009, -0.00007216, -0.00002761, 0.00001314, 0.00004980, 0.00008214, 0.00011006, 0.00013351, 0.00015255, 0.00016727, 0.00017787, 0.00018456, 0.00018762, 0.00018735, 0.00018409, 0.00017819, 0.00017001, 0.00015992, 0.00014827, 0.00013541, 0.00012169, 0.00010743, 0.00009291, 0.00007841, 0.00006417};
287
    
288
289
290
291
292
293
294
    // Output frequency of filter2 in Hz      
    int filter2_freq = 1;
    // Number of blocks required to run filter2
    int filter2_block_len = filter2_win.size();
    // Number of output blocks between filter2 runs
    int filter2_block_step = filter1_freq / filter2_freq;

295
    std::cerr << "FILTER 2 LEN: " << filter2_win.size() << std::endl;    
296
297
298
299
300

    // =====================================================================
    // Load initial number of points for filter1.
    // =====================================================================

301
302
    while(1) {
        // wait for data to arrive
303
        buffer_in->pop(data);
304
        while ( data->get_sec()  == 0 ) {
305
        buffer_in->pop(data);
306
        }
307

308
309
310
311
312
313
        // Stall to get the center of minute
        buffer_in->pop(data);
        int sec = data->get_sec();
        while ( ( data->get_msec() < 500e6 ) || ( data->get_sec() == sec ) ) {
            buffer_in->pop(data);
        }
314
        
315
        std::cerr << "------------- START  PLASMON -------------" << std::endl;
316

317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
        // Add the first data point
        // buffer_in->pop(data);
        tofilter1.push_back(new data_obs_vector(data));
        // record the time
        data->get_time(&this_timespec);
        last_timeblock_edge = this_timespec;

        // Initialize counters
        float time_block_check = 0;
        int filter1_block_count = 1;
        int filter1_block_numdata = 1;
        int filter2_count = filter2_block_step; // Apply straight ahead first time
        float total_blocks = 0;
        
        // =================================================
        // Continuously run concatenated filters one and two.
        // =================================================
        //      
        //int counter = 0;
        std::string datastr;
        while (1) {
338

339
340
            // Grab new data
            buffer_in->pop(data);
341

342
343
            tofilter1.back()->get_time(&last_timespec);
            data->get_time(&this_timespec);
Thomas Edwards's avatar
Thomas Edwards committed
344

345
346
347
348
349
350
351
352
353
354
355
356
            //counter++;
            
            data->string(&datastr,5);
            // std::cerr << "INCOMING: " << std::endl;
            // std::cerr << data->get_sec()
            // 	//   << "."
            // 	  << " "
            // 	  << data->get_msec() << " "
            // 	  << datastr << " "
            // 	//   << counter
            // 	  << std::endl;
        
Thomas Edwards's avatar
Thomas Edwards committed
357

358
359
360
361
            // One data point is produced from filter1 for each block of 
            // data, corresponding to its output filter frequency
            // Check here if we hit the next block
            if ( (this_timespec.tv_nsec % filter1_delta_t_msec) < (last_timespec.tv_nsec % filter1_delta_t_msec) ) {
362

363
364
365
366
367
368
369
                // Keep track of the time blocks, end the last one and start the next
                this_timeblock_edge = this_timespec;
                long long elapsed_time_last_block = (((this_timeblock_edge.tv_sec - last_timeblock_edge.tv_sec) * 1e9) + (this_timeblock_edge.tv_nsec - last_timeblock_edge.tv_nsec)); 
                
                // --------------------------------------------------------------
                // Degbugging monitoring info
                // --------------------------------------------------------------
370
                // std::cerr << "-----------------------------------" << std::endl;
371
                // timestamp=tofilter1.back()->get_time("%Y-%m-%d %H:%M:%S",true);
372

373
374
375
                // Determine number of blocks
                float num_blocks = (elapsed_time_last_block/(1e9/16.));
                total_blocks += num_blocks;
Thomas Edwards's avatar
Thomas Edwards committed
376

377
378
379
380
                // // Debug printing
                // std::cerr << "Total blocks: " << total_blocks << std::endl;
                // std::cerr << "Data in que: " << tofilter1.size() << std::endl;
                // std::cerr << "Blocks*8: " << round(8*(total_blocks)) << std::endl;
Thomas Edwards's avatar
Thomas Edwards committed
381

382
383
384
385
386
387
388
389
390
391
392
393
394
                // --------------------------------------------------------------
                // Given the elapsed time, can we run some number of filters to 
                // reduce the amount of data down to what we expect?
                // --------------------------------------------------------------
                // If we find more than one block elapsed, then we've probably skipped some data. Let's hold off and get some extra data first.
                if (round(num_blocks) > 1) {
                    std::cerr << "[!!!] Time skip detected! Waiting for more data." << std::endl;
                }
                // See if we can run at least one filter
                else if (int(total_blocks/16) >= 1) {
                    // save information on how many points we need to drop later
                    int points_to_drop = 0;
                    // Check if we have (8*(blocks) +/- 1) data in the filter que
395
                    time_block_check = round(8*(total_blocks));
396
397
398
399
400
401
402
403
404
                    bool run_the_filter = ((tofilter1.size() <= (time_block_check + 1)) && (tofilter1.size() >= (time_block_check - 1))) && (int(total_blocks/16) >= 1) && (tofilter1.size() >= 129);
                    while (run_the_filter) {
                        // Use the time_block_check information to assume if we are over or under
                        // this is just the +/- part, so we can expand or reduce the filter to match
                        int filter_jitter = 0;

                        // Checking the filter size for perfectly divisible by 8
                        if (tofilter1.size() % 8 != 0 ){
                            slip_count++;
405
                        } else {
406
                            slip_count = 0;
407
                        }
408

409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
                        // Check slip count to see if we need to slip
                        if (slip_count > slip_limit) {
                            std::cerr << "[!!!] Slipping data to fit time!" << std::endl;
                            slip_count = 0;
                            filter_jitter = tofilter1.size() % 8;
                            // make the remainder a +/- 4 instead of 0 to 8
                            if (filter_jitter > 4) {
                                filter_jitter -= 8;
                            }
                        }
                        // // Debug printing
                        // std::cerr << "Latest timestamp: " << timestamp << " | ";
                        // std::cerr << "No. Rec: " << tofilter1.size() << " | ";
                        // std::cerr << "Elapsed time: ";
                        // std::cerr << elapsed_time_last_block << " | ";
                        // std::cerr << "Expected blocks: " << num_blocks << " | ";
                        // std::cerr << "Slip count: " << slip_count << " | ";
                        // std::cerr << "Filters: " << total_blocks/16. << std::endl;
                        // std::cerr << "Data in que: " << tofilter1.size() << " | blocks*8: " << time_block_check << " | Run the filter?: " << run_the_filter << std::endl;
                        // std::cerr << "OOR err, Filter Jitter: " << filter_jitter << " | ";
                    
                        // Filter data
                        filter1_win = &filter1_wins.at(129+filter_jitter);
                        filter1_slice = std::vector<data_obs*>(tofilter1.begin(), tofilter1.begin()+129+filter_jitter);
                        filter_data_temp->filter_fir_sym(&filter1_slice, filter1_win);
                        points_to_drop = 8+filter_jitter;

                        // // Debug printing
                        // std::cerr << "points_to_drop: " << points_to_drop << std::endl;

                        // Append this data to the queue for filter 2
                        tofilter2.push_back(new data_obs_vector(filter_data_temp));

                        // fix total blocks
                        total_blocks = total_blocks*((1.-(points_to_drop / float(tofilter1.size()))));

445
446
447
448
449
                        // Drop the oldest data points, as determined by the oldest block
                        for (int ix = 0; ix < points_to_drop; ix++) {
                            delete(tofilter1[ix]);
                        }
                        tofilter1.erase(tofilter1.begin(), tofilter1.begin() + points_to_drop);
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474

                        // // Debug printing
                        // std::cerr << "Total blocks (post filter): " << total_blocks << std::endl;

                        // remove a time block and update our estimate
                        time_block_check = round(8*(total_blocks));
                        run_the_filter = ((tofilter1.size() <= (time_block_check + 1)) && (tofilter1.size() >= (time_block_check - 1))) && (int(total_blocks/16) >= 1) && (tofilter1.size() >= 129);

                        // Wait until filter2 queue is filled
                        if (tofilter2.size() == filter2_block_len) {

                            // Wait for downsampling and apply filter2
                            if (filter2_count == filter2_block_step) {
                                //data->filter_fir_sym(&tofilter2,&filter2_win);
                                filter_data_temp->filter_fir_sym(&tofilter2, &filter2_win);

                                buffer_out->put(filter_data_temp);
                                filter2_count = 1;
                            } else {
                                filter2_count++;
                            }

                            // Delete oldest data from filter2
                            delete(tofilter2[0]);
                            tofilter2.erase(tofilter2.begin());
475

476
477
478
479
480
481
482
483
484
485
486
487
488
489
                        }
                    }
                } else {
                    // --------------------------------------------------------------
                    // Check the data: Do we have too much or too little?
                    // --------------------------------------------------------------
                    if (tofilter1.size() > max_filter1) {
                        std::cerr << "[!!!] Too much data!" << std::endl;

                        // [TODO] Reset everthing, try to start again                    
                    } else if (tofilter1.size() < min_filter1) {
                        std::cerr << "[!!!] Not enough data!" << std::endl;
                    } else {
                        // std::cerr << "[!!!] Enough to run filter! But for some reason we didn't!" << std::endl;
490
491
492
                    }
                }
                // --------------------------------------------------------------
493
                // Otherwise keep getting data
494
                // --------------------------------------------------------------
Thomas Edwards's avatar
Thomas Edwards committed
495

496
497
                // rotate the time block edge now that we've finished handling the block
                last_timeblock_edge = this_timeblock_edge;
498
499
            }

500
501
502
503
            // check error state
            if (tofilter1.size() > que_limit) {
                std::cerr << "[!!!] WARNING! Que limit has been reached! Restarting filter!" << std::endl;
                // remove all data in ques
504
505
506
                for (int ix = 0; ix < tofilter1.size(); ix++) {
                    delete(tofilter1[ix]);
                }
507
                tofilter1.clear();
508
509
510
                for (int ix = 0; ix < tofilter2.size(); ix++) {
                    delete(tofilter2[ix]);
                }
511
                tofilter2.clear();
512
513

                // break from loop to trigger a restart
514
515
516
517
518
519
                break;
            }
            
            // Add the new data into the que
            filter1_block_numdata++;
            tofilter1.push_back(new data_obs_vector(data));
520

521
522
        }
    }    
523

524
    return (0);
525
526

}