prediction help? #3
Description
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!