prediction help?

[antithing]

Hi, and thank you for making this code available. I have a dataset where measurements were taken at 1hz, and I am trying to use your filter to add predicted sample in between the measurements, so that my output is at 10hz. If you have a moment, could you point me at the best settings to use for this? I have it working ok when the velocity is linear, but the motion is not consistent, and on direction changes, the output looks bad... .I have:

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

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

  double dt = 1.0/30; // Time step

  Eigen::MatrixXd A(n, n); // System dynamics matrix
  Eigen::MatrixXd C(m, n); // Output matrix
  Eigen::MatrixXd Q(n, n); // Process noise covariance
  Eigen::MatrixXd R(m, m); // Measurement noise covariance
  Eigen::MatrixXd P(n, n); // Estimate error covariance

  // Discrete LTI projectile motion, measuring position only
  A << 1, dt, 0, 0, 1, dt, 0, 0, 1;
  C << 1, 0, 0;

  // Reasonable covariance matrices
  Q << .05, .05, .0, .05, .05, .0, .0, .0, .0;
  R << 3; //5
  P << 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01;
  // Construct the filter
  KalmanFilter kf(dt,A, C, Q, R, P);

  // List of noisy position measurements (y)
  std::vector<double> measurements = {
     10,11,20,13,14,15,16,17,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);

  std::ofstream savefile;
  savefile.open("dataHigh.csv");
  std::ofstream savefile2;
  savefile2.open("dataLow.csv");

  for(int i = 0; i < measurements.size(); i++) {

      y << measurements[i]; // REAL MEASUREMENT

      kf.update(y);

      for (int ji = 0; ji < 10; ji++)  //TEN PREDICTED SAMPLES BETWEEN
      {
          t += dt; 
         kf.update(y);
          std::cout << "t = " << t << ", " << "y[" << i << "] = " << y.x()
              << ", x_hat[" << i << "] = " << kf.state().x() << std::endl;

          //save
          savefile << kf.state().x();
          savefile << "\n";
          savefile2 << measurements[i];
          savefile2 << "\n";

          y << kf.state().transpose(); //COPY SAMPLE TO Y

      }
  }

  return 0;
}

thank you again!

[antithing]

Please see attached image for a better explanation. The measurement, and the prediction overlaid. Thanks!