From 7baeecb3f9d9b06e26a4dbb9bbe9daab3ecedf59 Mon Sep 17 00:00:00 2001 From: toni Date: Fri, 9 Sep 2016 17:16:10 +0200 Subject: [PATCH] added butterworth filter \n added activity rec for baro using butter \n added moduel for walker with activity --- CMakeLists.txt | 94 ------ .../v2/modules/WalkModuleButterActivity.h | 64 ++++ main.cpp | 4 +- math/filter/Butterworth.h | 312 ++++++++++++++++++ sensors/pressure/ActivityButterPressure.h | 185 +++++++++++ tests/Tests.h | 2 +- tests/math/filter/TestButter.cpp | 257 +++++++++++++++ tests/sensors/pressure/TestBarometer.cpp | 47 +++ 8 files changed, 868 insertions(+), 97 deletions(-) delete mode 100755 CMakeLists.txt create mode 100644 grid/walk/v2/modules/WalkModuleButterActivity.h create mode 100644 math/filter/Butterworth.h create mode 100644 sensors/pressure/ActivityButterPressure.h create mode 100644 tests/math/filter/TestButter.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100755 index 395d28e..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,94 +0,0 @@ -# Usage: -# Create build folder, like RC-build next to RobotControl and WifiScan folder -# CD into build folder and execute 'cmake -DCMAKE_BUILD_TYPE=Debug ../RobotControl' -# make - -CMAKE_MINIMUM_REQUIRED(VERSION 2.8) - -# select build type -SET( CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" ) - -PROJECT(Indoor) - -IF(NOT CMAKE_BUILD_TYPE) - MESSAGE(STATUS "No build type selected. Default to Debug") - SET(CMAKE_BUILD_TYPE "Debug") -ENDIF() - - - -INCLUDE_DIRECTORIES( - ../ - /mnt/firma/kunden/HandyGames/ -) - - -FILE(GLOB HEADERS - ./*.h - ./*/*.h - ./*/*/*.h - ./*/*/*/*.h - ./*/*/*/*/*.h - ./*/*/*/*/*/*.h -) - -FILE(GLOB SOURCES - ./*.cpp - ./*/*.cpp - ./*/*/*.cpp - ./*/*/*/*.cpp -) - - -if(${CMAKE_GENERATOR} MATCHES "Visual Studio") - - SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} /D_X86_ /D_USE_MATH_DEFINES") - SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /Zi /Oi /GL /Ot /Ox /D_X86_ /D_USE_MATH_DEFINES") - SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /DEBUG") - SET(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /LTCG /INCREMENTAL:NO") - - set(CMAKE_CONFIGURATION_TYPES Release Debug) - -else() - -# system specific compiler flags -ADD_DEFINITIONS( - - -std=gnu++11 - - -Wall - -Werror=return-type - -Wextra - -Wpedantic - - -fstack-protector-all - - -g3 - -O0 - -march=native - - -DWITH_TESTS - -DWITH_ASSERTIONS - -DWITH_DEBUG_LOG - - -) - -endif() - -# build a binary file -ADD_EXECUTABLE( - ${PROJECT_NAME} - ${HEADERS} - ${SOURCES} -) - -# needed external libraries -TARGET_LINK_LIBRARIES( - ${PROJECT_NAME} - gtest - pthread -) - -SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER}) - diff --git a/grid/walk/v2/modules/WalkModuleButterActivity.h b/grid/walk/v2/modules/WalkModuleButterActivity.h new file mode 100644 index 0000000..b110d1c --- /dev/null +++ b/grid/walk/v2/modules/WalkModuleButterActivity.h @@ -0,0 +1,64 @@ +#ifndef WALKMODULEBUTTERACTIVITY_H +#define WALKMODULEBUTTERACTIVITY_H + +#include "WalkModule.h" +#include "WalkStateHeading.h" + +#include "../../../../geo/Heading.h" +#include "../../../../math/Distributions.h" +#include "../../../../sensors/pressure/ActivityButterPressure.h" + + +/** favor z-transitions */ +template class WalkModuleButterActivity : public WalkModule { + +public: + + /** ctor */ + WalkModuleButterActivity() { + ; + } + + virtual void updateBefore(WalkState& state) override { + (void) state; + } + + virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override { + (void) state; + (void) startNode; + (void) endNode; + + } + + virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override { + (void) state; + (void) curNode; + (void) nextNode; + } + + double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override { + + (void) state; + (void) startNode; + + const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm; + + if(state.act == ActivityButterPressure::Activity::DOWN){ + if (deltaZ_cm < 0) {return 0;} + if (deltaZ_cm == 0) {return 0.1;} + return 0.9; + } else if (state.act == ActivityButterPressure::Activity::UP){ + if (deltaZ_cm > 0) {return 0;} + if (deltaZ_cm == 0) {return 0.1;} + return 0.9; + } else { + if (deltaZ_cm == 0) {return 0.9;} + return 0.1; + } + + } + + +}; + +#endif // WALKMODULEBUTTERACTIVITY_H diff --git a/main.cpp b/main.cpp index 7e30379..304cd38 100755 --- a/main.cpp +++ b/main.cpp @@ -17,9 +17,9 @@ int main(int argc, char** argv) { ::testing::InitGoogleTest(&argc, argv); // skip all tests starting with LIVE_ - ::testing::GTEST_FLAG(filter) = "-*.LIVE_*"; + ::testing::GTEST_FLAG(filter) = "*Barometer*"; - ::testing::GTEST_FLAG(filter) = "*GridWalk2HeadingControl*"; + ::testing::GTEST_FLAG(filter) = "*Activity*"; //::testing::GTEST_FLAG(filter) = "*Grid.*"; //::testing::GTEST_FLAG(filter) = "*Dijkstra.*"; diff --git a/math/filter/Butterworth.h b/math/filter/Butterworth.h new file mode 100644 index 0000000..c41fc71 --- /dev/null +++ b/math/filter/Butterworth.h @@ -0,0 +1,312 @@ +#ifndef BUTTERWORTHLP_H +#define BUTTERWORTHLP_H + +#include +#include +#include + +namespace Filter { + + /** butterworth lowpass filter*/ + template class ButterworthLP { + + private: + + /** Cascaded second-order sections */ + class SOS{ + + public: + + SOS(const Scalar b0, const Scalar b1, const Scalar b2, const Scalar a1, const Scalar a2, const Scalar gain) : + _b0(b0), _b1(b1), _b2(b2), _a1(a1), _a2(a2), _gain(gain), _z1(0), _z2(0), + _preCompStateSpaceOutputVec1(_b1 - _b0*_a1), + _preCompStateSpaceOutputVec2(_b2 - _b0*_a2){ + + } + + void safeRestoreState(Scalar &z1, Scalar &z2, const bool restore){ + + if (restore) + { + _z1 = z1; + _z2 = z2; + } + else + { + z1 = _z1; + z2 = _z2; + } + } + + void stepInitialization(const Scalar value){ + + // Set second-order section state to steady state w.r.t. given step response value + // http://www.emt.tugraz.at/publications/diplomarbeiten/da_hoebenreich/node21.html + _z1 = _z2 = value / (1.0 + _a1 + _a2); + } + + Scalar process(const Scalar input){ + + /** + SOS state space model + z' = A * z + b * x + y = c^T * z + d * x + with A = [-a1 -a2; 1 0] b = [1; 0] c = [b1 - b0*a1; b2 - b0*a2] d = [b0] + */ + + Scalar x = input; + Scalar y = x; + Scalar z1_new, z2_new; + + z1_new = -_a1 * _z1 - _a2 * _z2 + x; + z2_new = _z1; + y = _preCompStateSpaceOutputVec1 * _z1 + _preCompStateSpaceOutputVec2 * _z2 + _b0 * x; + _z1 = z1_new; + _z2 = z2_new; + + // Include SOS gain factor + y *= _gain; + + return y; + } + + ~SOS() + { } + + private: + + const Scalar _b0, _b1, _b2, _a1, _a2, _gain; + + const Scalar _preCompStateSpaceOutputVec1, _preCompStateSpaceOutputVec2; + + Scalar _z1, _z2; + + }; + + + size_t _numSOS; + std::vector _sosVec; + + public: + + + /** + Generates a Butterworth lowpass filter with a given normalized cutoff frequency and filter order. + @param normalizedCutoffFrequency (0, 1) Normalized cutoff frequency := cuttoffFreq / samplingFreq. + @param filterOrder [1, inf] Butterworth filter order. + */ + ButterworthLP(const Scalar normalizedCutoffFrequency, const size_t filterOrder) : + _numSOS(0) + { + if (!coefficients(normalizedCutoffFrequency, filterOrder)) + { + throw std::domain_error(std::string("Failed to design a filter due to invalid parameters (normalized cutoff frequency and / or filter order) or instability of the resulting digitalized filter.")); + } + } + + + /** + Generates a Butterworth lowpass filter of a given order with a given cutoff frequency [Hz] in respect of the data sampling frequency [Hz]. + @param samplingFrequency [1, inf] [Hz] Sampling frequency of the data. + @param cutoffFrequency [1, samplingFrequency - 1] [Hz] Cutoff frequency + @param filterOrder [1, inf] Butterworth filter order. + */ + ButterworthLP(const Scalar samplingFrequency, const Scalar cutoffFrequency, const size_t filterOrder) : + ButterworthLP(cutoffFrequency / samplingFrequency, filterOrder) + { } + + /** + Set the filter state to a steady state w.r.t. to the given input value (assuming a constant filter input for infinite time steps in the past; + see also http://www.emt.tugraz.at/publications/diplomarbeiten/da_hoebenreich/node21.html). + @param value Desired steady state output value + */ + void stepInitialization(const Scalar value){ + Scalar stepResponseValue = value; + + // Propagate step initialization through all second-order sections + for (size_t i = 0; i < _numSOS; ++i) + { + _sosVec[i].stepInitialization(stepResponseValue); + stepResponseValue = _sosVec[i].process(stepResponseValue); + } + } + + + /** + Processes the input value online depending on the current filter state. + @param input [-inf, inf] Input value + @return [-inf, inf] Filter response + */ + Scalar process(const Scalar input){ + Scalar x = input; + Scalar y = x; + + // Cascade all second-order sections s.t. output of SOS i is input for SOS i+1 + for (size_t i = 0; i < _numSOS; ++i) + { + y = _sosVec[i].process(x); + x = y; + } + + return y; + } + + + /** + Processes the input data offline. + @param *input Ptr to input data. + @param *output Ptr to output data. + @param size [0, inf] Length of data. + @param initialConditionValue [-inf, inf] Initializes the filter state to a steady state w.r.t. the given initial value (see stepInitialization). + @param forwardBackward {true, false} Eliminate phase delay by filtering twice: forward and backward. + Note that forward-backward filtering corresponds to filtering with a 2n-th order filter. + */ + void filter(const Scalar *input, Scalar *output, const size_t size, const Scalar initialConditionValue, const bool forwardBackward){ + + // Save all current SOS states before offline filtering + std::vector zSaved(_numSOS * 2); + for (size_t i = 0; i < _numSOS; ++i) + { + _sosVec[i].safeRestoreState(zSaved[i], zSaved[i + 1], false); + } + + // Set initial step response conditions + stepInitialization(initialConditionValue); + + // Filtering on input data + for (size_t i = 0; i < size; ++i) + { + output[i] = process(input[i]); + } + + // Additional backward filtering on filtered output data if requested + if (forwardBackward) + { + for (size_t i = size; i > 0;) + { + output[--i] = process(output[i]); + } + } + + // Restore all SOS states + for (size_t i = 0; i < _numSOS; ++i) + { + _sosVec[i].safeRestoreState(zSaved[i], zSaved[i + 1], true); + } + } + + private: + + bool coefficients(const Scalar normalizedCutoffFrequency, const size_t filterOrder){ + + // Assure valid parameters + if (filterOrder < 1 || normalizedCutoffFrequency <= 0 || normalizedCutoffFrequency > 1) + { + return false; + } + + std::vector> poles(filterOrder); + + // Prewarp the analog prototype's cutoff frequency + Scalar omegaCutoff = 2 * tan(M_PI * normalizedCutoffFrequency); + + Scalar gain = pow(omegaCutoff, filterOrder); + Scalar initialGain = gain; + + std::complex two(2.0, 0); + + for (size_t i = 0, i_end = (filterOrder + 1) / 2; i < i_end; ++i){ + + size_t i2 = 2 * i; + + /** + Design the analog prototype Butterworth lowpass filter + */ + + // Generate s-poles of prototype filter + Scalar phi = static_cast(i2 + 1) * M_PI / (2 * filterOrder); + Scalar real = -sin(phi); + Scalar imag = cos(phi); + + std::complex pole = std::complex(real, imag); + + /** + Customize analog prototype filter w.r.t cutoff frequency + */ + + // Scale s-pole with the cutoff frequency + pole *= omegaCutoff; + + /** + Digitalize the analog filter + */ + + // Map pole from s-plane to z-plane using bilinear transform + std::complex s = pole; + pole = (two + s) / (two - s); + + // Update overall gain in respect of z-pole gain + gain *= abs((two - s)); + + // Ensure z-pole lies in unit circle of z-plane + if (abs(pole) > 1) + { + return false; + } + + // Add stable z-pole + poles[i2] = pole; + + // Odd filter order: ignore the second complex conjugate pole + if (i2 + 1 >= filterOrder) + { + break; + } + + // Do the same as above with the conjugate complex pole + pole = std::complex(real, -imag); + pole *= omegaCutoff; + s = pole; + pole = (two + s) / (two - s); + gain *= abs((two - s)); + if (abs(pole) > 1) + { + return false; + } + poles[i2 + 1] = pole; + } + + // Distribute the overall gain over all z-poles + Scalar overallGain = initialGain * (initialGain / gain); + Scalar distributedPoleGain = pow(overallGain, 1.0 / static_cast(filterOrder)); + Scalar distributedPolePairGain = distributedPoleGain * distributedPoleGain; + + /** + Generate second-order sections from conjugate complex z-pole pairs + */ + + for (size_t i = 0, i_end = filterOrder - 1; i < i_end; i += 2){ + + addSOS(SOS(1.0, 2.0, 1.0, -(poles[i] + poles[i + 1]).real(), (poles[i] * poles[i + 1]).real(), distributedPolePairGain)); + } + + // Odd filter order: remaining single z-pole requires additional second-order section + if (filterOrder % 2 == 1){ + + addSOS(SOS(1.0, 1.0, 0.0, -poles[filterOrder - 1].real(), 0.0, distributedPoleGain)); + } + + return true; + } + + void addSOS(const SOS sos){ + + _sosVec.push_back(sos); + ++_numSOS; + } + + + }; +} + +#endif // BUTTERWORTHLP_H diff --git a/sensors/pressure/ActivityButterPressure.h b/sensors/pressure/ActivityButterPressure.h new file mode 100644 index 0000000..34ad055 --- /dev/null +++ b/sensors/pressure/ActivityButterPressure.h @@ -0,0 +1,185 @@ +#ifndef ACTIVITYBUTTERPRESSURE_H +#define ACTIVITYBUTTERPRESSURE_H + +#include "../../data/Timestamp.h" +#include "../../math/filter/Butterworth.h" +#include "../../math/FixedFrequencyInterpolator.h" +#include "../../math/MovingAVG.h" + +#include "BarometerData.h" + +/** + * receives pressure measurements, interpolates them to a ficex frequency, lowpass filtering + * activity recognition based on a small window given by matlabs diff(window) + */ +class ActivityButterPressure { + +public: + + enum Activity {DOWN, STAY, UP}; + + + + struct History { + Timestamp ts; + BarometerData data; + History(const Timestamp ts, const BarometerData data) : ts(ts), data(data) {;} + }; + + private: + //just for debugging and plotting + std::vector input; + std::vector inputInterp; + std::vector output; + std::vector sumHist; + std::vector mvAvgHist; + std::vector actHist; + + Activity currentActivity; + MovingAVG mvAvg = MovingAVG(20); + + /** change this values for much success */ + const bool additionalLowpassFilter = false; + const int diffSize = 20; //the number values used for finding the activity. + const float threshold = 0.025; // if diffSize is getting smaller, treshold needs to be adjusted in the same direction! + Filter::ButterworthLP butter = Filter::ButterworthLP(10,0.05f,2); + Filter::ButterworthLP butter2 = Filter::ButterworthLP(10,0.1f,2); + FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); + +public: + + + /** ctor */ + ActivityButterPressure() : currentActivity(STAY){ + ; + } + + + /** add new sensor readings that were received at the given timestamp */ + Activity add(const Timestamp& ts, const BarometerData& baro) { + + //init + static bool firstCall = false; + if(!firstCall){ + butter.stepInitialization(baro.hPa); + firstCall = true; + + butter2.stepInitialization(0); + return STAY; + } + + input.push_back(History(ts, baro)); + + bool newInterpolatedValues = false; + + //interpolate & butter + auto callback = [&] (const Timestamp ts, const float val) { + float interpValue = val; + inputInterp.push_back(History(ts, BarometerData(interpValue))); + + //butter + float butterValue = butter.process(interpValue); + output.push_back(History(ts, BarometerData(butterValue))); + + newInterpolatedValues = true; + + }; + ffi.add(ts, baro.hPa, callback); + + if(newInterpolatedValues == true){ + + //getActivity + if(output.size() > diffSize){ + //diff + std::vector diff; + for(int i = output.size() - diffSize; i < output.size() - 1; ++i){ + + float diffVal = output[i+1].data.hPa - output[i].data.hPa; + + diff.push_back(diffVal); + } + + float sum = 0; + for(float val : diff){ + sum += val; + } + + float actValue = 0; + if(additionalLowpassFilter == true){ + //additional butter/moving average for the results + //mvAvg.add(sum); + //actValue = mvAvg.get(); + actValue = butter2.process(sum); + }else{ + actValue = sum; + } + sumHist.push_back(actValue); + + if(actValue > threshold){ + currentActivity = DOWN; + } + else if (actValue < -threshold){ + currentActivity = UP; + } + else{ + currentActivity = STAY; + } + } + } + + actHist.push_back(History(ts, BarometerData(currentActivity))); + + return currentActivity; + + } + + /** get the current Activity */ + Activity getCurrentActivity() { + return currentActivity; + } + + std::vector getSensorHistory(){ + std::vector tmp; + + for(History val : input){ + tmp.push_back(val.data.hPa); + } + + return tmp; + } + + std::vector getInterpolatedHistory(){ + std::vector tmp; + + for(History val : inputInterp){ + tmp.push_back(val.data.hPa); + } + + return tmp; + } + + std::vector getOutputHistory(){ + + std::vector tmp; + + for(History val : output){ + tmp.push_back(val.data.hPa); + } + + return tmp; + } + + std::vector getSumHistory(){ + return sumHist; + } + + + std::vector getActHistory(){ + + return actHist; + } + + +}; + +#endif // ACTIVITYBUTTERPRESSURE_H diff --git a/tests/Tests.h b/tests/Tests.h index 0bca2de..eff92ac 100755 --- a/tests/Tests.h +++ b/tests/Tests.h @@ -6,7 +6,7 @@ #include static inline std::string getDataFile(const std::string& name) { - return "/mnt/data/workspaces/Indoor/tests/data/" + name; + return "/home/toni/Documents/programme/localization/Indoor/tests/data/" + name; } diff --git a/tests/math/filter/TestButter.cpp b/tests/math/filter/TestButter.cpp new file mode 100644 index 0000000..750909d --- /dev/null +++ b/tests/math/filter/TestButter.cpp @@ -0,0 +1,257 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" +#include "../../../math/filter/Butterworth.h" +#include "../../../misc/Time.h" +#include "../../../math/Interpolator.h" + +#include"../../../sensors/pressure/ActivityButterPressure.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + + + +TEST(Butterworth, offlineSinus) { + + //input data + std::minstd_rand gen; + std::uniform_real_distribution noise (-0.1, +0.1); + + int size = 1100; //Fs + double* input = new double[size]; + double* output = new double[size]; + + // 17.5hz sin signal with random noise [-0.1, 0.1] + for( int i=0; i < size; ++i ){ + input[i] = sin(0.1 * i) + noise(gen); + } + + //butterworth + Filter::ButterworthLP butter(size,20,5); + butter.stepInitialization(0); + butter.filter(input, output, size, 0, true); + + K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementLines linesInput; + K::GnuplotPlotElementLines linesOutput; + + for(int i=0; i < size-1; ++i){ + + K::GnuplotPoint2 input_p1(i, input[i]); + K::GnuplotPoint2 input_p2(i+1, input[i+1]); + + K::GnuplotPoint2 output_p1(i, output[i]); + K::GnuplotPoint2 output_p2(i+1, output[i+1]); + + linesInput.addSegment(input_p1, input_p2); + linesOutput.addSegment(output_p1, output_p2); + } + linesOutput.setColorHex("#00FF00"); + + plot.add(&linesInput); + plot.add(&linesOutput); + + gp.draw(plot); + gp.flush(); + + sleep(10); +} + +TEST(Butterworth, onlineSinus) { + + int size = 1100; //Fs + double* input = new double[size]; + double* output = new double[size]; + + Filter::ButterworthLP butter(size,20,5); + butter.stepInitialization(0); + + //input data + std::minstd_rand gen; + std::uniform_real_distribution noise (-0.1, +0.1); + + // 17.5hz sin signal with random noise [-0.1, 0.1] + for( int i=0; i < size; ++i ){ + input[i] = sin(0.1 * i) + noise(gen); + + output[i] = butter.process(input[i]); + } + + K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementLines linesInput; + K::GnuplotPlotElementLines linesOutput; + + for(int i=0; i < size-1; ++i){ + + K::GnuplotPoint2 input_p1(i, input[i]); + K::GnuplotPoint2 input_p2(i+1, input[i+1]); + + K::GnuplotPoint2 output_p1(i, output[i]); + K::GnuplotPoint2 output_p2(i+1, output[i+1]); + + linesInput.addSegment(input_p1, input_p2); + linesOutput.addSegment(output_p1, output_p2); + } + linesOutput.setColorHex("#00FF00"); + + plot.add(&linesInput); + plot.add(&linesOutput); + + gp.draw(plot); + gp.flush(); + + sleep(1); +} + +TEST(Butterworth, offlineOctaveBaro) { + + + double* input = new double[100000]; + double* output = new double[100000]; + + Interpolator interp; + + //read file + std::string line; + std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat"); + std::ifstream infile(filename); + + int counter = 0; + while (std::getline(infile, line)) + { + std::istringstream iss(line); + int ts; + double value; + + while (iss >> ts >> value) { + + interp.add(ts, value); + + while(interp.getMaxKey() > counter*20 ){ + double interpValue = interp.get(counter*20); + + input[counter] = interpValue; + //std::cout << counter*20 << " " << interpValue << " i" << std::endl; + ++counter; + } + + //std::cout << ts << " " << value << " r" << std::endl; + + } + } + + Filter::ButterworthLP butter(50,0.2,2); + butter.filter(input, output, counter, 938.15, true); + + K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementLines linesInput; + K::GnuplotPlotElementLines linesOutput; + + for(int i=0; i < counter-1; ++i){ + + K::GnuplotPoint2 input_p1(i, input[i]); + K::GnuplotPoint2 input_p2(i+1, input[i+1]); + + K::GnuplotPoint2 output_p1(i, output[i]); + K::GnuplotPoint2 output_p2(i+1, output[i+1]); + + linesInput.addSegment(input_p1, input_p2); + linesOutput.addSegment(output_p1, output_p2); + } + linesOutput.setColorHex("#00FF00"); + + plot.add(&linesInput); + plot.add(&linesOutput); + + gp.draw(plot); + gp.flush(); + + sleep(1); + +} + +TEST(Butterworth, onlineOctaveBaro) { + + std::vector input; + std::vector output; + + Interpolator interp; + + Filter::ButterworthLP butter(50,0.02,2); + butter.stepInitialization(938.15); + + //read file + std::string line; + std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat"); + std::ifstream infile(filename); + + int counter = 1; + while (std::getline(infile, line)) + { + std::istringstream iss(line); + int ts; + double value; + + while (iss >> ts >> value) { + + interp.add(ts, value); + + while(interp.getMaxKey() > counter*20 ){ + double interpValue = interp.get(counter*20); + + //std::cout << counter*20 << " " << interpValue << " i" << std::endl; + + input.push_back(interpValue); + + output.push_back(butter.process(interpValue)); + + ++counter; + } + + //std::cout << ts << " " << value << " r" << std::endl; + + } + } + + K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementLines linesInput; + K::GnuplotPlotElementLines linesOutput; + + for(int i=0; i < input.size()-1; ++i){ + + K::GnuplotPoint2 input_p1(i, input[i]); + K::GnuplotPoint2 input_p2(i+1, input[i+1]); + + K::GnuplotPoint2 output_p1(i, output[i]); + K::GnuplotPoint2 output_p2(i+1, output[i+1]); + + linesInput.addSegment(input_p1, input_p2); + linesOutput.addSegment(output_p1, output_p2); + } + linesOutput.setColorHex("#00FF00"); + + plot.add(&linesInput); + plot.add(&linesOutput); + + gp.draw(plot); + gp.flush(); + + sleep(1); +} + + +#endif diff --git a/tests/sensors/pressure/TestBarometer.cpp b/tests/sensors/pressure/TestBarometer.cpp index 5a79260..bb5a204 100644 --- a/tests/sensors/pressure/TestBarometer.cpp +++ b/tests/sensors/pressure/TestBarometer.cpp @@ -4,6 +4,7 @@ #include "../../../sensors/pressure/RelativePressure.h" #include "../../../sensors/pressure/PressureTendence.h" +#include "../../../sensors/pressure/ActivityButterPressure.h" #include @@ -121,5 +122,51 @@ TEST(Barometer, LIVE_tendence2) { } +TEST(Barometer, Activity) { + ActivityButterPressure act; + + //read file + std::string line; + std::string filename = getDataFile("baro/logfile_UAH_R2_S3_baro.dat"); + std::ifstream infile(filename); + + while (std::getline(infile, line)) + { + std::istringstream iss(line); + int ts; + double value; + + while (iss >> ts >> value) { + ActivityButterPressure::Activity currentAct = act.add(Timestamp::fromMS(ts), BarometerData(value)); + } + } + + std::vector sum = act.getSumHistory(); + std::vector interpolated = act.getInterpolatedHistory(); + std::vector raw = act.getSensorHistory(); + std::vector butter = act.getOutputHistory(); + std::vector actHist = act.getActHistory(); + + K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementLines rawLines; + + for(int i=0; i < actHist.size()-1; ++i){ + + K::GnuplotPoint2 input_p1(actHist[i].ts.sec(), actHist[i].data.hPa); + K::GnuplotPoint2 input_p2(actHist[i+1].ts.sec(), actHist[i+1].data.hPa); + + rawLines.addSegment(input_p1, input_p2); + } + + plot.add(&rawLines); + + gp.draw(plot); + gp.flush(); + + sleep(1); + +} + #endif