view include/IirFilter.h @ 149:134bff10e561 ClockSync

Added simple one-variable one-measurement Kalman filter, Pid controller(which output is not used). Virtual clock is now much more precise and reactive for period. Still it is lagging behind a bit on the overall offset.

/*
 * IirFilter.h
 *
 *  Created on: 17 Sep 2015
 *      Author: giulio
 */

#ifndef IIRFILTER_H_
#define IIRFILTER_H_

#define IIR_FILTER_STAGE_COEFFICIENTS (5)
#define IIR_FILTER_STAGE_STATES (IIR_FILTER_STAGE_COEFFICIENTS - 1)

class IirFilterStage{ //TODO : to save (some) memory we should only store the coefficients pointers here,
						//so that IirFilter can share them among multiple stages if needbe)
private:
	double coefficients[IIR_FILTER_STAGE_COEFFICIENTS]; // these are b0,b1,b2,a1,a2
	double states[IIR_FILTER_STAGE_STATES]; // these are xprev, xprevprev, yprev, yprevprev
public:
	IirFilterStage();
	void setCoefficients(double* newCoefficients);
	void setStates(double* newStates);
	double process(double in);
	void process(double* inout, int length);
	void process(double* in, double* out, int length);
};

class IirFilter{
private:
	struct IirFilterStage** stages;
	int numberOfStages;
	void dealloc();
public:
	IirFilter();
	IirFilter(int newNumberOfStages);
	IirFilter(int newNumberOfStages, double *newCoefficients);
	void setCoefficients(double* newCoefficients);
	void setStates(double* newStates);
	void setNumberOfStages(int newNumberOfStages);
	double process(double in);
	void process(double* inout, int length);
	void process(double* in, double* out, int length);
};

#endif /* IIRFILTER_H_ */