'Kalman filter for longitude and latitude

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

Also I displayed my result on the chart. First is original in the second Kalman filter was used.

1) Original

2) Kalaman filter was used

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;
} 


Sources

This article follows the attribution requirements of Stack Overflow and is licensed under CC BY-SA 3.0.

Source: Stack Overflow

Solution Source