Commit fe1077ad authored by Michael Zbyszyński's avatar Michael Zbyszyński

Removing duplicates

parent 49776d0b
/**
* @file BayesianFilter.cpp
* @author Jules Francoise jules.francoise@ircam.fr
* @date 2013-12-24
*
* @brief Non-linear Bayesian filtering for EMG Enveloppe Extraction.
* Based on Matlab code by Terence Sanger : kidsmove.org/bayesemgdemo.html
* Reference:
* - Sanger, T. (2007). Bayesian filtering of myoelectric signals. Journal of neurophysiology, 1839–1845.
*
* @copyright
* Copyright (C) 2013-2014 by IRCAM - Centre Pompidou.
* All Rights Reserved.
*
* License (BSD 3-clause)
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "BayesianFilter.h"
#include "filter_utilities.h"
#pragma mark -
#pragma mark Constructors
BayesianFilter::BayesianFilter()
{
mvc.assign(channels, 1.);
init();
}
BayesianFilter::~BayesianFilter()
{
}
void BayesianFilter::resize(std::size_t size)
{
if (size > 0) {
channels = size;
init();
}
}
std::size_t BayesianFilter::size() const
{
return channels;
}
#pragma mark -
#pragma mark Main Algorithm
void BayesianFilter::init()
{
mvc.resize(channels, 1.);
output.assign(channels, 0.);
prior.resize(channels);
state.resize(channels);
g.resize(channels);
for (unsigned int i=0; i<channels; i++) {
prior[i].resize(levels);
state[i].resize(levels);
g[i].resize(3);
double val(1.);
for (unsigned int t=0; t<levels; t++) {
state[i][t] = val * mvc[i] / double(levels);
val += 1;
prior[i][t] = 1. / levels;
}
double diff = diffusion * diffusion / (samplerate * std::pow(mvc[i] / levels, 2));
g[i][0] = diff / 2.;
g[i][1] = 1. - diff - this->jump_rate;
g[i][2] = diff / 2.;
}
}
void BayesianFilter::update(vector<float> const& observation)
{
if (observation.size() != this->channels) {
resize(observation.size());
}
for (std::size_t i=0; i<channels; i++)
{
// -- 1. Propagate
// -----------------------------------------
vector<double> a(1, 1.);
vector<double> oldPrior(prior[i].size());
// oldPrior.swap(prior[i]);
copy(prior[i].begin(), prior[i].end(), oldPrior.begin());
filtfilt(g[i], a, oldPrior, prior[i]);
// set probability of a sudden jump
for (unsigned int t=0; t<levels; t++) {
prior[i][t] = prior[i][t] + jump_rate / mvc[i];
}
// -- 4. Calculate the posterior likelihood function
// -----------------------------------------
// calculate posterior density using Bayes rule
vector<double> posterior(levels);
double sum_posterior(0.);
for (unsigned int t=0; t<levels; t++) {
double x_2 = state[i][t] * state[i][t];
posterior[t] = this->prior[i][t] * exp(- observation[i] * observation[i] / x_2) / x_2;
sum_posterior += posterior[t];
}
// -- 5. Output the signal estimate output(x(t)) = argmax P(x,t);
// -----------------------------------------
// find the maximum of the posterior density
unsigned int pp(0);
double tmpMax(posterior[0]);
for (unsigned int t=0; t<levels; t++) {
if (posterior[t] > tmpMax) {
tmpMax = posterior[t];
pp = t;
}
posterior[t] /= sum_posterior;
}
// convert index of peak value to scaled EMG value
output[i] = state[i][pp] / mvc[i];
// -- 7. Repeat from step 2 > prior for next iteration is posterior from this iteration
// -----------------------------------------
copy(posterior.begin(), posterior.end(), prior[i].begin());
}
}
/**
* @file BayesianFilter.h
* @author Jules Francoise jules.francoise@ircam.fr
* @date 2013-12-24
*
* @brief Non-linear Bayesian filtering for EMG Enveloppe Extraction.
* Based on Matlab code by Terence Sanger : kidsmove.org/bayesemgdemo.html
* Reference:
* - Sanger, T. (2007). Bayesian filtering of myoelectric signals. Journal of neurophysiology, 1839–1845.
*
* @copyright
* Copyright (C) 2013-2014 by IRCAM - Centre Pompidou.
* All Rights Reserved.
*
* License (BSD 3-clause)
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef __emg_bayesfilter__BayesianFilter__
#define __emg_bayesfilter__BayesianFilter__
#include <algorithm>
#include <iostream>
#include <cmath>
#include <vector>
using namespace std;
/*!
@mainpage
# Bayesian Filtering for EMG envelope extraction
Non-linear baysian filter.
Code is based on Matlab example code from Terence Sanger, available at [kidsmove.org/bayesemgdemo.html](kidsmove.org/bayesemgdemo.html)
## Reference:
* Sanger, T. (2007). __Bayesian filtering of myoelectric signals.__ _Journal of neurophysiology_, 1839–1845.
## Contact
@copyright Copyright (C) 2012-2014 by IRCAM. All Rights Reserved.
@author Jules Francoise - Ircam - jules.francoise@ircam.fr
@date 2013-12-26
*/
/*!
@class BayesianFilter
@brief Main class for non-linear Bayesian fitering
@copyright Copyright 2013 Ircam - Jules Francoise. All Rights Reserved.
@author Jules Francoise - Ircam - jules.francoise@ircam.fr
@date 2013-12-26
*/
class BayesianFilter {
public:
#pragma mark -
#pragma mark Public attributes
vector<float> output; //<! bayes estimates
#pragma mark -
#pragma mark Constructors
/*!
Constructor
@param _samplerate Sampling frequency of input stream
@param _clipping Signal Clipping
@param _alpha Diffusion rate
@param _beta Probability of sudden jumps
@param _levels number of output levels
@param _rectification signal rectification
*/
BayesianFilter();
~BayesianFilter();
void resize(std::size_t size);
std::size_t size() const;
#pragma mark -
#pragma mark Main Algorithm
/*!
@brief Initialize filter
Resets Prior to uniform distribution
*/
void init();
/*!
@brief Update filter state and compute prediction.
The output of the system (envelope estimated by Maximum A Posteriori) is stored in the public member output
@param observation observation (input) vector
*/
void update(vector<float> const& observation);
#pragma mark -
#pragma mark Python
#ifdef SWIGPYTHON
void update(int _inchannels, double *observation, int _outchannels, double *_output)
{
vector<float> observation_vec(_inchannels);
for (unsigned int t=0; t<_inchannels; t++)
observation_vec[t] = float(observation[t]);
this->update(observation_vec);
for (unsigned int t=0; t<_inchannels; t++)
_output[t] = double(this->output[t]);
}
#endif
/**
@brief Maximum Value contraction (estimated using Standard deviation on isometric max contraction)
*/
std::vector<double> mvc;
/**
@brief Number of output levels
*/
unsigned int levels = 100;
/**
@brief Sampling frequency
*/
double samplerate = 200.;
/**
@brief Diffusion rate (typically 1e-9 <> 1e-1)
*/
double diffusion = 0.1;
/**
@brief Probability of sudden jumps (typically 1e-24 <> 1e-1)
*/
double jump_rate = 0.1;
protected:
/**
@brief Number of input channels
*/
std::size_t channels = 1;
/**
@brief Latent variable (the driving rate)
*/
vector< vector<double> > state;
/**
@brief Prior
*/
vector< vector<double> > prior;
/**
@brief Approximate spatial second derivative operator
*/
vector< vector<double> > g;
};
#endif
//
/**
* @file filter_utilities.cpp
* @author Jules Francoise jules.francoise@ircam.fr
* @date 2013-12-24
*
* @brief Filtering utilities
* >> c++ implementations of scipy.signal standard filtering functions
*
* @copyright
* Copyright (C) 2013-2014 by IRCAM - Centre Pompidou.
* All Rights Reserved.
*
* License (BSD 3-clause)
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "filter_utilities.h"
void filtfilt(vector<double> const& b, vector<double> const& a, vector<double> & x, vector<double> & y, PADTYPE padtype, int padlen)
{
int ntaps = max(a.size(), b.size());
if (padtype == NONE)
padlen=0;
unsigned int edge;
if (padlen < 0)
edge = ntaps * 3;
else
edge = padlen;
if (x.size() <= edge)
throw runtime_error("The length of the input vector x must be at least padlen.");
vector<double> ext;
if (padtype != NONE && edge > 0) {
// Make an extension of length 'edge' at each
// end of the input array.
switch (padtype) {
case EVEN:
even_ext<double>(x, ext, edge);
break;
case ODD:
odd_ext<double>(x, ext, edge);
break;
default:
const_ext<double>(x, ext, edge);
}
} else {
ext = x;
}
// Get the steady state of the filter's step response.
vector<double> zi;
lfilter_zi(b, a, zi);
// Forward filter.
y.resize(ext.size());
vector<double> zip = zi;
for (unsigned int i=0 ; i<zi.size(); i++) {
zip[i] = zi[i] * x[0];
}
vector<double> y_reverse(ext.size());
lfilter(b, a, ext, y_reverse, zip);
reverse_copy(y_reverse.begin(), y_reverse.end(), y.begin());
// Backward filter.
// Create y0 so zi*y0 broadcasts appropriately.
for (unsigned int i=0 ; i<zip.size(); i++) {
zip[i] = zi[i] * x[x.size()-1];
}
lfilter(b, a, y, y_reverse, zip);
// Reverse y.
y.resize(x.size());
reverse_copy(y_reverse.begin()+edge, y_reverse.end()-edge, y.begin());
}
void lfilter(vector<double> const& b, vector<double> const& a, vector<double> const& x, vector<double> & y, vector<double> const& zi)
{
vector<double> _b = b;
vector<double> _a = a;
// Pad a or b with zeros so they are the same length.
unsigned int k = max(a.size(), b.size());
if (_a.size() < k)
_a.resize(k, 0.);
else if (_b.size() < k)
_b.resize(k, 0.);
if (_a[0] != 1.0) {
// Normalize the coefficients so a[0] == 1.
for (unsigned int i=0; i<k; i++) {
_a[i] /= _a[0];
_b[i] /= _a[0];
}
}
vector<double> z = zi;
unsigned int n = x.size();
y.resize(n);
for (unsigned int m=0; m<n; m++) {
y[m] = _b[0] * x[m] + z[0];
for (unsigned int i=0; i<k-2; i++) {
z[i] = _b[i+1] * x[m] + z[i+1] - _a[i+1] * y[m];
}
z[k-2] = _b[k-1] * x[m] - _a[k-1] * y[m];
}
}
void lfilter_zi(vector<double> const& b, vector<double> const& a, vector<double> & zi) {
vector<double> _b = b;
vector<double> _a = a;
if (_a[0] != 1.0) {
// Normalize the coefficients so a[0] == 1.
for (unsigned int i=0; i<_a.size(); i++) {
_a[i] /= _a[0];
_b[i] /= _a[0];
}
}
unsigned int n = max(_a.size(), _b.size());
// Pad a or b with zeros so they are the same length.
if (_a.size() < n)
_a.resize(n, 0.);
else if (_b.size() < n)
_b.resize(n, 0.);
vector<double> IminusA((n-1)*(n-1), 0.);
for (unsigned int i=0; i<n-1; i++) {
IminusA[i*(n-1)+i] = 1.;
}
vector<double> companion((n-1)*(n-1), 0.);
for (unsigned int i=0; i<n-1; i++) {
companion[i] = -_a[i+1] / (1.0 * _a[0]);
}
for (unsigned int i=1; i<n-1; i++) {
companion[i*(n-1)+i-1] = 1.;
}
for (unsigned int i=0; i<n-1; i++) {
for (unsigned int j=0; j<n-1; j++) {
IminusA[i*(n-1)+j] -= companion[j*(n-1)+i];
}
}
zi.clear();
zi.resize(n - 1, 0.);
double tmpSum(0.);
for (unsigned int i=0; i<n-1; i++) {
zi[0] += _b[i+1] - _a[i+1] * _b[0];
tmpSum += IminusA[i*(n-1)];
}
zi[0] /= tmpSum;
// TODO: remove B
// Solve zi = A*zi + B
double asum = 1.0;
double csum = 0.0;
for (unsigned int k=1; k<n-1; k++) {
asum += _a[k];
csum += _b[k] - _a[k]*_b[0];
zi[k] = asum*zi[0] - csum;
}
}
/**
* @file filter_utilities.h
* @author Jules Francoise jules.francoise@ircam.fr
* @date 2013-12-24
*
* @brief Filtering utilities
* >> c++ implementations of scipy.signal standard filtering functions
*
* @copyright
* Copyright (C) 2013-2014 by IRCAM - Centre Pompidou.
* All Rights Reserved.
*
* License (BSD 3-clause)
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef __emg_bayesfilter__filter_utilities__
#define __emg_bayesfilter__filter_utilities__
#include <algorithm>
#include <iostream>
#include <exception>
#include <stdexcept>
#include <vector>
using namespace std;
typedef enum _padtype {
EVEN,
ODD,
CONSTANT,
NONE
} PADTYPE;
void filtfilt(vector<double> const& b, vector<double> const& a, vector<double> & x, vector<double> & y, PADTYPE padtype = ODD, int padlen=-1);
void lfilter_zi(vector<double> const& b, vector<double> const& a, vector<double> & zi);
void lfilter(vector<double> const& b, vector<double> const& a, vector<double> const& x, vector<double> & y, vector<double> const& zi);
/*!
1D python-like even_ext function.
*/
template <typename datatype>
void even_ext(vector<datatype> const& src, vector<datatype> & dst, unsigned int n)
{
if (n<1)
dst = src;
if (n > src.size() - 1)
throw runtime_error("The extension length n is too big. It must not exceed src.size()-1.");
dst.resize(2 * n + src.size());
int t(0);
for (int i=n; i>0; i--) {
dst[t++] = src[i];
}
copy(src.begin(), src.end(), dst.begin()+n);
t += src.size();