数据滤波--一阶数值滤波与一阶卡尔曼滤波

数据滤波

1 一阶数值滤波

data_filtr = 0;
data_detect =0;

data_filtr = data_filtr*a + (1-a)*data_detect;

2 一阶卡尔曼滤波

/**
 * @brief One dimensional kalman filter, simple assume that velocity is constant.
 * 
 * @param pre_x 
 * @param pre_p 
 * @param z 
 * @param cur_x 
 * @param cur_p 
 */
void kalmanFilter(const double pre_x, const double pre_p, const double z, double &cur_x, double cur_p)
{
  double tmp_x = pre_x;
  double tmp_p = pre_p + param_Q;
  double y = z - tmp_x;
  double K = tmp_p / (tmp_p + param_R);
  cur_x = tmp_x + K * y;
  cur_p = (1 - K) * tmp_p;
}