1
$\begingroup$

(cross posted from stack overflow) 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 image

for an example, the red is measured data, with stepping in between measurements. The blue is the Kalman corrected.

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

The main Kalman code is based on this:

https://github.com/hmartiro/kalman-cpp/blob/master/kalman.cpp

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/30; // 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.

$\endgroup$
0
$\begingroup$

I'm sorry that it was not very clear to me what you were asking. But I think I have developed some sense on it as follows:

You are physically capturing 10 noisy sensor measurements per second, which I call it as the physical measurements. But for some reasons, you want to fill in between those true-physical data samples with new synthetic data, generated mathematically at a rate of 100 samples per second (10 times more dense than available with the physical sensor measurement)

The solution to this problem has different names, based on your interpretation and which method you choose to follow, such as a sampler, a predictor, an estimator, a curve fitting or an interpolation...

It seems, you want to use a Kalman filter (an estimator actually) to generate those new synthetic samples in between the true physical measurements. And your question is whether you should feed those synthetically generated new sampes back into the Kalman filter (for residual calculations) as if the data were generated from a true physical sensor, so that the filter can use it in residual calculations to get rid of the noise present in it and refine the state estimates?

That's what I understand from your question set up and my answer to it is no you cannot. Those new data cannot replace true phsyical measurements. They do not involve new information that's called as the innovations process of a random process (that you're trying to estimate), hence they cannot be used to make refined computations or decisions alone.

In fact, the Kalman filter will only propagate the current state $X(t)$ into the next phsyical measurement point $X(t+T_s)$ by using the system dynamic matrix alone. (i.e., it will solve the differential equation numerically based on the initial condition provided from the previous state estimate, and sample this solution at the requested high data rate)

$\endgroup$
  • $\begingroup$ Thank you! So perhaps a kalman is not in fact the best approach here? In the interests of clarity, this image: imghost.io/image/dMTQp explains a bit more. My data streams in real time, as in the red plotted line. I need to get as close to the blue line as I can, to smooth the animation. i am trying to use a position + velocity * timestep model at the moment, but am searching for a smarter, more predictive way. thanks again. $\endgroup$ – anti Jun 10 '17 at 12:09
  • 1
    $\begingroup$ yes you can use the Kalman filter perfectly to achive it. It will propagate the state from one red data point to the next as much as it can using the system dynamic equation alone. On the other hand, the blue line seems pretty much a linear one? So you have bunch of other methods to approximate it. The simplest being a linear interpolator. How much accuracy do you need? A Kalman filter estimator could be an overkill, but if you prefer the best accuracy go with the Kalman filter. $\endgroup$ – Fat32 Jun 10 '17 at 12:37
  • 1
    $\begingroup$ However note that for a real-time operation and with nonstationary dynamical system (system with random (unknown) acceleration) the Kalman estimator needs some time to converge. Things go complicated and you must make more detailed analysis of the situation. When you have a noisy measurement of nonstationary object, there is no (imho) perfect solution to it known. $\endgroup$ – Fat32 Jun 10 '17 at 12:46
  • $\begingroup$ ok, thanks again. I need all the accuracy i can get, so will keep fighting to wrap my head around the kalman. What other potential methods could be useful here, if i cannot solve it? The motion will be random, in acceleration and velocity (it is a handheld sensor) . $\endgroup$ – anti Jun 10 '17 at 12:56
  • 1
    $\begingroup$ hmm complicated problem. a Kalman filter will definetely help but the only true solution is to increase the phsiycal sampling rate. $\endgroup$ – Fat32 Jun 10 '17 at 13:17

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.