diff --git a/src/main/java/com/kalmanFilters/Tracker1D.java b/src/main/java/com/kalmanFilters/Tracker1D.java new file mode 100644 index 00000000..26bb37ba --- /dev/null +++ b/src/main/java/com/kalmanFilters/Tracker1D.java @@ -0,0 +1,158 @@ +package com.kalmanFilters; + + +/** + * Kalman filter tracking in one dimension. + */ +class Tracker1D { + + // Settings + + /** + * Time step + */ + private final double mt, mt2, mt2d2, mt3d2, mt4d4; + + /** + * Process noise covariance + */ + private final double mQa, mQb, mQc, mQd; + + /** + * Estimated state + */ + private double mXa, mXb; + + /** + * Estimated covariance + */ + private double mPa, mPb, mPc, mPd; + + /** + * Creates a tracker. + * + * @param timeStep Delta time between predictions. Usefull to calculate speed. + * @param processNoise Standard deviation to calculate noise covariance from. + */ + public Tracker1D(double timeStep, double processNoise) { + + // Lookup time step + mt = timeStep; + mt2 = mt * mt; + mt2d2 = mt2 / 2.0; + mt3d2 = mt2 * mt / 2.0; + mt4d4 = mt2 * mt2 / 4.0; + + // Process noise covariance + double n2 = processNoise * processNoise; + mQa = n2 * mt4d4; + mQb = n2 * mt3d2; + mQc = mQb; + mQd = n2 * mt2; + + // Estimated covariance + mPa = mQa; + mPb = mQb; + mPc = mQc; + mPd = mQd; + } + + /** + * Reset the filter to the given state. + *

+ * Should be called after creation, unless position and velocity are assumed to be both zero. + * + * @param position + * @param velocity + * @param noise + */ + public void setState(double position, double velocity, double noise) { + + // State vector + mXa = position; + mXb = velocity; + + // Covariance + double n2 = noise * noise; + mPa = n2 * mt4d4; + mPb = n2 * mt3d2; + mPc = mPb; + mPd = n2 * mt2; + } + + /** + * Update (correct) with the given measurement. + * + * @param position + * @param noise + */ + public void update(double position, double noise) { + + double r = noise * noise; + + // y = z - H . x + double y = position - mXa; + + // S = H.P.H' + R + double s = mPa + r; + double si = 1.0 / s; + + // K = P.H'.S^(-1) + double Ka = mPa * si; + double Kb = mPc * si; + + // x = x + K.y + mXa = mXa + Ka * y; + mXb = mXb + Kb * y; + + // P = P - K.(H.P) + double Pa = mPa - Ka * mPa; + double Pb = mPb - Ka * mPb; + double Pc = mPc - Kb * mPa; + double Pd = mPd - Kb * mPb; + + mPa = Pa; + mPb = Pb; + mPc = Pc; + mPd = Pd; + } + + /** + * Predict state. + * + * @param acceleration Should be 0 unless there's some sort of control input (a gas pedal, for instance). + */ + public void predict(double acceleration) { + + // x = F.x + G.u + mXa = mXa + mXb * mt + acceleration * mt2d2; + mXb = mXb + acceleration * mt; + + // P = F.P.F' + Q + double Pdt = mPd * mt; + double FPFtb = mPb + Pdt; + double FPFta = mPa + mt * (mPc + FPFtb); + double FPFtc = mPc + Pdt; + double FPFtd = mPd; + + mPa = FPFta + mQa; + mPb = FPFtb + mQb; + mPc = FPFtc + mQc; + mPd = FPFtd + mQd; + } + + /** + * @return Estimated position. + */ + public double getPosition() { return mXa; } + + /** + * @return Estimated velocity. + */ + public double getVelocity() { return mXb; } + + /** + * @return Accuracy + */ + public double getAccuracy() { return Math.sqrt(mPd / mt2); } +} \ No newline at end of file diff --git a/src/main/java/com/marianhello/bgloc/Config.java b/src/main/java/com/marianhello/bgloc/Config.java index db809d0d..39e0beba 100644 --- a/src/main/java/com/marianhello/bgloc/Config.java +++ b/src/main/java/com/marianhello/bgloc/Config.java @@ -63,6 +63,7 @@ public class Config implements Parcelable private HashMap httpHeaders; private Integer maxLocations; private LocationTemplate template; + private Boolean applyKalmanFilter; public Config () { } @@ -95,6 +96,7 @@ public Config(Config config) { if (config.template instanceof AbstractLocationTemplate) { this.template = ((AbstractLocationTemplate)config.template).clone(); } + this.applyKalmanFilter = config.applyKalmanFilter; } private Config(Parcel in) { @@ -123,6 +125,7 @@ private Config(Parcel in) { Bundle bundle = in.readBundle(); setHttpHeaders((HashMap) bundle.getSerializable("httpHeaders")); setTemplate((LocationTemplate) bundle.getSerializable(AbstractLocationTemplate.BUNDLE_KEY)); + setApplyKalmanFilter((Boolean) in.readValue(null)); } public static Config getDefault() { @@ -151,6 +154,7 @@ public static Config getDefault() { config.httpHeaders = null; config.maxLocations = 10000; config.template = null; + config.applyKalmanFilter = true; return config; } @@ -183,6 +187,7 @@ public void writeToParcel(Parcel out, int flags) { out.writeString(getSyncUrl()); out.writeInt(getSyncThreshold()); out.writeInt(getMaxLocations()); + out.writeValue(getApplyKalmanFilter()); Bundle bundle = new Bundle(); bundle.putSerializable("httpHeaders", getHttpHeaders()); bundle.putSerializable(AbstractLocationTemplate.BUNDLE_KEY, (AbstractLocationTemplate) getTemplate()); @@ -520,6 +525,15 @@ public void setTemplate(LocationTemplate template) { this.template = template; } + public boolean hasApplyKalmanFilter() { + return applyKalmanFilter != null; + } + + public void setApplyKalmanFilter(Boolean applyKalmanFilter) { this.applyKalmanFilter = applyKalmanFilter; } + + public Boolean getApplyKalmanFilter() { return applyKalmanFilter; } + + @Override public String toString () { return new StringBuffer() @@ -547,6 +561,7 @@ public String toString () { .append(" httpHeaders=").append(getHttpHeaders().toString()) .append(" maxLocations=").append(getMaxLocations()) .append(" postTemplate=").append(hasTemplate() ? getTemplate().toString() : null) + .append(" applyKalmanFilter=").append(getApplyKalmanFilter()) .append("]") .toString(); } @@ -639,6 +654,9 @@ public static Config merge(Config config1, Config config2) { if (config2.hasTemplate()) { merger.setTemplate(config2.getTemplate()); } + if (config2.hasApplyKalmanFilter()) { + merger.setApplyKalmanFilter(config2.getApplyKalmanFilter()); + } return merger; } diff --git a/src/main/java/com/marianhello/bgloc/data/sqlite/SQLiteConfigurationContract.java b/src/main/java/com/marianhello/bgloc/data/sqlite/SQLiteConfigurationContract.java index e162a726..2d0c07ee 100644 --- a/src/main/java/com/marianhello/bgloc/data/sqlite/SQLiteConfigurationContract.java +++ b/src/main/java/com/marianhello/bgloc/data/sqlite/SQLiteConfigurationContract.java @@ -40,6 +40,7 @@ public static abstract class ConfigurationEntry implements BaseColumns { public static final String COLUMN_NAME_HEADERS = "http_headers"; public static final String COLUMN_NAME_MAX_LOCATIONS = "max_locations"; public static final String COLUMN_NAME_TEMPLATE = "template"; + public static final String COLUMN_NAME_APPLY_KALMAN_FILTER = "applyKalmanFilter"; public static final String SQL_CREATE_CONFIG_TABLE = "CREATE TABLE " + ConfigurationEntry.TABLE_NAME + " (" + @@ -67,7 +68,8 @@ public static abstract class ConfigurationEntry implements BaseColumns { ConfigurationEntry.COLUMN_NAME_SYNC_THRESHOLD + INTEGER_TYPE + COMMA_SEP + ConfigurationEntry.COLUMN_NAME_HEADERS + TEXT_TYPE + COMMA_SEP + ConfigurationEntry.COLUMN_NAME_MAX_LOCATIONS + INTEGER_TYPE + COMMA_SEP + - ConfigurationEntry.COLUMN_NAME_TEMPLATE + TEXT_TYPE + + ConfigurationEntry.COLUMN_NAME_TEMPLATE + TEXT_TYPE + COMMA_SEP + + ConfigurationEntry.COLUMN_NAME_APPLY_KALMAN_FILTER + INTEGER_TYPE + " )"; public static final String SQL_DROP_CONFIG_TABLE = diff --git a/src/main/java/com/marianhello/bgloc/data/sqlite/SQLiteConfigurationDAO.java b/src/main/java/com/marianhello/bgloc/data/sqlite/SQLiteConfigurationDAO.java index ff04df1a..09c90d86 100644 --- a/src/main/java/com/marianhello/bgloc/data/sqlite/SQLiteConfigurationDAO.java +++ b/src/main/java/com/marianhello/bgloc/data/sqlite/SQLiteConfigurationDAO.java @@ -56,7 +56,8 @@ public Config retrieveConfiguration() throws JSONException { ConfigurationEntry.COLUMN_NAME_SYNC_THRESHOLD, ConfigurationEntry.COLUMN_NAME_HEADERS, ConfigurationEntry.COLUMN_NAME_MAX_LOCATIONS, - ConfigurationEntry.COLUMN_NAME_TEMPLATE + ConfigurationEntry.COLUMN_NAME_TEMPLATE, + ConfigurationEntry.COLUMN_NAME_APPLY_KALMAN_FILTER }; String whereClause = null; @@ -123,7 +124,7 @@ private Config hydrate(Cursor c) throws JSONException { config.setHttpHeaders(new JSONObject(c.getString(c.getColumnIndex(ConfigurationEntry.COLUMN_NAME_HEADERS)))); config.setMaxLocations(c.getInt(c.getColumnIndex(ConfigurationEntry.COLUMN_NAME_MAX_LOCATIONS))); config.setTemplate(LocationTemplateFactory.fromJSONString(c.getString(c.getColumnIndex(ConfigurationEntry.COLUMN_NAME_TEMPLATE)))); - + config.setApplyKalmanFilter( (c.getInt(c.getColumnIndex(ConfigurationEntry.COLUMN_NAME_APPLY_KALMAN_FILTER)) == 1) ? true : false ); return config; } @@ -154,6 +155,7 @@ private ContentValues getContentValues(Config config) throws NullPointerExceptio values.put(ConfigurationEntry.COLUMN_NAME_HEADERS, new JSONObject(config.getHttpHeaders()).toString()); values.put(ConfigurationEntry.COLUMN_NAME_MAX_LOCATIONS, config.getMaxLocations()); values.put(ConfigurationEntry.COLUMN_NAME_TEMPLATE, config.hasTemplate() ? config.getTemplate().toString() : null); + values.put(ConfigurationEntry.COLUMN_NAME_APPLY_KALMAN_FILTER, (config.getApplyKalmanFilter() == true) ? 1 : 0); return values; } diff --git a/src/main/java/com/marianhello/bgloc/data/sqlite/SQLiteOpenHelper.java b/src/main/java/com/marianhello/bgloc/data/sqlite/SQLiteOpenHelper.java index 95b16017..272ccc28 100644 --- a/src/main/java/com/marianhello/bgloc/data/sqlite/SQLiteOpenHelper.java +++ b/src/main/java/com/marianhello/bgloc/data/sqlite/SQLiteOpenHelper.java @@ -82,7 +82,7 @@ public void onUpgrade(SQLiteDatabase db, int oldVersion, int newVersion) { alterSql.add(SQL_CREATE_CONFIG_TABLE); case 11: alterSql.add("ALTER TABLE " + LocationEntry.TABLE_NAME + - " ADD COLUMN " + LocationEntry.COLUMN_NAME_RADIUS + REAL_TYPE); + " ADD COLUMN " + LocationEntry.COLUMN_NAME_STOP_TERMINATE + REAL_TYPE); alterSql.add("ALTER TABLE " + LocationEntry.TABLE_NAME + " ADD COLUMN " + LocationEntry.COLUMN_NAME_HAS_ACCURACY + INTEGER_TYPE); alterSql.add("ALTER TABLE " + LocationEntry.TABLE_NAME + @@ -140,4 +140,4 @@ public void execAndLogSql(SQLiteDatabase db, String sql) { Log.e(TAG, "Error executing sql: " + e.getMessage()); } } -} +} \ No newline at end of file diff --git a/src/main/java/com/marianhello/bgloc/provider/AbstractLocationProvider.java b/src/main/java/com/marianhello/bgloc/provider/AbstractLocationProvider.java index 8b6494ae..fed45c99 100644 --- a/src/main/java/com/marianhello/bgloc/provider/AbstractLocationProvider.java +++ b/src/main/java/com/marianhello/bgloc/provider/AbstractLocationProvider.java @@ -26,6 +26,7 @@ import com.marianhello.bgloc.data.BackgroundLocation; import com.marianhello.logging.LoggerManager; import com.marianhello.utils.Tone; +import com.kalmanFilters.Tracker1D; /** * AbstractLocationProvider @@ -41,6 +42,15 @@ public abstract class AbstractLocationProvider implements LocationProvider { private ProviderDelegate mDelegate; + private Tracker1D mLatitudeTracker, mLongitudeTracker, mAltitudeTracker; + private boolean mPredicted; + private static final double DEG_TO_METER = 111225.0; + private static final double METER_TO_DEG = 1.0 / DEG_TO_METER; + private static final double TIME_STEP = 1.0; + private static final double COORDINATE_NOISE = 4.0 * METER_TO_DEG; + private static final double ALTITUDE_NOISE = 10.0; + + protected AbstractLocationProvider(Context context) { mContext = context; logger = LoggerManager.getLogger(getClass()); @@ -169,4 +179,93 @@ protected void playDebugTone (int name) { toneGenerator.startTone(name, duration); } + + /** + * Apply Kalman filter to location as recorder by provider + * @param location + */ + protected Location applyKalmanFilter(Location location) { + final double accuracy = location.getAccuracy(); + double position, noise; + + // Latitude + position = location.getLatitude(); + noise = accuracy * METER_TO_DEG; + if (mLatitudeTracker == null) { + mLatitudeTracker = new Tracker1D(TIME_STEP, COORDINATE_NOISE); + mLatitudeTracker.setState(position, 0.0, noise); + } + + if (!mPredicted) + mLatitudeTracker.predict(0.0); + + mLatitudeTracker.update(position, noise); + + // Longitude + position = location.getLongitude(); + noise = accuracy * Math.cos(Math.toRadians(location.getLatitude())) * METER_TO_DEG ; + + if (mLongitudeTracker == null) { + + mLongitudeTracker = new Tracker1D(TIME_STEP, COORDINATE_NOISE); + mLongitudeTracker.setState(position, 0.0, noise); + } + + if (!mPredicted) + mLongitudeTracker.predict(0.0); + + mLongitudeTracker.update(position, noise); + + // Altitude + if (location.hasAltitude()) { + + position = location.getAltitude(); + noise = accuracy; + + if (mAltitudeTracker == null) { + + mAltitudeTracker = new Tracker1D(TIME_STEP, ALTITUDE_NOISE); + mAltitudeTracker.setState(position, 0.0, noise); + } + + if (!mPredicted) + mAltitudeTracker.predict(0.0); + + mAltitudeTracker.update(position, noise); + } + + // Reset predicted flag + mPredicted = false; + + // Latitude + mLatitudeTracker.predict(0.0); + location.setLatitude(mLatitudeTracker.getPosition()); + + // Longitude + mLongitudeTracker.predict(0.0); + location.setLongitude(mLongitudeTracker.getPosition()); + + // Altitude + if (lastLocation != null && lastLocation.hasAltitude()) { + mAltitudeTracker.predict(0.0); + location.setAltitude(mAltitudeTracker.getPosition()); + } + + // Speed + if (lastLocation != null && lastLocation.hasSpeed()) + location.setSpeed(lastLocation.getSpeed()); + + // Bearing + if (lastLocation != null && lastLocation.hasBearing()) + location.setBearing(lastLocation.getBearing()); + + // Accuracy (always has) + location.setAccuracy((float) (mLatitudeTracker.getAccuracy() * DEG_TO_METER)); + + // Set times + location.setTime(System.currentTimeMillis()); + + logger.debug("Location after applying kalman filter: {}", location.toString()); + return location; + } } diff --git a/src/main/java/com/marianhello/bgloc/provider/ActivityRecognitionLocationProvider.java b/src/main/java/com/marianhello/bgloc/provider/ActivityRecognitionLocationProvider.java index bfb21998..04a1953d 100644 --- a/src/main/java/com/marianhello/bgloc/provider/ActivityRecognitionLocationProvider.java +++ b/src/main/java/com/marianhello/bgloc/provider/ActivityRecognitionLocationProvider.java @@ -47,7 +47,6 @@ public ActivityRecognitionLocationProvider(Context context) { @Override public void onCreate() { super.onCreate(); - Intent detectedActivitiesIntent = new Intent(DETECTED_ACTIVITY_UPDATE); detectedActivitiesPI = PendingIntent.getBroadcast(mContext, 9002, detectedActivitiesIntent, PendingIntent.FLAG_UPDATE_CURRENT); registerReceiver(detectedActivitiesReceiver, new IntentFilter(DETECTED_ACTIVITY_UPDATE)); @@ -85,17 +84,21 @@ public boolean isStarted() { @Override public void onLocationChanged(Location location) { logger.debug("Location change: {}", location.toString()); + Location currentLocation = location; + if(mConfig.getApplyKalmanFilter()) { + currentLocation = applyKalmanFilter(location); + } if (lastActivity.getType() == DetectedActivity.STILL) { - handleStationary(location); + handleStationary(currentLocation); stopTracking(); return; } - showDebugToast("acy:" + location.getAccuracy() + ",v:" + location.getSpeed()); + showDebugToast("acy:" + currentLocation.getAccuracy() + ",v:" + currentLocation.getSpeed()); - lastLocation = location; - handleLocation(location); + lastLocation = currentLocation; + handleLocation(currentLocation); } public void startTracking() { @@ -262,4 +265,4 @@ public void onDestroy() { unregisterReceiver(detectedActivitiesReceiver); super.onDestroy(); } -} +} \ No newline at end of file diff --git a/src/main/java/com/marianhello/bgloc/provider/RawLocationProvider.java b/src/main/java/com/marianhello/bgloc/provider/RawLocationProvider.java index fe781d36..96203d21 100644 --- a/src/main/java/com/marianhello/bgloc/provider/RawLocationProvider.java +++ b/src/main/java/com/marianhello/bgloc/provider/RawLocationProvider.java @@ -87,8 +87,13 @@ public boolean isStarted() { public void onLocationChanged(Location location) { logger.debug("Location change: {}", location.toString()); - showDebugToast("acy:" + location.getAccuracy() + ",v:" + location.getSpeed()); - handleLocation(location); + Location currentLocation = location; + if(mConfig.getApplyKalmanFilter()) { + currentLocation = applyKalmanFilter(location); + } + + showDebugToast("acy:" + currentLocation.getAccuracy() + ",v:" + currentLocation.getSpeed()); + handleLocation(currentLocation); } @Override diff --git a/src/main/java/com/tenforwardconsulting/bgloc/DistanceFilterLocationProvider.java b/src/main/java/com/tenforwardconsulting/bgloc/DistanceFilterLocationProvider.java index 7426d8a6..cf65f9f6 100644 --- a/src/main/java/com/tenforwardconsulting/bgloc/DistanceFilterLocationProvider.java +++ b/src/main/java/com/tenforwardconsulting/bgloc/DistanceFilterLocationProvider.java @@ -284,16 +284,21 @@ public Location getLastBestLocation() { public void onLocationChanged(Location location) { logger.debug("Location change: {} isMoving={}", location.toString(), isMoving); + Location currentLocation = location; + if(mConfig.getApplyKalmanFilter()) { + currentLocation = applyKalmanFilter(location); + } + if (!isMoving && !isAcquiringStationaryLocation && stationaryLocation==null) { // Perhaps our GPS signal was interupted, re-acquire a stationaryLocation now. setPace(false); } - showDebugToast( "mv:" + isMoving + ",acy:" + location.getAccuracy() + ",v:" + location.getSpeed() + ",df:" + scaledDistanceFilter); + showDebugToast( "mv:" + isMoving + ",acy:" + currentLocation.getAccuracy() + ",v:" + currentLocation.getSpeed() + ",df:" + scaledDistanceFilter); if (isAcquiringStationaryLocation) { - if (stationaryLocation == null || stationaryLocation.getAccuracy() > location.getAccuracy()) { - stationaryLocation = location; + if (stationaryLocation == null || stationaryLocation.getAccuracy() > currentLocation.getAccuracy()) { + stationaryLocation = currentLocation; } if (++locationAcquisitionAttempts == MAX_STATIONARY_ACQUISITION_ATTEMPTS) { isAcquiringStationaryLocation = false; @@ -310,7 +315,7 @@ public void onLocationChanged(Location location) { // Got enough samples, assume we're confident in reported speed now. Play "woohoo" sound. playDebugTone(Tone.DOODLY_DOO); isAcquiringSpeed = false; - scaledDistanceFilter = calculateDistanceFilter(location.getSpeed()); + scaledDistanceFilter = calculateDistanceFilter(currentLocation.getSpeed()); setPace(true); } else { playDebugTone(Tone.BEEP); @@ -320,25 +325,25 @@ public void onLocationChanged(Location location) { playDebugTone(Tone.BEEP); // Only reset stationaryAlarm when accurate speed is detected, prevents spurious locations from resetting when stopped. - if ( (location.getSpeed() >= 1) && (location.getAccuracy() <= mConfig.getStationaryRadius()) ) { + if ( (currentLocation.getSpeed() >= 1) && (currentLocation.getAccuracy() <= mConfig.getStationaryRadius()) ) { resetStationaryAlarm(); } // Calculate latest distanceFilter, if it changed by 5 m/s, we'll reconfigure our pace. - Integer newDistanceFilter = calculateDistanceFilter(location.getSpeed()); + Integer newDistanceFilter = calculateDistanceFilter(currentLocation.getSpeed()); if (newDistanceFilter != scaledDistanceFilter.intValue()) { logger.info("Updating distanceFilter: new={} old={}", newDistanceFilter, scaledDistanceFilter); scaledDistanceFilter = newDistanceFilter; setPace(true); } - if (lastLocation != null && location.distanceTo(lastLocation) < mConfig.getDistanceFilter()) { + if (lastLocation != null && currentLocation.distanceTo(lastLocation) < mConfig.getDistanceFilter()) { return; } } else if (stationaryLocation != null) { return; } // Go ahead and cache, push to server - lastLocation = location; - handleLocation(location); + lastLocation = currentLocation; + handleLocation(currentLocation); } public void resetStationaryAlarm() {