Question

我有一个数据集，其中测量是以10 Hz进行的，我正在尝试使用卡尔曼滤波器在测量之间添加预测样本，以便我的输出为100 Hz。当速度是线性时，我可以正常工作，但是当方向改变时，滤波器需要一段时间才能赶上。我是卡尔曼模特的新手，所以很可能在我的设置中犯了一些错误。有关示例，请参阅attached image，红色是测量数据，介于两次测量之间。蓝色是卡尔曼纠正的。

std::vector<double> measurements是我正在测试的虚拟数据数组。

主卡尔曼代码基于Github: hmartiro/kalman-cppkalman.cpp

测量/估算循环基于this

我是否正确将预测反馈到每个循环的过滤器中？或者是这一行：

yPos << kf.state().transpose();错了？

我的代码是：

int main(int argc, char* argv[]) {

  int n = 3; // Number of states
  int m = 1; // Number of measurements

  double dt = 1.0/10; // Time step

  Eigen::MatrixXd matA(n, n); // System dynamics matrix
  Eigen::MatrixXd matC(m, n); // Output matrix
  Eigen::MatrixXd matQ(n, n); // Process noise covariance
  Eigen::MatrixXd matR(m, m); // Measurement noise covariance
  Eigen::MatrixXd matP(n, n); // Estimate error covariance

  // Discrete motion, measuring position only
  matA << 1, dt, 0, 0, 1, dt, 0, 0, 1;
  matC << 1, 0, 0;

  // Reasonable covariance matrices
  matQ << 0.001, 0.001, .0, 0.001, 0.001, .0, .0, .0, .0;
  matR << 0.03;
  matP << .1, .1, .1, .1, 10000, 10, .1, 10, 100;

  // Construct the filter
  KalmanFilter kf(dt,matA, matC, matQ, matR, matP);

  // List of noisy position measurements (yPos)
  std::vector<double> measurements = {
     10,11,13,13.5,14,15.2,15.6,16,18,22,20,21,19,18,17,16,17.5,19,21,22,23,25,26,25,24,21,20,18,16
  };

  // Best guess of initial states
  Eigen::VectorXd x0(n);
  x0 << measurements[0], 0, 0;
  kf.init(dt,x0);

  // Feed measurements into filter, output estimated states
  double t = 0;
  Eigen::VectorXd y(m);


  for(int i = 0; i < measurements.size(); i++) { //ACTUAL MEASURED SAMPLE

      yPos << measurements[i];

      kf.update(yPos);

      for (int ji = 0; ji < 10; ji++)  // TEN PREDICTED SAMPLES
      {
          t += dt;       

         kf.update(yPos);


          yPos << kf.state().transpose(); //USE PREDICTION AS NEW SAMPLE

      }
  }

  return 0;
}

谢谢。

使用卡尔曼滤波器来预测测量之间的样本

0 个答案: