一维数据的kalman滤波

//一维数据kalman滤波，假设过程转换和观测转换均为1，且过程控制输入为0
class KalmanFilter
{
	KalmanFilter()
	{
		prevData = 0;//存放上一时刻的后验估计值
		P = 10;//存放上一时刻的后验估计协方差
		Q = 0.0001;//存放过程噪声协方差
		R = 0.005;//存放观测噪声协方差
	}

	/*
	prevData:存放上一时刻的后验估计值
	P:存放上一时刻的后验估计协方差
	Q:存放过程噪声协方差
	R:存放观测噪声协方差
	*/
	void Init(double prevData, double P,double Q,double R)
	{
		this->prevData = prevData;
		this->P = P;
		this->Q = Q;
		this->R = R;
	}
	
	/*inData:观测值*/
	double filter(double inData/*观测值*/)
	{
		//下面的变量仅参与中间计算
		double Kg;//卡尔曼增益
		double currData;//当前时刻的先验估计值
		double currP;//当前时刻的先验估计协方差

		//预测
		currData = prevData;//根据上一时刻的后验估计预测出当前时刻的先验估计
		currP = P + Q;//根据上一时刻的后验估计协方差预测出当前时刻的先验估计协方差

		//更新
		Kg = currP / (currP + R);
		prevData = currData + Kg*(inData - currData);
		P = (1 - Kg)*currP;

		return prevData;
	}

private:
	double prevData;//存放上一时刻的后验估计值
	double P;//存放上一时刻的后验估计协方差
	double Q;//存放过程噪声协方差
	double R;//存放观测噪声协方差
};