/*
The first two formulas represent the prediction of the Kalman Filter. 
And since there is no information about the driving forces it is very simple. 
The second three formulas calculate the measurement update. The variables are 
x for the filtered value,  
q for the process noise, 
r for the sensor noise, 
p for the estimated error and 
k for the Kalman Gain.
The state of the filter is defined by the values of these variables.

The filter is applied with each measurement and initialized with the 
process noise q, 
the sensor noise r, 
the initial estimated error p and the initial value x. 
The initial values for p is not very important since it is adjusted during the process.
It must be just high enough to narrow down. The initial value for the readout is also not
very important, since it is updated during the process.
*/
#define alpha_lpf		0.95
float pxg[2]={1000,0};
float pyg[2]={1000,0};
float pzg[2]={1000,0};
float ppitch[2]={1000,0};
float proll[2]={1000,0};
float pyaw[2]={1000,0};

//kalman filter
int gx_kalman(int sinyal)
{	static float K;
	static float tmp;
	static long int x[3];
  pxg[0]=pxg[0]+0.001;//ketelitian
	tmp=1/(pxg[0]+0.01);
	K=pxg[0]*tmp;
	x[1]=x[0]+K*(sinyal-x[0]);
	pxg[1]=(1-K)*pxg[0];
	x[0]=x[1];
	pxg[0]=pxg[1];
	return x[1];
}

int gy_kalman(int sinyal)
{	static float K;
	static float tmp;
	static long int x[3];
  pyg[0]=pyg[0]+0.001;//ketelitian
	tmp=1/(pyg[0]+0.01);
	K=pyg[0]*tmp;
	x[1]=x[0]+K*(sinyal-x[0]);
	pyg[1]=(1-K)*pyg[0];
	x[0]=x[1];
	pyg[0]=pyg[1];
	return x[1];
}

int gz_kalman(int sinyal)
{	static float K;
	static float tmp;
	static long int x[3];
  pzg[0]=pzg[0]+0.001;//ketelitian
	tmp=1/(pzg[0]+0.01);
	K=pzg[0]*tmp;
	x[1]=x[0]+K*(sinyal-x[0]);
	pzg[1]=(1-K)*pzg[0];
	x[0]=x[1];
	pzg[0]=pzg[1];
	return x[1];
}

int pitch_kalman(int sinyal)
{	static float K;
	static float tmp;
	static long int x[3];
  ppitch[0]=ppitch[0]+0.001;//ketelitian
	tmp=1/(ppitch[0]+0.01);
	K=ppitch[0]*tmp;
	x[1]=x[0]+K*(sinyal-x[0]);
	ppitch[1]=(1-K)*ppitch[0];
	x[0]=x[1];
	ppitch[0]=ppitch[1];
	return x[1];
}

int roll_kalman(int sinyal)
{	static float K;
	static float tmp;
	static long int x[3];
  proll[0]=proll[0]+0.001;//ketelitian
	tmp=1/(proll[0]+0.01);
	K=proll[0]*tmp;
	x[1]=x[0]+K*(sinyal-x[0]);
	proll[1]=(1-K)*proll[0];
	x[0]=x[1];
	proll[0]=proll[1];
	return x[1];
}

int yaw_kalman(int sinyal)
{	static float K;
	static float tmp;
	static long int x[3];
  pyaw[0]=pyaw[0]+0.001;//ketelitian
	tmp=1/(pyaw[0]+0.01);
	K=pyaw[0]*tmp;
	x[1]=x[0]+K*(sinyal-x[0]);
	pyaw[1]=(1-K)*pyaw[0];
	x[0]=x[1];
	pyaw[0]=pyaw[1];
	return x[1];
}

//low pass filter
//example from wikipedia y[i] := a * x[i] + (1-a) * y[i-1]
float x_lpf(float accel)
{	static float lpf;
	return lpf = (alpha_lpf*lpf) + (1-alpha_lpf)*(accel);
}
float y_lpf(float accel)
{	static float lpf;
	return lpf = (alpha_lpf*lpf) + (1-alpha_lpf)*(accel);
}
float z_lpf(float accel)
{	static float lpf;
	return lpf = (alpha_lpf*lpf) + (1-alpha_lpf)*(accel);
}
float pitch_lpf(float pitch)
{	static float lpf;
	return lpf = (alpha_lpf*lpf) + (1-alpha_lpf)*(pitch);
}
float roll_lpf(float roll)
{	static float lpf;
	return lpf = (alpha_lpf*lpf) + (1-alpha_lpf)*(roll);
}
float yaw_lpf(float yaw)
{	static float lpf;
	return lpf = (alpha_lpf*lpf) + (1-alpha_lpf)*(yaw);
}
//high pass filter
//example from wikipedia y[i] := a * y[i-1] + a * (x[i] - x[i-1])
float x_hpf(float gyro)
{	static float lpf, last_lpf;
	lpf = alpha_lpf*gyro + alpha_lpf*(lpf - last_lpf);
	last_lpf = lpf;
	return lpf;
}
float y_hpf(float gyro)
{	static float lpf, last_lpf;
	lpf = alpha_lpf*gyro + alpha_lpf*(lpf - last_lpf);
	last_lpf = lpf;
	return lpf;
}
float z_hpf(float gyro)
{	static float lpf, last_lpf;
	lpf = alpha_lpf*gyro + alpha_lpf*(lpf - last_lpf);
	last_lpf = lpf;
	return lpf;
}