Basically, I get inputs from aircraft (lon, lat) point to track it. To do this I implemented the openCV Kalman filter algorithm but Kalman filter gives me following result.
lon = 37.461758852005005 => 63 (big difference)
lat = 126.43827617168427 => 18
=======================================
lon = 126.43832445144653 => 84
lat = 37.461774945259094 => 25
=======================================
lon = 126.43832445144653 => 95
lat = 37.461774945259094 => 28
=======================================
lon = 126.43837273120880 => 102
lat = 37.461817860603333 => 30
=======================================
lon = 126.43846929073334 => 107
lat = 37.461855411529541 => 31
=======================================
lon = 126.43851757049561 => 111
lat = 37.461909055709839 => 32
=======================================
lon = 126.43856585025787 => 114
lat = 37.461919784545898 => 33
What I have tried:
struct LonLat
{
double Lat;
double Lon;
LonLat(double lat, double lon )
:Lat(lat), Lon(lon)
{
}
LonLat()
{
Lat = 0.0;
Lon = 0.0;
}
bool operator == (const LonLat& other )
{
return (this->Lat == other.Lat && this->Lon == other.Lon);
}
bool checkFilterRange() const
{
if (cLat > 90 || cLat < -90) return false;
if (cLon > 180 || cLon < -180) return false;
return true;
}
void copy(const LonLat& other)
{
this->Lat = other.Lat;
this->Lon = other.Lon;
}
};
class FilterError
{
public:
FilterError()
{
_errorPoint = false;
_kalman= cv::KalmanFilter(4, 2, 0);
_measurement = cv::Mat_<float>(2, 1);
_measurement.setTo(cv::Scalar(0));
_delta_t = 1 / 20.0;
}
~FilterError()
{}
void filterKalman(LonLat& lonLat)
{
if (!_entered)
{
_kalman.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, _delta_t, 0, 0, 1, 0, _delta_t, 0, 0, 1, 0, 0, 0, 0, 1);
int k = 5;
_kalman.statePre.at<float>(0) = (float)lonLat.Lon;
_kalman.statePre.at<float>(1) = (float)lonLat.Lat;
_kalman.statePre.at<float>(2) = 0;
_kalman.statePre.at<float>(3) = 0;
setIdentity(_kalman.measurementMatrix);
setIdentity(_kalman.processNoiseCov, Scalar::all(1e-4));
setIdentity(_kalman.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(_kalman.errorCovPost, Scalar::all(.1));
_entered = true;
}
Mat prediction = _kalman.predict();
Point predictPt(prediction.at<float>(0), prediction.at<float>(1));
_measurement(0) = (float)lonLat.Lon;
_measurement(1) = (float)lonLat.Lat;
Mat estimated = _kalman.correct(_measurement);
Point statePt(estimated.at<float>(0), estimated.at<float>(1));
lonLat.Lon = statePt.x;
lonLat.Lat = statePt.y;
}
private:
bool _entered;
cv::KalmanFilter _kalman;
cv::Mat_<float> _measurement;
}