卡尔曼滤波kalman的c语言实现，输入输出为double vector

参考网页：http://bbs.elecfans.com/jishu_487667_1_2.html
有所改进

/*
Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
R:测量噪声，R增大，动态响应变慢，收敛稳定性变好
*/
#define KALMAN_Q 0.02
#define KALMAN_R 7.0000
double KalmanFilter(
	const double ResrcData,
	double ProcessNiose_Q,//Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
	double MeasureNoise_R,//R:测量噪声，R增大，动态响应变慢，收敛稳定性变好
	//double InitialPrediction //p==1
	const int iData
	)
{
     
	double R = MeasureNoise_R;
	double Q = ProcessNiose_Q;

	static double x_last = 0;
	if (iData == 0)
	{
     
		x_last = ResrcData;//第一个值赋给x_last，否则处理后的数据从0开始逼近测量值。
	}
	double x_mid = x_last;
	double x_now;

	static double p_last = 0;
	double p_mid;
	double p_now;
	double kg;

	x_mid = x_last;								//x_last=x(k-1|k-1),x_mid=x(k|k-1)
	p_mid = p_last + Q;							//p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
	kg = p_mid / (p_mid + R);					//kg为kalman filter，R为噪声
	x_now = x_mid + kg*(ResrcData - x_mid);		//估计出的最优值
	p_now = (1 - kg)*p_mid;						//最优值对应的covariance       

	p_last = p_now; //更新covariance值
	x_last = x_now; //更新系统状态值

	return x_now;
}

double_vec data_process::algorithm_kalman(const double_vec &dVecIn)
{
     
	double_vec dVecOut;
	int iSize = int(dVecIn.size());
	if (iSize == 0)
	{
     
		return dVecOut;
	}
	dVecOut.resize(iSize);
	for (int i = 0; i < iSize; ++i)
	{
     
		dVecOut[i] = KalmanFilter(dVecIn[i], KALMAN_Q, KALMAN_R, i);
	}
	return dVecOut;
}