FHWS/Fusion2016
Archived
3
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

initial commit before ownership transfer

Browse Source
This commit is contained in:
FrankE
2016-01-25 17:57:49 +01:00
parent 36056ad002
commit 353bba8342
37 changed files with 7639 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
code/toni/BarometerEvaluation.h Executable file
View File

@@ -0,0 +1,58 @@
#pragma once
#include "../particles/MyState.h"
#include "BarometerObservation.h"
#include "barometric.h"
#include <KLib/math/distribution/Normal.h>
double g_BarometerObservation = 0.0;
class BarometerEvaluation {
public:
double getProbability(const MyState& state, const BarometerObservation* obs) const {
//rho_z
double barometerSigma = 0.3;
//The height of the single floor levels.
const static double floor_height[3] = {4.1, 3.4, 3.4};
if(USE_BAROMETRIC_FORMULAR){
//height the particle has climbed.
double h_1 = 0.0;
for(int i = std::min(state.z_nr_old, state.z_nr); i < std::max(state.z_nr_old, state.z_nr); i++){
h_1 += floor_height[i];
}
if(h_1 != 0.0){
// use the barometric formular to calculate the relative pressure
// the calculation is done assuming sea level height at every floor.
double mslp = BarometricFormular::s_getSeaLevelPressure();
double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
barometerSigma = std::abs(mslp - pressure);
}
}
else {
// constant value for sigma if we assume all floors are same in height
barometerSigma = 0.30 / 1.0; //hPa
}
// evaluate the current particle with a normal distribution
const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma/2, obs->hpa);
//Just for the visualization. i'm a lazy bastard
g_BarometerObservation = obs->hpa;
assert(barometerProbability == barometerProbability);
assert(state.hPa == state.hPa);
assert(obs->hpa == obs->hpa);
//std::cout << barometerProbability << std::endl;
return pow(2.0, barometerProbability);
//return barometerProbability;
}
};

17
code/toni/BarometerObservation.h Executable file
View File

@@ -0,0 +1,17 @@
#pragma once
#include <cstdint>
struct BarometerObservation {
double hpa;
BarometerObservation() { ; }
BarometerObservation(const float hpa) : hpa(hpa) {
;}
};

91
code/toni/BarometerSensorReader.h Executable file
View File

@@ -0,0 +1,91 @@
#pragma once
#include "circular.h"
#include "BarometerObservation.h"
#include "../SensorReader.h"
#include <sstream>
//circular_buffer<double> measurementHistory(1000);
class BarometerSensorReader{
private:
circular_buffer<double> measurementHistory;
public:
BarometerSensorReader(){
if(!USE_STATIC_CIRCULAR_BUFFERING){
//8.33min
measurementHistory.reserve(10000);
}
else{
//30 * 500ms = 1,5s
measurementHistory.reserve(30);
}
}
BarometerObservation* readBarometer(const SensorEntry& se) {
std::string tmp = se.data;
BarometerObservation* obs = new BarometerObservation();
//Read the hPa
double hPa = stod(tmp);
// load the measurement at current time into the history
double currentMeasurement = hPa - measurementHistory[0];
if(USE_BAROMETER_SMOOTHING_RC_LOWPASS){
//smoothing with alpha value
if(measurementHistory.size() > 1){
double alpha = 0.1;
double lastMeasurement = measurementHistory[measurementHistory.size() - 1];
currentMeasurement = (alpha * currentMeasurement) + ((1.0 - alpha) * lastMeasurement);
obs->hpa = currentMeasurement;
}else{
obs->hpa = 0;
}
measurementHistory.push_back(currentMeasurement);
}
else if (USE_BAROMETER_SMOOTHING_HEAD_TAIL){
currentMeasurement = hPa;
measurementHistory.push_back(currentMeasurement);
// calculate the relative air pressure by getting the mean of the first and last three entrys of the history
// and subtract them.
if (measurementHistory.size() > 5){
double meanTail = (measurementHistory[0] + measurementHistory[1] + measurementHistory[2]) / 3.0;
double meanHead = (measurementHistory[measurementHistory.size() - 1] + measurementHistory[measurementHistory.size() - 2] + measurementHistory[measurementHistory.size() - 3]) / 3.0;
obs->hpa = meanHead - meanTail;
}
else{
obs->hpa = 0;
}
}
else //no data smoothing
{
measurementHistory.push_back(currentMeasurement);
obs->hpa = currentMeasurement;
}
return obs;
}
//TODO
void readVerticalAcceleration(const SensorEntry& se){
//Problem: Koordinatensystem LinearAcceleraton ist relativ zum Telefon und nicht zum
//Weltkoordinatensystem. Brauchen die Beschleunigung nach Oben in Weltkoordinaten.
}
};

52
code/toni/TFRingBuffer.h Executable file
View File

@@ -0,0 +1,52 @@
#ifndef TFObjectPool_TFRingBuffer_h
#define TFObjectPool_TFRingBuffer_h
#include <atomic>
#include <cstddef>
template <typename T> class TFRingBuffer {
T *m_buffer;
std::atomic<size_t> m_head;
std::atomic<size_t> m_tail;
const size_t m_size;
size_t next(size_t current) {
return (current + 1) % m_size;
}
public:
TFRingBuffer(const size_t size) : m_size(size), m_head(0), m_tail(0) {
m_buffer = new T[size];
}
virtual ~TFRingBuffer() {
delete[] m_buffer;
}
bool push(const T &object) {
size_t head = m_head.load(std::memory_order_relaxed);
size_t nextHead = next(head);
if (nextHead == m_tail.load(std::memory_order_acquire)) {
return false;
}
m_buffer[head] = object;
m_head.store(nextHead, std::memory_order_release);
return true;
}
bool pop(T &object) {
size_t tail = m_tail.load(std::memory_order_relaxed);
if (tail == m_head.load(std::memory_order_acquire)) {
return false;
}
object = m_buffer[tail];
m_tail.store(next(tail), std::memory_order_release);
return true;
}
};
#endif

109
code/toni/barometric.h Executable file
View File

@@ -0,0 +1,109 @@
#ifndef BAROMETRIC
#define BAROMETRIC
const static double mslp = 980.25; // mean sea level spressure
const static double int_lapse_rate = 0.0065; // a
const static double int_exponent = 5.255; // international barometric formular exponent calculated from (M * g) / (R * a)
//The height of the single floor levels.
const static double floor_height[5] = {0.0, 4.1, 3.4, 3.4, 3.4};
class BarometricFormular
{
private:
const double temperature; // T in Kelvin
const double universal_gas_constant; // R
const double molar_mass; // M
const double gravitational_acceleration; // g
const double lapse_rate; // a
double _exponent;
public:
/** ctor */
BarometricFormular(const double R, const double M, const double g, const double a, const double T):
universal_gas_constant(R), molar_mass(M), gravitational_acceleration(g), lapse_rate(a), temperature(T){
_exponent = (M * g) / (R * a);
}
/** ctor only with Temperature*/
BarometricFormular(const double T) :
universal_gas_constant(8.314), molar_mass(0.02896), gravitational_acceleration(9.80665), lapse_rate(0.0065), temperature(T){
_exponent = (molar_mass * gravitational_acceleration) / (universal_gas_constant * lapse_rate);
}
/** Atmospheric Pressure Calculation */
double getAtmosphericPressure(double p_0, double h_1) const{
return p_0 * std::pow((1.0 - ((lapse_rate * h_1)/temperature)), _exponent);
}
/** Atmospheric Pressure Calculation above sea level*/
double getAtmosphericPressure(double h_1) const{
return mslp * std::pow((1.0 - ((lapse_rate * h_1)/temperature)), _exponent);
}
//TODO:: Height from pressure for the general formular
//Static Functions
/** International Barometric Formular*/
static double s_getAtmosphericPressure(double p_0, double h_1, double kelvin){
return p_0 * std::pow((1.0 - ((int_lapse_rate * h_1)/kelvin)), int_exponent);
}
/** International Barometric Formular above Sea Level*/
static double s_getAtmosphericPressure(double h_1, double kelvin){
return mslp * std::pow((1.0 - ((int_lapse_rate * h_1)/kelvin)), int_exponent);
}
/** International Barometric Formular above Sea Level at 15 degree*/
static double s_getAtmosphericPressure(double height_above_sea_level){
return mslp * std::pow((1.0 - ((int_lapse_rate * height_above_sea_level)/288.15)), int_exponent);
}
/** Get the height above sea level using a pressure measurment above sea level*/
static double getHeightAboveSeaLevel(double p, double kelvin){
// http://www.wolframalpha.com/input/?i=solve+for+h+++p+%3D+980.25*%281+-+0.0065+*+h%2FT%29^5.255
return 41.4811 * ((3.70882 * kelvin) - (std::pow(p, 0.1902949571836346) * kelvin));
}
/** This is a helper Class only for gnupplot visualization for ipin2015*/
static double getHeightForVisualizationOnly(double p, double z_0, double kelvin){
// the height of the reference (first) pressure measurement
double h_0 = 0.0;
for(int i = 0; i <= z_0; i++){
h_0 += floor_height[i];
}
// pressure value of h_0 above sea level
// we define that the bottom of floor 0 is sea level ;).
double p_0 = s_getAtmosphericPressure(h_0, kelvin);
// pressure value of the current particle above floor 0 (sea level)
double p_height = p_0 + p;
// height of the particle above floor 0 (sea level)
return getHeightAboveSeaLevel(p_height, kelvin);
}
static double s_getSeaLevelPressure(){
return mslp;
}
static double getPressureOfFloorForVizualization(double z, double kelvin){
int i = z + 0.5;
double h_z = floor_height[i+1];
double p_z = s_getAtmosphericPressure(h_z, kelvin);
return std::abs(mslp - p_z);
}
};
#endif // BAROMETRIC

492
code/toni/circular.h Executable file
View File

@@ -0,0 +1,492 @@
/******************************************************************************
* $Id: $
* $Name: $
*
* Author: Pete Goodliffe
*
* ----------------------------------------------------------------------------
* Copyright 2002 Pete Goodliffe All rights reserved.
*
* ----------------------------------------------------------------------------
* Purpose: STL-style circular buffer
*
* ----------------------------------------------------------------------------
* History: See source control system log.
*
*****************************************************************************/
#ifndef CIRCULAR_BUFFER_H
#define CIRCULAR_BUFFER_H
#include <exception>
#include <iterator>
#include <memory>
/******************************************************************************
* Iterators
*****************************************************************************/
/**
* Iterator type for the circular_buffer class.
*
* This one template class provides all variants of forward/reverse
* const/non const iterators through plentiful template magic.
*
* You don't need to instantiate it directly, use the good public functions
* availble in circular_buffer.
*/
template <typename T, //circular_buffer type
//(incl const)
typename T_nonconst, //with any consts
typename elem_type = typename T::value_type> //+ const for const iter
class circular_buffer_iterator
{
public:
typedef circular_buffer_iterator<T, T_nonconst, elem_type> self_type;
typedef T cbuf_type;
typedef std::random_access_iterator_tag iterator_category;
typedef typename cbuf_type::value_type value_type;
typedef typename cbuf_type::size_type size_type;
typedef typename cbuf_type::pointer pointer;
typedef typename cbuf_type::const_pointer const_pointer;
typedef typename cbuf_type::reference reference;
typedef typename cbuf_type::const_reference const_reference;
typedef typename cbuf_type::difference_type difference_type;
circular_buffer_iterator(cbuf_type *b, size_type p)
: buf_(b), pos_(p) {}
// Converting a non-const iterator to a const iterator
circular_buffer_iterator
(const circular_buffer_iterator<T_nonconst, T_nonconst,
typename T_nonconst::value_type>
&other)
: buf_(other.buf_), pos_(other.pos_) {}
friend class circular_buffer_iterator<const T, T, const elem_type>;
// Use compiler generated copy ctor, copy assignment operator and dtor
elem_type &operator*() { return (*buf_)[pos_]; }
elem_type *operator->() { return &(operator*()); }
self_type &operator++()
{
pos_ += 1;
return *this;
}
self_type operator++(int)
{
self_type tmp(*this);
++(*this);
return tmp;
}
self_type &operator--()
{
pos_ -= 1;
return *this;
}
self_type operator--(int)
{
self_type tmp(*this);
--(*this);
return tmp;
}
self_type operator+(difference_type n) const
{
self_type tmp(*this);
tmp.pos_ += n;
return tmp;
}
self_type &operator+=(difference_type n)
{
pos_ += n;
return *this;
}
self_type operator-(difference_type n) const
{
self_type tmp(*this);
tmp.pos_ -= n;
return tmp;
}
self_type &operator-=(difference_type n)
{
pos_ -= n;
return *this;
}
difference_type operator-(const self_type &c) const
{
return pos_ - c.pos_;
}
bool operator==(const self_type &other) const
{
return pos_ == other.pos_ && buf_ == other.buf_;
}
bool operator!=(const self_type &other) const
{
return pos_ != other.pos_ && buf_ == other.buf_;
}
bool operator>(const self_type &other) const
{
return pos_ > other.pos_;
}
bool operator>=(const self_type &other) const
{
return pos_ >= other.pos_;
}
bool operator<(const self_type &other) const
{
return pos_ < other.pos_;
}
bool operator<=(const self_type &other) const
{
return pos_ <= other.pos_;
}
private:
cbuf_type *buf_;
size_type pos_;
};
template <typename circular_buffer_iterator_t>
circular_buffer_iterator_t operator+
(const typename circular_buffer_iterator_t::difference_type &a,
const circular_buffer_iterator_t &b)
{
return circular_buffer_iterator_t(a) + b;
}
template <typename circular_buffer_iterator_t>
circular_buffer_iterator_t operator-
(const typename circular_buffer_iterator_t::difference_type &a,
const circular_buffer_iterator_t &b)
{
return circular_buffer_iterator_t(a) - b;
}
/******************************************************************************
* circular_buffer
*****************************************************************************/
/**
* This class provides a circular buffer in the STL style.
*
* You can add data to the end using the @ref push_back function, read data
* using @ref front() and remove data using @ref pop_front().
*
* The class also provides random access through the @ref operator[]()
* function and its random access iterator. Subscripting the array with
* an invalid (out of range) index number leads to undefined results, both
* for reading and writing.
*
* This class template accepts three template parameters:
* <li> T The type of object contained
* <li> always_accept_data_when_full Determines the behaviour of
* @ref push_back when the buffer is full.
* Set to true new data is always added, the
* old "end" data is thrown away.
* Set to false, the new data is not added.
* No error is returned neither is an
* exception raised.
* <li> Alloc Allocator type to use (in line with other
* STL containers).
*
* @short STL style circule buffer
* @author Pete Goodliffe
* @version 1.00
*/
template <typename T,
bool always_accept_data_when_full = true,
typename Alloc = std::allocator<T> >
class circular_buffer
{
public:
enum
{
version_major = 1,
version_minor = 0
};
// Typedefs
typedef circular_buffer<T, always_accept_data_when_full, Alloc>
self_type;
typedef Alloc allocator_type;
typedef typename Alloc::value_type value_type;
typedef typename Alloc::pointer pointer;
typedef typename Alloc::const_pointer const_pointer;
typedef typename Alloc::reference reference;
typedef typename Alloc::const_reference const_reference;
typedef typename Alloc::size_type size_type;
typedef typename Alloc::difference_type difference_type;
typedef circular_buffer_iterator
<self_type, self_type>
iterator;
typedef circular_buffer_iterator
<const self_type, self_type, const value_type>
const_iterator;
typedef std::reverse_iterator<iterator> reverse_iterator;
typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
// Lifetime
enum { default_capacity = 100 };
explicit circular_buffer(size_type capacity = default_capacity)
: array_(alloc_.allocate(capacity)), array_size_(capacity),
head_(1), tail_(0), contents_size_(0)
{
}
circular_buffer(const circular_buffer &other)
: array_(alloc_.allocate(other.array_size_)),
array_size_(other.array_size_),
head_(other.head_), tail_(other.tail_),
contents_size_(other.contents_size_)
{
try
{
assign_into(other.begin(), other.end());
}
catch (...)
{
destroy_all_elements();
alloc_.deallocate(array_, array_size_);
throw;
}
}
template <class InputIterator>
circular_buffer(InputIterator from, InputIterator to)
: array_(alloc_.allocate(1)), array_size_(1),
head_(1), tail_(0), contents_size_(0)
{
circular_buffer tmp;
tmp.assign_into_reserving(from, to);
swap(tmp);
}
~circular_buffer()
{
destroy_all_elements();
alloc_.deallocate(array_, array_size_);
}
circular_buffer &operator=(const self_type &other)
{
circular_buffer tmp(other);
swap(tmp);
return *this;
}
void swap(circular_buffer &other)
{
std::swap(array_, other.array_);
std::swap(array_size_, other.array_size_);
std::swap(head_, other.head_);
std::swap(tail_, other.tail_);
std::swap(contents_size_, other.contents_size_);
}
allocator_type get_allocator() const { return alloc_; }
// Iterators
iterator begin() { return iterator(this, 0); }
iterator end() { return iterator(this, size()); }
const_iterator begin() const { return const_iterator(this, 0); }
const_iterator end() const { return const_iterator(this, size()); }
reverse_iterator rbegin() { return reverse_iterator(end()); }
reverse_iterator rend() { return reverse_iterator(begin()); }
const_reverse_iterator rbegin() const
{
return const_reverse_iterator(end());
}
const_reverse_iterator rend() const
{
return const_reverse_iterator(begin());
}
// Size
size_type size() const { return contents_size_; }
size_type capacity() const { return array_size_; }
bool empty() const { return !contents_size_; }
size_type max_size() const
{
return alloc_.max_size();
}
void reserve(size_type new_size)
{
if (capacity() < new_size)
{
circular_buffer tmp(new_size);
tmp.assign_into(begin(), end());
swap(tmp);
}
}
// Accessing
reference front() { return array_[head_]; }
reference back() { return array_[tail_]; }
const_reference front() const { return array_[head_]; }
const_reference back() const { return array_[tail_]; }
void push_back(const value_type &item)
{
size_type next = next_tail();
if (contents_size_ == array_size_)
{
if (always_accept_data_when_full)
{
array_[next] = item;
increment_head();
}
}
else
{
alloc_.construct(array_ + next, item);
}
increment_tail();
}
void pop_front()
{
size_type destroy_pos = head_;
increment_head();
alloc_.destroy(array_ + destroy_pos);
}
void clear()
{
for (size_type n = 0; n < contents_size_; ++n)
{
alloc_.destroy(array_ + index_to_subscript(n));
}
head_ = 1;
tail_ = contents_size_ = 0;
}
reference operator[](size_type n) { return at_unchecked(n); }
const_reference operator[](size_type n) const { return at_unchecked(n); }
reference at(size_type n) { return at_checked(n); }
const_reference at(size_type n) const { return at_checked(n); }
private:
reference at_unchecked(size_type index) const
{
return array_[index_to_subscript(index)];
}
reference at_checked(size_type index) const
{
if (size >= contents_size_)
{
throw std::out_of_range();
}
return at_unchecked(index);
}
// Rounds an unbounded to an index into array_
size_type normalise(size_type n) const { return n % array_size_; }
// Converts external index to an array subscript
size_type index_to_subscript(size_type index) const
{
return normalise(index + head_);
}
void increment_tail()
{
++contents_size_;
tail_ = next_tail();
}
size_type next_tail()
{
return (tail_ + 1 == array_size_) ? 0 : tail_ + 1;
}
void increment_head()
{
// precondition: !empty()
++head_;
--contents_size_;
if (head_ == array_size_) head_ = 0;
}
template <typename f_iter>
void assign_into(f_iter from, f_iter to)
{
if (contents_size_) clear();
while (from != to)
{
push_back(*from);
++from;
}
}
template <typename f_iter>
void assign_into_reserving(f_iter from, f_iter to)
{
if (contents_size_) clear();
while (from != to)
{
if (contents_size_ == array_size_)
{
reserve(static_cast<size_type>(array_size_ * 1.5));
}
push_back(*from);
++from;
}
}
void destroy_all_elements()
{
for (size_type n = 0; n < contents_size_; ++n)
{
alloc_.destroy(array_ + index_to_subscript(n));
}
}
allocator_type alloc_;
value_type *array_;
size_type array_size_;
size_type head_;
size_type tail_;
size_type contents_size_;
};
template <typename T,
bool consume_policy,
typename Alloc>
bool operator==(const circular_buffer<T, consume_policy, Alloc> &a,
const circular_buffer<T, consume_policy, Alloc> &b)
{
return a.size() == b.size() && std::equal(a.begin(), a.end(), b.begin());
}
template <typename T,
bool consume_policy,
typename Alloc>
bool operator!=(const circular_buffer<T, consume_policy, Alloc> &a,
const circular_buffer<T, consume_policy, Alloc> &b)
{
return a.size() != b.size() || !std::equal(a.begin(), a.end(), b.begin());
}
template <typename T,
bool consume_policy,
typename Alloc>
bool operator<(const circular_buffer<T, consume_policy, Alloc> &a,
const circular_buffer<T, consume_policy, Alloc> &b)
{
return std::lexicographical_compare(a.begin(), a.end(), b.begin(), b.end());
}
#endif