FusionEngine.java revision 20de160ca32a8f2936a80ffd70551a22e2371d25 
1/*
2 * Copyright (C) 2012 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 *      http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17package com.android.location.fused;
18
19import java.io.FileDescriptor;
20import java.io.PrintWriter;
21import java.util.HashMap;
22
23import com.android.location.provider.ProviderRequestUnbundled;
24
25import android.content.Context;
26import android.location.Location;
27import android.location.LocationListener;
28import android.location.LocationManager;
29import android.location.LocationRequest;
30import android.os.Bundle;
31import android.os.Looper;
32import android.os.SystemClock;
33import android.os.WorkSource;
34import android.util.Log;
35
36public class FusionEngine implements LocationListener {
37    public interface Callback {
38        public void reportLocation(Location location);
39    }
40
41    private static final String TAG = "FusedLocation";
42    private static final String NETWORK = LocationManager.NETWORK_PROVIDER;
43    private static final String GPS = LocationManager.GPS_PROVIDER;
44
45    // threshold below which a location is considered stale enough
46    // that we shouldn't use its bearing, altitude, speed etc
47    private static final double WEIGHT_THRESHOLD = 0.5;
48    // accuracy in meters at which a Location's weight is halved (compared to 0 accuracy)
49    private static final double ACCURACY_HALFLIFE_M = 20.0;
50    // age in seconds at which a Location's weight is halved (compared to 0 age)
51    private static final double AGE_HALFLIFE_S = 60.0;
52
53    private static final double ACCURACY_DECAY_CONSTANT_M = Math.log(2) / ACCURACY_HALFLIFE_M;
54    private static final double AGE_DECAY_CONSTANT_S = Math.log(2) / AGE_HALFLIFE_S;
55
56    private final Context mContext;
57    private final LocationManager mLocationManager;
58    private final Looper mLooper;
59
60    // all fields are only used on mLooper thread. except for in dump() which is not thread-safe
61    private Callback mCallback;
62    private Location mFusedLocation;
63    private Location mGpsLocation;
64    private Location mNetworkLocation;
65    private double mNetworkWeight;
66    private double mGpsWeight;
67
68    private boolean mEnabled;
69    private ProviderRequestUnbundled mRequest;
70
71    private final HashMap<String, ProviderStats> mStats = new HashMap<String, ProviderStats>();
72
73    public FusionEngine(Context context, Looper looper) {
74        mContext = context;
75        mLocationManager = (LocationManager) context.getSystemService(Context.LOCATION_SERVICE);
76        mNetworkLocation = new Location("");
77        mNetworkLocation.setAccuracy(Float.MAX_VALUE);
78        mGpsLocation = new Location("");
79        mGpsLocation.setAccuracy(Float.MAX_VALUE);
80        mLooper = looper;
81
82        mStats.put(GPS, new ProviderStats());
83        mStats.get(GPS).available = mLocationManager.isProviderEnabled(GPS);
84        mStats.put(NETWORK, new ProviderStats());
85        mStats.get(NETWORK).available = mLocationManager.isProviderEnabled(NETWORK);
86    }
87
88    public void init(Callback callback) {
89        Log.i(TAG, "engine started (" + mContext.getPackageName() + ")");
90        mCallback = callback;
91    }
92
93    /**
94     * Called to stop doing any work, and release all resources
95     * This can happen when a better fusion engine is installed
96     * in a different package, and this one is no longer needed.
97     * Called on mLooper thread
98     */
99    public void deinit() {
100        mRequest = null;
101        disable();
102        Log.i(TAG, "engine stopped (" + mContext.getPackageName() + ")");
103    }
104
105    private boolean isAvailable() {
106        return mStats.get(GPS).available || mStats.get(NETWORK).available;
107    }
108
109    /** Called on mLooper thread */
110    public void enable() {
111        mEnabled = true;
112        updateRequirements();
113    }
114
115    /** Called on mLooper thread */
116    public void disable() {
117        mEnabled = false;
118        updateRequirements();
119    }
120
121    /** Called on mLooper thread */
122    public void setRequest(ProviderRequestUnbundled request, WorkSource source) {
123        mRequest = request;
124        mEnabled = request.getReportLocation();
125        updateRequirements();
126    }
127
128    private static class ProviderStats {
129        public boolean available;
130        public boolean requested;
131        public long requestTime;
132        public long minTime;
133        public long lastRequestTtff;
134        @Override
135        public String toString() {
136            StringBuilder s = new StringBuilder();
137            s.append(available ? "AVAILABLE" : "UNAVAILABLE");
138            s.append(requested ? " REQUESTED" : " ---");
139            return s.toString();
140        }
141    }
142
143    private void enableProvider(String name, long minTime) {
144        ProviderStats stats = mStats.get(name);
145
146        if (!stats.requested) {
147            stats.requestTime = SystemClock.elapsedRealtime();
148            stats.requested = true;
149            stats.minTime = minTime;
150            mLocationManager.requestLocationUpdates(name, minTime, 0, this, mLooper);
151        } else if (stats.minTime != minTime) {
152            stats.minTime = minTime;
153            mLocationManager.requestLocationUpdates(name, minTime, 0, this, mLooper);
154        }
155    }
156
157    private void disableProvider(String name) {
158        ProviderStats stats = mStats.get(name);
159
160        if (stats.requested) {
161            stats.requested = false;
162            mLocationManager.removeUpdates(this);  //TODO GLOBAL
163        }
164    }
165
166    private void updateRequirements() {
167        if (mEnabled == false || mRequest == null) {
168            mRequest = null;
169            disableProvider(NETWORK);
170            disableProvider(GPS);
171            return;
172        }
173
174        ProviderStats gpsStats = mStats.get(GPS);
175        ProviderStats networkStats = mStats.get(NETWORK);
176
177        long networkInterval = Long.MAX_VALUE;
178        long gpsInterval = Long.MAX_VALUE;
179        for (LocationRequest request : mRequest.getLocationRequests()) {
180            switch (request.getQuality()) {
181                case LocationRequest.ACCURACY_FINE:
182                case LocationRequest.POWER_HIGH:
183                    if (request.getInterval() < gpsInterval) {
184                        gpsInterval = request.getInterval();
185                    }
186                    if (request.getInterval() < networkInterval) {
187                        networkInterval = request.getInterval();
188                    }
189                    break;
190                case LocationRequest.ACCURACY_BLOCK:
191                case LocationRequest.ACCURACY_CITY:
192                case LocationRequest.POWER_LOW:
193                    if (request.getInterval() < networkInterval) {
194                        networkInterval = request.getInterval();
195                    }
196                    break;
197            }
198        }
199
200        if (gpsInterval < Long.MAX_VALUE) {
201            enableProvider(GPS, gpsInterval);
202        } else {
203            disableProvider(GPS);
204        }
205        if (networkInterval < Long.MAX_VALUE) {
206            enableProvider(NETWORK, networkInterval);
207        } else {
208            disableProvider(NETWORK);
209        }
210    }
211
212    private static double weighAccuracy(Location loc) {
213        double accuracy = loc.getAccuracy();
214        return Math.exp(-accuracy * ACCURACY_DECAY_CONSTANT_M);
215    }
216
217    private static double weighAge(Location loc) {
218        long ageSeconds = SystemClock.elapsedRealtimeNanos() - loc.getElapsedRealtimeNanos();
219        ageSeconds /= 1000000000L;
220        if (ageSeconds < 0) ageSeconds = 0;
221        return Math.exp(-ageSeconds * AGE_DECAY_CONSTANT_S);
222    }
223
224    private double weigh(double gps, double network) {
225        return (gps * mGpsWeight) + (network * mNetworkWeight);
226    }
227
228    private double weigh(double gps, double network, double wrapMin, double wrapMax) {
229        // apply aliasing
230        double wrapWidth = wrapMax - wrapMin;
231        if (gps - network > wrapWidth / 2) network += wrapWidth;
232        else if (network - gps > wrapWidth / 2) gps += wrapWidth;
233
234        double result = weigh(gps, network);
235
236        // remove aliasing
237        if (result > wrapMax) result -= wrapWidth;
238        return result;
239    }
240
241    private void updateFusedLocation() {
242        // naive fusion
243        mNetworkWeight = weighAccuracy(mNetworkLocation) * weighAge(mNetworkLocation);
244        mGpsWeight = weighAccuracy(mGpsLocation) * weighAge(mGpsLocation);
245        // scale mNetworkWeight and mGpsWeight so that they add to 1
246        double totalWeight = mNetworkWeight + mGpsWeight;
247        mNetworkWeight /= totalWeight;
248        mGpsWeight /= totalWeight;
249
250        Location fused = new Location(LocationManager.FUSED_PROVIDER);
251        // fuse lat/long
252        // assumes the two locations are close enough that earth curvature doesn't matter
253        fused.setLatitude(weigh(mGpsLocation.getLatitude(), mNetworkLocation.getLatitude()));
254        fused.setLongitude(weigh(mGpsLocation.getLongitude(), mNetworkLocation.getLongitude(),
255                -180.0, 180.0));
256
257        // fused accuracy
258        //TODO: use some real math instead of this crude fusion
259        // one suggestion is to fuse in a quadratic manner, eg
260        // sqrt(weigh(gpsAcc^2, netAcc^2)).
261        // another direction to explore is to consider the difference in the 2
262        // locations. If the component locations overlap, the fused accuracy is
263        // better than the component accuracies. If they are far apart,
264        // the fused accuracy is much worse.
265        fused.setAccuracy((float)weigh(mGpsLocation.getAccuracy(), mNetworkLocation.getAccuracy()));
266
267        // fused time - now
268        fused.setTime(System.currentTimeMillis());
269        fused.setElapsedRealtimeNanos(SystemClock.elapsedRealtimeNanos());
270
271        // fuse altitude
272        if (mGpsLocation.hasAltitude() && !mNetworkLocation.hasAltitude() &&
273                mGpsWeight > WEIGHT_THRESHOLD) {
274            fused.setAltitude(mGpsLocation.getAltitude());   // use GPS
275        } else if (!mGpsLocation.hasAltitude() && mNetworkLocation.hasAltitude() &&
276                mNetworkWeight > WEIGHT_THRESHOLD) {
277            fused.setAltitude(mNetworkLocation.getAltitude());   // use Network
278        } else if (mGpsLocation.hasAltitude() && mNetworkLocation.hasAltitude()) {
279            fused.setAltitude(weigh(mGpsLocation.getAltitude(), mNetworkLocation.getAltitude()));
280        }
281
282        // fuse speed
283        if (mGpsLocation.hasSpeed() && !mNetworkLocation.hasSpeed() &&
284                mGpsWeight > WEIGHT_THRESHOLD) {
285            fused.setSpeed(mGpsLocation.getSpeed());   // use GPS if its not too old
286        } else if (!mGpsLocation.hasSpeed() && mNetworkLocation.hasSpeed() &&
287                mNetworkWeight > WEIGHT_THRESHOLD) {
288            fused.setSpeed(mNetworkLocation.getSpeed());   // use Network
289        } else if (mGpsLocation.hasSpeed() && mNetworkLocation.hasSpeed()) {
290            fused.setSpeed((float)weigh(mGpsLocation.getSpeed(), mNetworkLocation.getSpeed()));
291        }
292
293        // fuse bearing
294        if (mGpsLocation.hasBearing() && !mNetworkLocation.hasBearing() &&
295                mGpsWeight > WEIGHT_THRESHOLD) {
296            fused.setBearing(mGpsLocation.getBearing());   // use GPS if its not too old
297        } else if (!mGpsLocation.hasBearing() && mNetworkLocation.hasBearing() &&
298                mNetworkWeight > WEIGHT_THRESHOLD) {
299            fused.setBearing(mNetworkLocation.getBearing());   // use Network
300        } else if (mGpsLocation.hasBearing() && mNetworkLocation.hasBearing()) {
301            fused.setBearing((float)weigh(mGpsLocation.getBearing(), mNetworkLocation.getBearing(),
302                    0.0, 360.0));
303        }
304
305        if (mNetworkLocation != null) {
306            fused.setExtraLocation(Location.EXTRA_NO_GPS_LOCATION, mNetworkLocation);
307        }
308
309        mFusedLocation = fused;
310
311        mCallback.reportLocation(mFusedLocation);
312    }
313
314    /** Called on mLooper thread */
315    @Override
316    public void onLocationChanged(Location location) {
317        if (GPS.equals(location.getProvider())) {
318            mGpsLocation = location;
319            updateFusedLocation();
320        } else if (NETWORK.equals(location.getProvider())) {
321            mNetworkLocation = location;
322            updateFusedLocation();
323        }
324    }
325
326    /** Called on mLooper thread */
327    @Override
328    public void onStatusChanged(String provider, int status, Bundle extras) {  }
329
330    /** Called on mLooper thread */
331    @Override
332    public void onProviderEnabled(String provider) {
333        ProviderStats stats = mStats.get(provider);
334        if (stats == null) return;
335
336        stats.available = true;
337    }
338
339    /** Called on mLooper thread */
340    @Override
341    public void onProviderDisabled(String provider) {
342        ProviderStats stats = mStats.get(provider);
343        if (stats == null) return;
344
345        stats.available = false;
346    }
347
348    public void dump(FileDescriptor fd, PrintWriter pw, String[] args) {
349        StringBuilder s = new StringBuilder();
350        s.append("mEnabled=" + mEnabled).append(' ').append(mRequest).append('\n');
351        s.append("fused=").append(mFusedLocation).append('\n');
352        s.append(String.format("gps %.3f %s\n", mGpsWeight, mGpsLocation));
353        s.append("    ").append(mStats.get(GPS)).append('\n');
354        s.append(String.format("net %.3f %s\n", mNetworkWeight, mNetworkLocation));
355        s.append("    ").append(mStats.get(NETWORK)).append('\n');
356        pw.append(s);
357    }
358}
359