3
$\begingroup$

I have a dataset where measurements were taken at 1 Hz, and I am trying to use a Kalman filter to add predicted samples in between the measurements, so that my output is at 10 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. What can I do to improve this? 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

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;
}
$\endgroup$
  • $\begingroup$ This is probably a better fit for code review, but before this is closed, please make sure your variable names are bigger than a single character. $\endgroup$ – tuskiomi Jun 7 '17 at 15:32
  • $\begingroup$ Welcome to Robotics anti, but I'm afraid that questions asking for code improvement suggestions are off-topic because there are many ways to solve any given coding problem. We prefer practical, answerable questions based on actual problems that you face, so please try to include what you want to achieve, what you tried, what you saw & what you expected to see. Please take a look at How to Ask, tour and the Robotics question checklist for advice on writing a good question. $\endgroup$ – Mark Booth Jun 8 '17 at 15:18
1
$\begingroup$

This is what I believe is happening from a 1 min glance over your code.

You are passing true measurement data to your KF at 3 hz. Between the actual measurements you are passing the new state estimates to your KF as if they were measurements. This is incorrect. What you should be doing is calling the prediction update. What you are doing now will artificially drive down the covariance of your KF because it thinks it is getting new measurements. This would explain why the filter responds slowly to actual measurements.

These are the lines from the git page that you want to run each time you want a new state estimate(without a new measurement!!!)

x_hat_new = A * x_hat; P = A*P*A.transpose() + Q;

$\endgroup$
  • $\begingroup$ Thank you! I have tried adding this function to the kalman : Eigen::VectorXd KalmanFilter::predict(Eigen::VectorXd& y) { this->x_hat_new = this->A * this->x_hat; this->P = this->A*this->P*this->A.transpose() + this->Q; return this->x_hat_new; } and changed the loop to: Eigen::VectorXd tmp = kf.predict(y); y << tmp; but this gives me the same result as the unfiltered data. What am I missing? thanks again $\endgroup$ – anti Jun 8 '17 at 9:32
1
$\begingroup$

Your dT is 1/30 for some reason, but you said you had 1 Hz (1/1) and wanted 10 Hz (1/10).

But to your question, I would propagate the "filler" samples by basing off the last Kalman estimate, then resetting the filler states to the Kalman states whenever the Kalman filter updates. Your path will be a little jerky, but right now you're treating future estimates as measurements, so your predicted states and measured states are identical, which means it's going to screw with your filter's covariance estimates.

I would only update the filter when you have a real measurement.

So, for example, right now you have something like:

fastTs = 1/10; % High speed sample rate
while (true)
    if (currentMeasurementExists)
        KalmanStates = KalmanFilter(KalmanStates, Measurement, fastTs);
    else
        KalmanStates = KalmanFilter(KalmanStates, FillerMeasurement, fastTs);
    end
    FillerMeasurement = C*KalmanStates; % C is the output matrix that converts states to measurements
end

What I would suggest instead is:

fastTs = 1/10;
slowTs = 1/1;
fastA = A(fastTs); % Sorry, no easy way to show this because dT is tucked away
slowA = A(slowTs); % in your 'A' matrix, but you need to use the correct sample times in the correct locations.
while (true)
    if (currentMeasurementExists)
        KalmanStates = KalmanFilter(KalmanStates, Measurement, slowTs);
        FillerStates = KalmanStates;
    else
        FillerStates = fastA*FillerStates; % Remember to use fastTs for filler, slowTs for real measurements. 
    end
    FillerMeasurements = C*FillerStates;
end

So to reiterate again, you are treating filler samples as real measurements and updating the Kalman filter accordingly, which is updating your Kalman states (and updating Kalman covariances to expect better samples).

What I think you should do instead is to split the Kalman filter off to do only filtering on only real measurements. You know how your system should progress - this is the A matrix. So, evolve the system from the last known "good" state (last Kalman update) using the A matrix. When you get a new "good" state (new Kalman update), then reset the filler states to the Kalman states and continue propagating.

I would bet the biggest performance hit you're taking on this is that you're screwing up the covariance estimates, because all of your "measurements" are exactly what the filter would predict.

$\endgroup$
  • $\begingroup$ Thank you! I am trying to wrap my brain around this. Where do i get C matrix from? $\endgroup$ – anti Jun 8 '17 at 9:24
  • $\begingroup$ I have tried to implement your help, but am a bit confused. If you have a minute, can you take a look? pastebin.com/H7bA53BD $\endgroup$ – anti Jun 8 '17 at 10:11
  • $\begingroup$ @anti - the code looks fine. You're asking where you get the C matrix from, but you define it in your code as matC. It's usually just a diagonal matrix of ones, which would mean your measurements are your states. $\endgroup$ – Chuck Jun 9 '17 at 4:00
  • $\begingroup$ thanks again, when I print the FillerState value after this line: FillerState = fastA*FillerState;, the value does not change until there is a new sample. (it seems exactly the same as kf->state()) Is this a problem with my FastA matrix? $\endgroup$ – anti Jun 9 '17 at 7:56

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