Skip to content

Instantly share code, notes, and snippets.

@mizutori

mizutori/Kalman Filter

Created March 5, 2017 00:39
Show Gist options
  • Save mizutori/df3eb1a30c9e1f5d07270188c06bdecd to your computer and use it in GitHub Desktop.
Save mizutori/df3eb1a30c9e1f5d07270188c06bdecd to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. mizutori created this gist Mar 5, 2017.
    34 changes: 34 additions & 0 deletions Kalman Filter
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,34 @@
    /* Kalman Filter */
    float Qvalue;

    long locationTimeInMillis = (long)(location.getElapsedRealtimeNanos() / 1000000);
    long elapsedTimeInMillis = locationTimeInMillis - runStartTimeInMillis;

    if(currentSpeed == 0.0f){
    Qvalue = 3.0f; //3 meters per second
    }else{
    Qvalue = currentSpeed; // meters per second
    }

    kalmanFilter.Process(location.getLatitude(), location.getLongitude(), location.getAccuracy(), elapsedTimeInMillis, Qvalue);
    double predictedLat = kalmanFilter.get_lat();
    double predictedLng = kalmanFilter.get_lng();

    Location predictedLocation = new Location("");//provider name is unecessary
    predictedLocation.setLatitude(predictedLat);//your coords of course
    predictedLocation.setLongitude(predictedLng);
    float predictedDeltaInMeters = predictedLocation.distanceTo(location);

    if(predictedDeltaInMeters > 60){
    Log.d(TAG, "Kalman Filter detects mal GPS, we should probably remove this from track");
    kalmanFilter.consecutiveRejectCount += 1;

    if(kalmanFilter.consecutiveRejectCount > 3){
    kalmanFilter = new KalmanLatLong(3); //reset Kalman Filter if it rejects more than 3 times in raw.
    }

    kalmanNGLocationList.add(location);
    return false;
    }else{
    kalmanFilter.consecutiveRejectCount = 0;
    }