#ifndef	__FILTER__H
#define	__FILTER__H

//*************************************************************

typedef struct
{
    float x;  // state
    float A;  // x(n)=A*x(n-1)+u(n),u(n)~N(0,q)
    float H;  // z(n)=H*x(n)+w(n),w(n)~N(0,r)
    float q;  // process(predict) noise convariance
    float r;  // measure noise convariance
    float p;  // estimated error convariance
    float gain;
} Kalman1_Str;

typedef struct
{
    float x[2];     // state: [0]-angle [1]-diffrence of angle, 2x1
    float A[2][2];  // X(n)=A*X(n-1)+U(n),U(n)~N(0,q), 2x2
    float H[2];     // Z(n)=H*X(n)+W(n),W(n)~N(0,r), 1x2
    float q[2];     // process(predict) noise convariance,2x1 [q0,0; 0,q1]
    float r;        // measure noise convariance
    float p[2][2];  // estimated error convariance,2x2 [p0 p1; p2 p3]
    float gain[2];  // 2x1
} Kalman2_Str;

//*************************************************************
extern	INT32S	FILTER_Fir(INT32S Value);
extern	INT32S	FILTER_Iir(INT32S Value);
extern	void	FILTER_Kalman1Init(Kalman1_Str *ptr, float init_x, float init_p);
extern	float	FILTER_Kalman1(Kalman1_Str *ptr, float z_measure);
extern	void	FILTER_Kalman2Init(Kalman2_Str *ptr, float *init_x, float (*init_p)[2]);
extern	float	FILTER_Kalman2(Kalman2_Str *ptr, float z_measure);

#endif