Using a Kalman filter to predict samples between measurements

Question

I have a dataset where measurements were taken at 10 Hz, and I am trying to use a Kalman filter to add predicted samples in between the measurements, so that my output is at 100 Hz. I have it working ok when the velocity is linear, but when the direction changes, the filter takes a while to catch up. I am new to Kalman models, so am very likely making some mistakes in my settings. See attached image for an example, the red is measured data, with stepping in between measurements. The blue is the Kalman corrected.

std::vector<double> measurements is a dummy data array I am testing with.

The main Kalman code is based on Github: hmartiro/kalman-cppkalman.cpp

The measurement / estimation loop is based on this

Am I correct in feeding the prediction back into the filter for every loop? Or is this line:

yPos << kf.state().transpose(); wrong?

My code is:

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;
}

Thank you.