adding bayesfilter

RapidAPI/RapidAPI.xcodeproj/project.pbxproj

@@ -14,6 +14,8 @@
 		BE325F511DB50BFE00F199A8 /* regression.cpp in Sources */ = {isa = PBXBuildFile; fileRef = BE325F4A1DB50BFE00F199A8 /* regression.cpp */; };
 		BE3852881EE847730008E1BD /* dtw.cpp in Sources */ = {isa = PBXBuildFile; fileRef = BE3852871EE847730008E1BD /* dtw.cpp */; };
 		BE38528B1EE94D740008E1BD /* seriesClassification.cpp in Sources */ = {isa = PBXBuildFile; fileRef = BE3852891EE94D740008E1BD /* seriesClassification.cpp */; };
+		BE3C12711FC85425009446A8 /* BayesianFilter.cpp in Sources */ = {isa = PBXBuildFile; fileRef = BE3C126D1FC85425009446A8 /* BayesianFilter.cpp */; };
+		BE3C12721FC85425009446A8 /* filter_utilities.cpp in Sources */ = {isa = PBXBuildFile; fileRef = BE3C126F1FC85425009446A8 /* filter_utilities.cpp */; };
 		BE553E2F1EE05FE1004EB00F /* svmClassification.cpp in Sources */ = {isa = PBXBuildFile; fileRef = BE553E2E1EE05FE1004EB00F /* svmClassification.cpp */; };
 		BE59A4521F1698E500B26E85 /* jsoncpp.cpp in Sources */ = {isa = PBXBuildFile; fileRef = BE59A44E1F1698E500B26E85 /* jsoncpp.cpp */; };
 		BE59A4531F1698E500B26E85 /* libsvm.cpp in Sources */ = {isa = PBXBuildFile; fileRef = BE59A4501F1698E500B26E85 /* libsvm.cpp */; };
@@ -54,6 +56,10 @@
 		BE3852871EE847730008E1BD /* dtw.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = dtw.cpp; sourceTree = "<group>"; };
 		BE3852891EE94D740008E1BD /* seriesClassification.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = seriesClassification.cpp; sourceTree = "<group>"; };
 		BE38528A1EE94D740008E1BD /* seriesClassification.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = seriesClassification.h; sourceTree = "<group>"; };
+		BE3C126D1FC85425009446A8 /* BayesianFilter.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = BayesianFilter.cpp; sourceTree = "<group>"; };
+		BE3C126E1FC85425009446A8 /* BayesianFilter.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = BayesianFilter.h; sourceTree = "<group>"; };
+		BE3C126F1FC85425009446A8 /* filter_utilities.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = filter_utilities.cpp; sourceTree = "<group>"; };
+		BE3C12701FC85425009446A8 /* filter_utilities.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = filter_utilities.h; sourceTree = "<group>"; };
 		BE553E2D1EE05F03004EB00F /* svmClassification.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = svmClassification.h; sourceTree = "<group>"; };
 		BE553E2E1EE05FE1004EB00F /* svmClassification.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = svmClassification.cpp; sourceTree = "<group>"; };
 		BE553E301EE06AB0004EB00F /* trainingExample.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = trainingExample.h; sourceTree = "<group>"; };
@@ -143,9 +149,22 @@
 			path = ../../src;
 			sourceTree = "<group>";
 		};
+		BE3C126C1FC85425009446A8 /* src */ = {
+			isa = PBXGroup;
+			children = (
+				BE3C126D1FC85425009446A8 /* BayesianFilter.cpp */,
+				BE3C126E1FC85425009446A8 /* BayesianFilter.h */,
+				BE3C126F1FC85425009446A8 /* filter_utilities.cpp */,
+				BE3C12701FC85425009446A8 /* filter_utilities.h */,
+			);
+			name = src;
+			path = bayesfilter/src;
+			sourceTree = "<group>";
+		};
 		BE59A44A1F1698E500B26E85 /* dependencies */ = {
 			isa = PBXGroup;
 			children = (
+				BE3C126C1FC85425009446A8 /* src */,
 				BE59A44B1F1698E500B26E85 /* json */,
 				BE59A44E1F1698E500B26E85 /* jsoncpp.cpp */,
 				BE59A44F1F1698E500B26E85 /* libsvm */,
@@ -233,7 +252,9 @@
 				BE325F351DB50BE100F199A8 /* main.cpp in Sources */,
 				BEF47B181F790BC7005B0C35 /* neuralNetwork.cpp in Sources */,
 				BE6783241F6FFA5C005E73D0 /* warpPath.cpp in Sources */,
+				BE3C12711FC85425009446A8 /* BayesianFilter.cpp in Sources */,
 				BE325F4E1DB50BFE00F199A8 /* knnClassification.cpp in Sources */,
+				BE3C12721FC85425009446A8 /* filter_utilities.cpp in Sources */,
 				BE59A4531F1698E500B26E85 /* libsvm.cpp in Sources */,
 				BE325F4F1DB50BFE00F199A8 /* modelSet.cpp in Sources */,
 				BE325F4D1DB50BFE00F199A8 /* classification.cpp in Sources */,

dependencies/bayesfilter/.gitignore

+**/.DS_Store
+**/Thumbs.db

dependencies/bayesfilter/README.md

+# INSTALLATION
+
+## Requirements
+* Swig
+* Python 2.7 + Numpy / Scipy (I think Numpy v1.8+ is required)
+
+## Building
+move 'python' directory and type:
+
+	make
+	make install
+	make clean
+
+To install in a specific location:
+
+	make install INSTALL_DIR=/path/to/install/location/
+
+To specify swig location:
+
+	make install SWIG=/path/to/swig/
+
+# usage
+Place the built python library somewhere in your python path. To add personal 
+libraries located in '/Path/To/Libs' to the python path, add the following 
+lines to your ".bash_profile":
+
+	PYTHONPATH=$PYTHONPATH:/Path/To/Libs
+	export PYTHONPATH
+
+Basic usage: 
+
+	import bayesfilter
+	
+	filt = bayesfilter.BayesianFilter()
+	filt.init()
+	
+	for t in RAW_EMG_MATRIX.shape[0]:
+		envelope_at_t = filt.update(RAW_EMG_MATRIX[t,:])
+
+# credits
+
+bayesfilter has been authored by Jules Françoise : jules.francoise@ircam.fr
\ No newline at end of file

dependencies/bayesfilter/bayesfilter/BayesianFilter.cpp

+/**
+ * @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());
+    }
+}

dependencies/bayesfilter/bayesfilter/BayesianFilter.h

+/**
+ * @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
+

dependencies/bayesfilter/bayesfilter/filter_utilities.cpp

+//
+/**
+ * @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());
+    
+