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/25702120/how-to-use-a-kalman-filter-to-predict-gps-positions-in-between-meassurements?rq=1
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.55539e+06
18 6.2161e+17 5.55539e+06
19 8.61643e+17 5.55539e+06
Prediction: [18.477747, 335.83078, 0.60824382, 13.324508]
Time between Points:10
Measurement:[40, 14, 500]
Estimated: [13.987329, 307.97446, 0.29939148, 10.829972]
20 5.65692e+17 959433
21 7.50019e+17 959433
22 9.45115e+17 959433
23 1.14021e+18 959433
24 1.51769e+18 959433
25 1.90788e+18 959433
26 2.29807e+18 959433
27 3.07069e+18 959433
28 3.85107e+18 959433
29 4.65122e+18 959433
Prediction: [16.981243, 416.27432, 0.29939148, 10.829972]
Time between Points:10
Measurement:[50, 15, 600]
Estimated: [14.705034, 484.33414, 0.1413258, 14.342192]
30 2.92129e+19 1.12591e+07
31 3.74806e+19 1.12591e+07
32 4.57483e+19 1.12591e+07
33 5.4016e+19 1.12591e+07
34 6.22837e+19 1.12591e+07
35 7.05514e+19 1.12591e+07
36 8.38512e+19 1.12591e+07
37 1.00387e+20 1.12591e+07
38 1.16922e+20 1.12591e+07
39 1.33457e+20 1.12591e+07
Prediction: [16.118294, 627.7558, 0.1413258, 14.342192]
These results make no sense to me, the steps should go up by one, and the prediction is way off.. Where am i going wrong here? And what exactly are the values in the prediction mat?
Thank you!