参考网页: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;
}