271 lines
9.8 KiB
C++
271 lines
9.8 KiB
C++
#ifndef ACTIVITYBUTTERPRESSUREPERCENT_H
|
|
#define ACTIVITYBUTTERPRESSUREPERCENT_H
|
|
|
|
#include "../../data/Timestamp.h"
|
|
#include "../../math/filter/Butterworth.h"
|
|
#include "../../math/FixedFrequencyInterpolator.h"
|
|
#include "../../math/distribution/Normal.h"
|
|
#include <KLib/Assertions.h>
|
|
#include <vector>
|
|
|
|
|
|
#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 ActivityButterPressurePercent {
|
|
|
|
public:
|
|
|
|
struct ActivityProbabilities{
|
|
float elevatorDown;
|
|
float stairsDown;
|
|
float stay;
|
|
float stairsUp;
|
|
float elevatorUp;
|
|
|
|
ActivityProbabilities(float elevatorDown, float stairsDown,
|
|
float stay, float stairsUp, float elevatorUp) :
|
|
elevatorDown(elevatorDown), stairsDown(stairsDown),
|
|
stay(stay), stairsUp(stairsUp), elevatorUp(elevatorUp) {;}
|
|
|
|
ActivityProbabilities() :
|
|
elevatorDown(0.01f), stairsDown(0.01f),
|
|
stay(0.96f), stairsUp(0.01f), elevatorUp(0.01f) {;}
|
|
};
|
|
|
|
|
|
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<History> input;
|
|
std::vector<History> inputInterp;
|
|
std::vector<History> output;
|
|
std::vector<float> sumHist;
|
|
std::vector<float> mvAvgHist;
|
|
std::vector<ActivityProbabilities> actHist;
|
|
|
|
bool initialize;
|
|
|
|
ActivityProbabilities currentActivity;
|
|
|
|
/** change this values for much success */
|
|
const int diffSize = 20; //the number values used for finding the activity.
|
|
Filter::ButterworthLP<float> butter = Filter::ButterworthLP<float>(10,0.05f,2);
|
|
FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));
|
|
|
|
const float variance = 0.02f;
|
|
|
|
const float muStairs = 0.04f;
|
|
const float muStay = 0.00f;
|
|
const float muEleveator = 0.08;
|
|
|
|
std::vector<float> densities = std::vector<float>(5, 1);
|
|
std::vector<float> densitiesOld = std::vector<float>(5, 1);;
|
|
|
|
public:
|
|
|
|
|
|
/** ctor */
|
|
ActivityButterPressurePercent() : currentActivity(ActivityProbabilities(0.01f, 0.01f, 0.96f, 0.01f, 0.01f)){
|
|
initialize = true;
|
|
}
|
|
|
|
|
|
/** add new sensor readings that were received at the given timestamp */
|
|
ActivityProbabilities add(const Timestamp& ts, const BarometerData& baro) {
|
|
|
|
//init
|
|
if(initialize){
|
|
butter.stepInitialization(baro.hPa);
|
|
initialize = false;
|
|
|
|
return currentActivity;
|
|
}
|
|
|
|
//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<float> 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 = sum;
|
|
//sumHist.push_back(actValue);
|
|
|
|
//calculate the probabilites of walking down/up etc...
|
|
densitiesOld = densities;
|
|
|
|
//in one building there is an ultra fast elevator, therefore we need to clip the activity value...
|
|
if(actValue > muEleveator){
|
|
actValue = muEleveator;
|
|
}
|
|
if(actValue < -muEleveator){
|
|
actValue = -muEleveator;
|
|
}
|
|
|
|
float densityElevatorDown = Distribution::Normal<float>::getProbability(muEleveator, variance, actValue);
|
|
float densityStairsDown = Distribution::Normal<float>::getProbability(muStairs, variance, actValue);
|
|
float densityStay = Distribution::Normal<float>::getProbability(muStay, variance, actValue);
|
|
float densityStairsUp = Distribution::Normal<float>::getProbability(-muStairs, variance, actValue);
|
|
float densityElevatorUp = Distribution::Normal<float>::getProbability(-muEleveator, variance, actValue);
|
|
|
|
_assertTrue( (densityElevatorDown == densityElevatorDown), "the probability of densityElevatorDown is null!");
|
|
_assertTrue( (densityStairsDown == densityStairsDown), "the probability of densityStairsDown is null!");
|
|
_assertTrue( (densityStay == densityStay), "the probability of densityStay is null!");
|
|
_assertTrue( (densityStairsUp == densityStairsUp), "the probability of densityStairsUp is null!");
|
|
_assertTrue( (densityElevatorUp == densityElevatorUp), "the probability of densityElevatorUp is null!");
|
|
|
|
//_assertTrue( (densityElevatorDown != 0), "the probability of densityElevatorDown is null!");
|
|
//_assertTrue( (densityStairsDown != 0), "the probability of densityStairsDown is null!");
|
|
//_assertTrue( (densityStay != 0), "the probability of densityStay is null!");
|
|
//_assertTrue( (densityStairsUp != 0), "the probability of densityStairsUp is null!");
|
|
//_assertTrue( (densityElevatorUp != 0), "the probability of densityElevatorUp is null!");
|
|
|
|
|
|
//todo: aging: wahrscheinlichkeit aufzug zu fahren oder treppe zu steigen, wird nicht knall hart auf 0 gesetzt,
|
|
//sobald der sensors nichts mehr hat, sondern wird mit der zeit geringer. größer NV?
|
|
|
|
//const Timestamp age = ts - ap.getTimestamp();
|
|
|
|
//wenn aufzug / treppe der größte wert, werden für x timestamps auf die jeweilige katerogie multipliziert.
|
|
densities[0] = densityElevatorDown;
|
|
densities[1] = densityStairsDown;
|
|
densities[2] = densityStay;
|
|
densities[3] = densityStairsUp;
|
|
densities[4] = densityElevatorUp;
|
|
|
|
//int highestValueIdx = densities.at(distance(densities.begin(), max_element (densities.begin(),densities.end())));
|
|
// if an activity other then staying is detected with a high probability, we are using the previous probability
|
|
// to keep it a little while longer. this prevents hard activity changes and helping the transition and evaluation
|
|
// to not jump between elevators/stairs and the floor and provide somewhat a smooother floorchange.
|
|
// TODO: Put this into the Project and not in Indoor, since this class should only provide the probability of the
|
|
// given activity! Since i had no time, this was the fastest solution for now.
|
|
// if(highestValueIdx != 2){
|
|
// for(int i = 0; i < densities.size(); ++i){
|
|
// densities[i] *= densitiesOld[i];
|
|
// }
|
|
// }
|
|
|
|
|
|
//normalize
|
|
float densitySum = densities[0] + densities[1] + densities[2] + densities[3] + densities[4];
|
|
|
|
for(int i = 0; i < densities.size(); ++i){
|
|
densities[i] /= densitySum;
|
|
|
|
//values cant be zero!
|
|
densities[i] = (densities[i] > 0.0f ? densities[i] : 0.01f);
|
|
}
|
|
|
|
// densityElevatorDown /= densitySum;
|
|
// densityStairsDown /= densitySum;
|
|
// densityStay /= densitySum;
|
|
// densityStairsUp /= densitySum;
|
|
// densityElevatorUp /= densitySum;
|
|
|
|
// if one value is 1.0 and all other are 0.0, fix that by providing a small possibility
|
|
// densityElevatorDown = (densityElevatorDown > 0.0f ? densityElevatorDown : 0.01f);
|
|
// densityStairsDown = (densityStairsDown > 0.0f ? densityStairsDown : 0.01f);
|
|
// densityStay = (densityStay > 0.0f ? densityStay : 0.01f);
|
|
// densityStairsUp = (densityStairsUp > 0.0f ? densityStairsUp : 0.01f);
|
|
// densityElevatorUp = (densityElevatorUp > 0.0f ? densityElevatorUp : 0.01f);
|
|
|
|
currentActivity = ActivityProbabilities(densities[0], densities[1], densities[2], densities[3], densities[4]);
|
|
|
|
}
|
|
|
|
//actHist.push_back(currentActivity);
|
|
}
|
|
|
|
//retruns for every call, indepedent of callback.
|
|
return currentActivity;
|
|
}
|
|
|
|
/** get the current Activity */
|
|
ActivityProbabilities getCurrentActivity() {
|
|
return currentActivity;
|
|
}
|
|
|
|
std::vector<float> getSensorHistory(){
|
|
std::vector<float> tmp;
|
|
|
|
for(History val : input){
|
|
tmp.push_back(val.data.hPa);
|
|
}
|
|
|
|
return tmp;
|
|
}
|
|
|
|
std::vector<float> getInterpolatedHistory(){
|
|
std::vector<float> tmp;
|
|
|
|
for(History val : inputInterp){
|
|
tmp.push_back(val.data.hPa);
|
|
}
|
|
|
|
return tmp;
|
|
}
|
|
|
|
std::vector<float> getOutputHistory(){
|
|
|
|
std::vector<float> tmp;
|
|
|
|
for(History val : output){
|
|
tmp.push_back(val.data.hPa);
|
|
}
|
|
|
|
return tmp;
|
|
}
|
|
|
|
std::vector<float> getSumHistory(){
|
|
return sumHist;
|
|
}
|
|
|
|
|
|
std::vector<ActivityProbabilities> getActHistory(){
|
|
|
|
return actHist;
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // ACTIVITYBUTTERPRESSUREPERCENT_H
|