#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;
}
评论0