|
|
static double KalmanFilter_x(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R)
{
double R = MeasureNoise_R;
double Q = ProcessNiose_Q;
static double x_last;
double x_mid = x_last;
double x_now;
static double px_last;
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=px_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);//估計(jì)出的最優(yōu)值
p_now=(1-kg)*p_mid;//最優(yōu)值對(duì)應(yīng)的covariance
px_last = p_now; //更新covariance值
x_last = x_now; //更新系統(tǒng)狀態(tài)值
return x_now;
} |
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
