Integrator.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00002 
00011 #include "Integrator.h"
00012 #include <iostream>
00013 #include <iterator>
00014 #include <numeric>
00015 
00016 Integrator::Integrator(double _dt, unsigned int _range) {
00017   setup(_dt, _range);
00018 }
00019 
00020 Integrator::~Integrator(void) {
00021 }
00022 
00023 void Integrator::reset(void) {
00024   buffer.clear();
00025   first = 0;
00026   sum = 0;
00027   last = 0;
00028   init_integration_flag = false;
00029   return;
00030 }
00031 
00032 void Integrator::setup(double _dt, unsigned int _range) {
00033   dt = _dt;
00034   range = _range;
00035   reset();
00036   return;
00037 }
00038 
00039 void Integrator::update (double _x) {
00040 
00041   // integration by trapezoidal rule:
00042   // (1/2 * first + sum(f(x_i), 1, N-1) + 1/2 * last) * dt
00043   if (!init_integration_flag) {
00044     first = _x; // update first value
00045     init_integration_flag = true;
00046     // first value is not counted to sum
00047   } else {
00048     sum += last; // sum is last is assumed to be 0 at first
00049     last = _x;
00050   }
00051 
00052   // if integration range is defined, use buffer
00053   if (range > 0) {
00054     buffer.push_back(_x); // save values (include first)
00055     if (buffer.size() > range) {
00056       buffer.pop_front(); // remove oldest data
00057       first = buffer.front(); // update first value
00058       sum -= first;
00059     }
00060   }
00061   
00062   return;
00063 }
00064 
00065 double Integrator::calculate(void) {
00066   // calc integration by trapezoidal rule
00067   // sum(1/2 * (f(x_i) - f(x_(i+1))), 0, N) * dt = (1/2 * f(0) + sum(f(x_i), 1, N-1) + 1/2 * f(N)) * dt
00068   return (0.5 * first + sum + 0.5 * last) * dt;
00069 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55