卡尔曼滤波器在温度测量抗干扰中的实际应用效果

double CSerialCtrlDemoDlg::KalmanFilter(KalmanInfo* kalmanInfo, double lastMeasurement)
{
	//预测下一时刻的值
	//x的先验估计由上一个时间点的后验估计值和输入信息给出，此处需要根据基站高度做一个修改
	double predictValue = kalmanInfo->A* kalmanInfo->filterValue;   
	
	//求协方差
	kalmanInfo->P = sqrt(kalmanInfo->A * kalmanInfo->A * kalmanInfo->P * kalmanInfo->P + kalmanInfo->Q * kalmanInfo->Q );  //计算先验均方差 p(n|n-1)=A^2*p(n-1|n-1)+q

	double preValue = kalmanInfo->filterValue;  //记录上次实际坐标的值
 
	//计算kalman增益
	kalmanInfo->kalmanGain = sqrt( (kalmanInfo->P * kalmanInfo->P * kalmanInfo->H * kalmanInfo->H) / (kalmanInfo->P * kalmanInfo->P * kalmanInfo->H * kalmanInfo->H + kalmanInfo->R * kalmanInfo->R) );  //Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
	
	//修正结果，即计算滤波值
	kalmanInfo->filterValue = predictValue + (lastMeasurem