filter_interface.h

浏览该文件的文档.

 
#ifndef FILTER_INTERFACE_H_
#define FILTER_INTERFACE_H_
 
 
 
#ifdef __cplusplus
namespace HYYRobotBase
{
extern "C" {
#endif
 
//-------------------------------滑动均值滤波----------------------
#define ONLINEFILTERNUM 10
#define MoveAverageFilter_BUF 2000
 
 
/*-------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*/
typedef struct MoveAverageFilter{
    int buf_num;
    double buf[MoveAverageFilter_BUF];
    int _index;
    int one_flag;
}MoveAverageFilter;
 
/*-------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*/
typedef struct MoveAverageFilters{
    MoveAverageFilter maf[ONLINEFILTERNUM];
    int dof;
}MoveAverageFilters;
 
extern void init_move_average_filter_online(MoveAverageFilter* maf, int buf_num);
 
 
extern double move_average_filter_online(MoveAverageFilter* maf, double value);
 
extern void init_move_average_filter_onlines(MoveAverageFilters* mafs, int buf_num, int _dof);
 
 
extern void move_average_filter_onlines(MoveAverageFilters* mafs, double* value_in, double* value_out, int _dof);
 
 
 
//-------------------------------------------------------------------卡尔曼滤波---------------------------------------------------------
#define MATRIX_MAX 100
 
/*-------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*/
typedef struct RMATRIX{
    double data[MATRIX_MAX][MATRIX_MAX]; 
    int row; 
    int col; 
}RMATRIX;
 
/*-------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*/
typedef struct KalmanFilter{
    RMATRIX A,C,Q,R,P,K,I; 
    int m, n; 
    double dt; 
    RMATRIX x_hat, x_hat_new;
 
    RMATRIX AT,CT;
    RMATRIX y;
    RMATRIX tmp, tmp1;
 
 
}KalmanFilter;
 
extern void initKalmanFilter(KalmanFilter* kf,double dt, RMATRIX* A, RMATRIX* C, RMATRIX* Q, RMATRIX* R, RMATRIX* P, RMATRIX* x0);
 
 
extern RMATRIX* KalmanFilter_update(KalmanFilter* kf, RMATRIX* y);
 
extern RMATRIX* KalmanFilter_update1(KalmanFilter* kf, RMATRIX* y, RMATRIX* A);
 
extern RMATRIX* getKalmanFilter_state(KalmanFilter* kf);
 
extern RMATRIX* getKalmanFilter_out(KalmanFilter* kf);
 
extern void initKalmanFilter_d(KalmanFilter* kf,double dt, double* A, double* C, double* Q, double* R, double* P, double* x0, int m, int n);
 
extern void initKalmanFilter_d1(KalmanFilter* kf,double dt, double** A, double** C, double** Q, double** R, double** P, double* x0, int m, int n);
 
//return state(n,1)
extern double* KalmanFilter_update_d(KalmanFilter* kf, double* y, double* filter_state);
 
//return state(n,1)
extern double* KalmanFilter_update1_d(KalmanFilter* kf, double* y, double* A, double* filter_state);
 
//return state(n,1)
extern double* KalmanFilter_update1_d1(KalmanFilter* kf, double* y, double** A, double* filter_state);
 
//return state(n,1)
extern double* getKalmanFilter_state_d(KalmanFilter* kf, double* state);
 
//return out (m,1)
extern double* getKalmanFilter_out_d(KalmanFilter* kf, double* out);
 
 
//----------------------------------------------------数据滤波系数矩阵, 状态为位置，速度，加速度组成的向量-------------------------------------------------
extern double* getKalmanFilterA(double* A, double dt, int n);
 
extern double* getKalmanFilterC(double* C, double dt, int m);
 
extern double* getKalmanFilterQ(double* Q, double value, int n);
 
extern double* getKalmanFilterR(double* R, double value, int m);
 
extern double* getKalmanFilterP(double* P, double value, int n);
 
 
//-------------------------------------------------------------------IIR滤波---------------------------------------------------------
#define ARRAY_DIM 50  
/*-------------------------------------------------------------------------*/
 
/*-------------------------------------------------------------------------*/
typedef enum TIIRPassTypes {
    iirLPF,  
    iirHPF,  
    iirBPF,  
    iirNOTCH,  
    iirALLPASS 
}TIIRPassTypes;
 
 
typedef struct TIIRCoeff {
    double a0[ARRAY_DIM]; double a1[ARRAY_DIM]; double a2[ARRAY_DIM]; double a3[ARRAY_DIM]; double a4[ARRAY_DIM];
    double b0[ARRAY_DIM]; double b1[ARRAY_DIM]; double b2[ARRAY_DIM]; double b3[ARRAY_DIM]; double b4[ARRAY_DIM];
    int NumSections;
}TIIRCoeff;
 
/*-------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*/
typedef enum TFilterPoly {
    BUTTERWORTH,  
    GAUSSIAN,  
    BESSEL,  
    ADJUSTABLE,  
    CHEBYSHEV, 
    INVERSE_CHEBY, 
    PAPOULIS,  
    ELLIPTIC,  
    NOT_IIR 
}TFilterPoly;
 
/*-------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*/
typedef struct TIIRFilterParams {
    TIIRPassTypes IIRPassType;     
    double OmegaC;                 
    double BW;                     
    double dBGain;                 
 
    TFilterPoly ProtoType;  
    int NumPoles;           
    double Ripple;          
    double StopBanddB;      
    double Gamma;           
}TIIRFilterParams;
 
 
/*-------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*/
typedef struct TIIRFilter{
    TIIRFilterParams IIRFilt;  
    TIIRCoeff IIRCoeff;
    double RegX1[ARRAY_DIM], RegX2[ARRAY_DIM], RegY1[ARRAY_DIM], RegY2[ARRAY_DIM];
    int flag_one;
}TIIRFilter;
 
/*-------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*/
typedef struct TIIRFilters{
    TIIRFilter irrfiler[10];  
    int n;
}TIIRFilters;
 
 
extern void initTIIRFilter(TIIRFilter* iirfiler, TIIRPassTypes types, double cornerFreq, double sampleFreq);
 
extern double IIRFilter(TIIRFilter* iirfiler, double Signal);
 
extern double* IIRFilterZeroPhase(TIIRFilter* iirfiler, double* Signal_in, double* Signal_out, int n);
 
extern void initTIIRFilters(TIIRFilters* iirfilers, TIIRPassTypes types, double cornerFreq, double sampleFreq, int n);
 
extern void IIRFilters(TIIRFilters* iirfilers, double* in, double* out);
 
 
 
 
#ifdef __cplusplus
}
}
#endif
 
 
#endif /* FILTER_INTERFACE_H_ */