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 + (lastMeasurement - predictValue) * kalmanInfo->kalmanGain; //利用残余的信息改善对x(t)的估计,给出后验估计,这个值也就是输出 X(k|k)= X
卡尔曼滤波器在温度测量抗干扰中的实际应用效果
最新推荐文章于 2022-04-29 12:33:06 发布