allan_variance.zip_ALLAN VARIANCE_IMU

#include <iostream> #include <fstream> #include <vector> #include <cstdlib> #include <cmath> using namespace std; void read_samples(const char *filename, vector<double>& x, vector<double>& y, vector<double>& z) { ifstream stream(filename); // For now, only work on the angular rates while (stream) { double x_sample, y_sample, z_sample, dummy; stream >> x_sample >> y_sample >> z_sample >> dummy >> dummy >> dummy; // Reject bogus samples from this particular // data stream. if (x_sample > 100 || x_sample < -100) continue; // Each sample is a sum of 16 samples. x.push_back(x_sample / 16); y.push_back(y_sample / 16); z.push_back(z_sample / 16); } // assume error only at EOF } double average(vector<double>::const_iterator begin, vector<double>::const_iterator end) { double sum = 0; size_t count = end - begin; while (begin != end) { sum += *begin++; } return sum / count; } double std_dev(vector<double>::const_iterator begin, vector<double>::const_iterator end) { double avg = average(begin, end); double sum = 0; size_t count = end - begin; while (begin != end) { sum += pow(*begin++ - avg, 2); } return sqrt(sum / count); } /** * Compute the Allan Variance for a data stream, given the number of samples * to be placed in each bin. */ double allan_variance(size_t bin_size, vector<double>::const_iterator begin, vector<double>::const_iterator end) { if (bin_size == 1) return std_dev(begin, end); vector<double> bin_avg; size_t n_bins = (end - begin)/bin_size; bin_avg.reserve(n_bins); while (begin + bin_size < end) { bin_avg.push_back(average(begin, begin+bin_size)); begin += bin_size; } return std_dev(bin_avg.begin(), bin_avg.end()); } int main(int argc, char **argv) { using namespace std; if (argc < 3) { cerr << "usage: allan_variance [input_file] [sample_period (s)]\n"; return EXIT_FAILURE; } vector<double> x, y, z; read_samples(argv[1], x, y, z); char *endptr; double sample_period = strtod(argv[2], &endptr); // Ensure that all of the sample period argument gets parsed if (endptr[0] != '\0') { cerr << "ERROR Parsing arguments. Second argument not parsed as a " "floating-point number.\n"; return EXIT_FAILURE; } // Maximum bin size based on computing the std deviation of seven // buckets. size_t max_bin = x.size() / 7; for (size_t i = 1; i < max_bin+1; ++i) { cout << i*sample_period << ", " << allan_variance(i, x.begin(), x.end()) << ", " << allan_variance(i, y.begin(), y.end()) << ", " << allan_variance(i, z.begin(), z.end()) << "\n"; } return EXIT_SUCCESS; }