#include	"config.h"


//FIR滤波算法
INT32S	FILTER_Fir(INT32S Value)
{
	#define	FILTER_FIR_LEN	10
	static INT8U	tState=0;
	static INT32S	tBuff[FILTER_FIR_LEN]={0,0,0,0,0,0,0,0,0,0};
	static INT32S	*ptr=(INT32S *)tBuff+(FILTER_FIR_LEN-1);
	static INT32S	tSum=(FILTER_FIR_LEN>>1);

	if(tState == 0)	//第一个数据进来，初始化
	{
		tSum += Value*FILTER_FIR_LEN;
		*ptr=Value;
		tState = 1;
	}
	else
	{
		tSum += Value;
	}
	tSum -= *ptr;
	*ptr-- = Value;
	
	if(ptr < tBuff)
	{
		ptr += FILTER_FIR_LEN;
	}
	return(tSum/FILTER_FIR_LEN);
}

//IIR滤波
INT32S	FILTER_Iir(INT32S Value)
{
	#define	FILTER_IIR_LEN	10
	static INT32S	tSum=0,tPer=0;
	tSum += Value;
	tSum -= tPer;
	tPer = tSum/FILTER_IIR_LEN;
	
	return(tPer);
}

//一维卡尔曼滤波
void	FILTER_Kalman1Init(Kalman1_Str *ptr, float init_x, float init_p)
{
    ptr->x = init_x;
    ptr->p = init_p;
    ptr->A = 1;
    ptr->H = 1;
    ptr->q = 2e2;	//10e-6;  // predict noise convariance 
    ptr->r = 5e2;	//10e-5;  // measure error convariance 
}

float	FILTER_Kalman1(Kalman1_Str *ptr, float z_measure)
{
    // Predict 
    ptr->x = ptr->A * ptr->x;
    ptr->p = ptr->A * ptr->A * ptr->p + ptr->q;  // p(n|n-1)=A^2*p(n-1|n-1)+q 

    // Measurement 
    ptr->gain = ptr->p * ptr->H / (ptr->p * ptr->H * ptr->H + ptr->r);
    ptr->x = ptr->x + ptr->gain * (z_measure - ptr->H * ptr->x);
    ptr->p = (1 - ptr->gain * ptr->H) * ptr->p;

    return ptr->x;
}

//二维卡尔曼滤波
//init_x：待测量的初始值，如有中值一般设成中值（如陀螺仪）
//init_p：后验状态估计值误差的方差的初始值
//q：预测（过程）噪声方差
//r：测量（观测）噪声方差。
//其中q和r参数尤为重要，一般得通过实验测试得到。
//以陀螺仪为例，测试方法是：保持陀螺仪不动，统计一段时间内的陀螺仪输出数据。数据会近似正态分布，按3σ原则，取正态分布的(3σ)^2作为r的初始化值。
void	FILTER_Kalman2Init(Kalman2_Str *ptr, float *init_x, float (*init_p)[2])
{
    ptr->x[0]    = init_x[0];
    ptr->x[1]    = init_x[1];
    ptr->p[0][0] = init_p[0][0];
    ptr->p[0][1] = init_p[0][1];
    ptr->p[1][0] = init_p[1][0];
    ptr->p[1][1] = init_p[1][1];
    //ptr->A       = {{1, 0.1}, {0, 1}};
    ptr->A[0][0] = 1;
    ptr->A[0][1] = 0.1;
    ptr->A[1][0] = 0;
    ptr->A[1][1] = 1;
    //ptr->H       = {1,0};
    ptr->H[0]    = 1;
    ptr->H[1]    = 0;
    //ptr->q       = {{10e-6,0}, {0,10e-6}};  // measure noise convariance 
    ptr->q[0]    = 10e-7;
    ptr->q[1]    = 10e-7;
    ptr->r       = 10e-7;  // estimated error convariance 
}

float	FILTER_Kalman2(Kalman2_Str *ptr, float z_measure)
{
    float temp0 = 0.0f;
    float temp1 = 0.0f;
    float temp = 0.0f;

    // Step1: Predict 
    ptr->x[0] = ptr->A[0][0] * ptr->x[0] + ptr->A[0][1] * ptr->x[1];
    ptr->x[1] = ptr->A[1][0] * ptr->x[0] + ptr->A[1][1] * ptr->x[1];
    // p(n|n-1)=A^2*p(n-1|n-1)+q 
    ptr->p[0][0] = ptr->A[0][0] * ptr->p[0][0] + ptr->A[0][1] * ptr->p[1][0] + ptr->q[0];
    ptr->p[0][1] = ptr->A[0][0] * ptr->p[0][1] + ptr->A[1][1] * ptr->p[1][1];
    ptr->p[1][0] = ptr->A[1][0] * ptr->p[0][0] + ptr->A[0][1] * ptr->p[1][0];
    ptr->p[1][1] = ptr->A[1][0] * ptr->p[0][1] + ptr->A[1][1] * ptr->p[1][1] + ptr->q[1];

    // Step2: Measurement 
    // gain = p * H^T * [r + H * p * H^T]^(-1), H^T means transpose. 
    temp0 = ptr->p[0][0] * ptr->H[0] + ptr->p[0][1] * ptr->H[1];
    temp1 = ptr->p[1][0] * ptr->H[0] + ptr->p[1][1] * ptr->H[1];
    temp  = ptr->r + ptr->H[0] * temp0 + ptr->H[1] * temp1;
    ptr->gain[0] = temp0 / temp;
    ptr->gain[1] = temp1 / temp;
    // x(n|n) = x(n|n-1) + gain(n) * [z_measure - H(n)*x(n|n-1)]
    temp = ptr->H[0] * ptr->x[0] + ptr->H[1] * ptr->x[1];
    ptr->x[0] = ptr->x[0] + ptr->gain[0] * (z_measure - temp); 
    ptr->x[1] = ptr->x[1] + ptr->gain[1] * (z_measure - temp);

    // Update @p: p(n|n) = [I - gain * H] * p(n|n-1) 
    ptr->p[0][0] = (1 - ptr->gain[0] * ptr->H[0]) * ptr->p[0][0];
    ptr->p[0][1] = (1 - ptr->gain[0] * ptr->H[1]) * ptr->p[0][1];
    ptr->p[1][0] = (1 - ptr->gain[1] * ptr->H[0]) * ptr->p[1][0];
    ptr->p[1][1] = (1 - ptr->gain[1] * ptr->H[1]) * ptr->p[1][1];

    return ptr->x[0];
}