|
发表于 2016-6-22 11:39:25
|
显示全部楼层
#define KALMAN_Q 0.0000001
#define KALMAN_R 10.0000
#define KALMAN_P0 2210.0000
double KalmanFilter(const double ResrcData,
double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)
{
double R = MeasureNoise_R;
double Q = ProcessNiose_Q;
static double x_last;
double x_mid = x_last;
double x_now;
static double p_last;
double p_mid ;
double p_now;
double kg;
x_mid=x_last;
p_mid=p_last+Q;
kg=p_mid/(p_mid+R);
x_now=x_mid+kg*(ResrcData-x_mid);
p_now=(1-kg)*p_mid;
p_last = p_now;
x_last = x_now;
return x_now;
}
上面是滤波函数
下面是AD采集值,KXR94.AXIS_X = (unsigned int)KalmanFilter((double)KXR94.AXIS_X,KALMAN_Q,KALMAN_R,KALMAN_P0);
我测出的图像如图,跟你的差距很大,是我哪里应用不当吗
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
|