Kalman filter: add samples between measurements. [closed]
I am trying to set up kalman filtering to add samples between the measured samples. I have this test data:
std::vector < cv::Point3d > data; (the x value is the timestamp)
cv::Point3d one(10, 11, 100);
cv::Point3d two(20, 12, 200);
cv::Point3d three(30, 13, 300);
cv::Point3d four(40, 14, 500);
cv::Point3d five(50, 15, 600);
data.push_back(one);
data.push_back(two);
data.push_back(three);
data.push_back(four);
data.push_back(five);
this is my kalman code: based on: https://stackoverflow.com/questions/2...
int main()
{
KalmanFilter KF(4, 2, 0);
Mat_<double> state(4, 1);
Mat_<double> processNoise(4, 1, CV_64F);
// intialization of KF...
float dt = 1; //time
KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, dt, 0,
0, 1, 0, dt,
0, 0, 1, 0,
0, 0, 0, 1);
KF.processNoiseCov = (cv::Mat_<float>(4, 4) << 0.2, 0, 0.2, 0, 0, 0.2, 0, 0.2, 0, 0, 0.3, 0, 0, 0, 0, 0.3);
Mat_<float> measurement(2, 1); measurement.setTo(Scalar(0));
KF.statePre.at<float>(0) = 0;
KF.statePre.at<float>(1) = 0;
KF.statePre.at<float>(2) = 0;
KF.statePre.at<float>(3) = 0;
setIdentity(KF.measurementMatrix);
setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(KF.errorCovPost, Scalar::all(.1));
std::cout << data.size() << std::endl;
for (int i = 1; i < data.size(); ++i)
{
const cv::Point3d & last = data[i - 1];
const cv::Point3d & current = data[i];
double steps = current.x - last.x;
std::cout << "Time between Points:" << current.x - last.x << endl;
std::cout << "Measurement:" << current << endl;
Mat prediction = KF.predict();
measurement(0) = last.y;
measurement(1) = last.z;
Mat estimated = KF.correct(measurement);
std::cout << "Estimated: " << estimated.t() << endl;
Mat prediction;
for (int j = 0; j < steps; j ++) //between
{
prediction = KF.predict();
std::cout << (long)((last.x - data[0].x) + j) << " " << prediction.at<double>(0) << " " << prediction.at<double>(1) << endl;
}
std::cout << "Prediction: " << prediction.t() << endl << endl;
}
return 0;
}
What i get is:
Time between Points:10
Measurement:[20, 12, 200]
Estimated: [7.3345551, 66.677773, 3.6654449, 33.322227]
0 5.27766e+13 1.14294e+10
1 7.5021e+14 1.14294e+10
2 3.18693e+15 1.14294e+10
3 1.34921e+16 1.14294e+10
4 5.69456e+16 1.14294e+10
5 1.91904e+17 1.14294e+10
6 3.95717e+17 1.14294e+10
7 8.15253e+17 1.14294e+10
8 1.67815e+18 1.14294e+10
9 3.45157e+18 1.14294e+10
Prediction: [43.989002, 399.90012, 3.6654449, 33.322227]
Time between Points:10
Measurement:[30, 13, 300]
Estimated: [12.395308, 202.58578, 0.60824382, 13.324508]
10 2.69206e+16 5.55539e+06
11 4.78165e+16 5.55539e+06
12 8.35837e+16 5.55539e+06
13 1.43592e+17 5.55539e+06
14 2.036e+17 5.55539e+06
15 2.63608e+17 5.55539e+06
16 3.59003e+17 5.55539e+06
17 4.79019e+17 5 ...
First step, you're using a mixture of float and double. You initialized the Kalman Filter as float, so use that for everything. For example, you prediction matrix is in the float format, but you're using .at<double>, so of course you get garbage.
Thank you! that is what I get for copy/pasting some code. :) Now I have readable results, but I think I have some errors in my model somewhere. Using:
I see pretty good results, AS LONG AS the velocity is constant. When there is a direction change, it takes a long time for the filter to right itself. Please see this image. Can I expect to get better than that? What would you suggest as settings in here? Thank you again.
https://s12.postimg.org/b1ao5pdul/268...
Ok, try reversing your process noise and measurement noise, and then 0.5 is a bit high, so try 0.1
The reason is process noise is how much change you expect the true numbers to change, and you have that low. So the filter thinks things should change slowly, so it does.
You also have a high measurement noise, which is how wrong the things you measure are likely to be. So that also makes it slow to change, since the numbers could be a bit lie, not just a small one.