Browse Source
Update the Gnsslogger to version 2.0 supporting weighted least square computations, several analysis plots and as well pseudorange residual analysispull/7/head
mohammedkhider
7 years ago
committed by
GitHub
77 changed files with 9409 additions and 1385 deletions
Binary file not shown.
|
After Width: | Height: | Size: 19 KiB |
@ -0,0 +1,174 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.android.apps.location.gps.gnsslogger; |
||||||
|
|
||||||
|
import android.app.Activity; |
||||||
|
import android.app.Fragment; |
||||||
|
import android.content.Intent; |
||||||
|
import android.location.LocationManager; |
||||||
|
import android.os.Bundle; |
||||||
|
import android.text.Editable; |
||||||
|
import android.text.SpannableStringBuilder; |
||||||
|
import android.text.style.ForegroundColorSpan; |
||||||
|
import android.util.Log; |
||||||
|
import android.view.LayoutInflater; |
||||||
|
import android.view.View; |
||||||
|
import android.view.View.OnClickListener; |
||||||
|
import android.view.ViewGroup; |
||||||
|
import android.widget.Button; |
||||||
|
import android.widget.ScrollView; |
||||||
|
import android.widget.TextView; |
||||||
|
|
||||||
|
/** The UI fragment that hosts a logging view. */ |
||||||
|
public class AgnssFragment extends Fragment { |
||||||
|
|
||||||
|
public static final String TAG = ":AgnssFragment"; |
||||||
|
private TextView mLogView; |
||||||
|
private ScrollView mScrollView; |
||||||
|
private GnssContainer mGpsContainer; |
||||||
|
private AgnssUiLogger mUiLogger; |
||||||
|
|
||||||
|
private final AgnssUIFragmentComponent mUiComponent = new AgnssUIFragmentComponent(); |
||||||
|
|
||||||
|
public void setGpsContainer(GnssContainer value) { |
||||||
|
mGpsContainer = value; |
||||||
|
} |
||||||
|
|
||||||
|
public void setUILogger(AgnssUiLogger value) { |
||||||
|
mUiLogger = value; |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public View onCreateView( |
||||||
|
LayoutInflater inflater, ViewGroup container, Bundle savedInstanceState) { |
||||||
|
View newView = inflater.inflate(R.layout.fragment_agnss, container, false /* attachToRoot */); |
||||||
|
mLogView = (TextView) newView.findViewById(R.id.log_view); |
||||||
|
mScrollView = (ScrollView) newView.findViewById(R.id.log_scroll); |
||||||
|
|
||||||
|
if (mUiLogger != null) { |
||||||
|
mUiLogger.setUiFragmentComponent(mUiComponent); |
||||||
|
} |
||||||
|
|
||||||
|
Button clearAgps = (Button) newView.findViewById(R.id.clearAgps); |
||||||
|
clearAgps.setOnClickListener( |
||||||
|
new OnClickListener() { |
||||||
|
@Override |
||||||
|
public void onClick(View view) { |
||||||
|
Log.i(GnssContainer.TAG + TAG, "Clearing AGPS"); |
||||||
|
LocationManager locationManager = mGpsContainer.getLocationManager(); |
||||||
|
locationManager.sendExtraCommand( |
||||||
|
LocationManager.GPS_PROVIDER, "delete_aiding_data", null); |
||||||
|
Log.i(GnssContainer.TAG + TAG, "Clearing AGPS command sent"); |
||||||
|
} |
||||||
|
}); |
||||||
|
|
||||||
|
Button fetchExtraData = (Button) newView.findViewById(R.id.fetchExtraData); |
||||||
|
fetchExtraData.setOnClickListener( |
||||||
|
new OnClickListener() { |
||||||
|
@Override |
||||||
|
public void onClick(View view) { |
||||||
|
Log.i(GnssContainer.TAG + TAG, "Fetching Extra data"); |
||||||
|
LocationManager locationManager = mGpsContainer.getLocationManager(); |
||||||
|
Bundle bundle = new Bundle(); |
||||||
|
locationManager.sendExtraCommand("gps", "force_xtra_injection", bundle); |
||||||
|
Log.i(GnssContainer.TAG + TAG, "Fetching Extra data Command sent"); |
||||||
|
} |
||||||
|
}); |
||||||
|
|
||||||
|
Button fetchTimeData = (Button) newView.findViewById(R.id.fetchTimeData); |
||||||
|
fetchTimeData.setOnClickListener( |
||||||
|
new OnClickListener() { |
||||||
|
@Override |
||||||
|
public void onClick(View view) { |
||||||
|
Log.i(GnssContainer.TAG + TAG, "Fetching Time data"); |
||||||
|
LocationManager locationManager = mGpsContainer.getLocationManager(); |
||||||
|
Bundle bundle = new Bundle(); |
||||||
|
locationManager.sendExtraCommand("gps", "force_time_injection", bundle); |
||||||
|
Log.i(GnssContainer.TAG + TAG, "Fetching Time data Command sent"); |
||||||
|
} |
||||||
|
}); |
||||||
|
|
||||||
|
Button requestSingleNlp = (Button) newView.findViewById(R.id.requestSingleNlp); |
||||||
|
requestSingleNlp.setOnClickListener( |
||||||
|
new OnClickListener() { |
||||||
|
@Override |
||||||
|
public void onClick(View view) { |
||||||
|
Log.i(GnssContainer.TAG + TAG, "Requesting Single NLP Location"); |
||||||
|
mGpsContainer.registerSingleNetworkLocation(); |
||||||
|
Log.i(GnssContainer.TAG + TAG, "Single NLP Location Requested"); |
||||||
|
} |
||||||
|
}); |
||||||
|
|
||||||
|
Button requestSingleGps = (Button) newView.findViewById(R.id.requestSingleGps); |
||||||
|
requestSingleGps.setOnClickListener( |
||||||
|
new OnClickListener() { |
||||||
|
@Override |
||||||
|
public void onClick(View view) { |
||||||
|
Log.i(GnssContainer.TAG + TAG, "Requesting Single GPS Location"); |
||||||
|
mGpsContainer.registerSingleGpsLocation(); |
||||||
|
Log.i(GnssContainer.TAG + TAG, "Single GPS Location Requested"); |
||||||
|
} |
||||||
|
}); |
||||||
|
Button clear = (Button) newView.findViewById(R.id.clear_log); |
||||||
|
clear.setOnClickListener( |
||||||
|
new OnClickListener() { |
||||||
|
@Override |
||||||
|
public void onClick(View view) { |
||||||
|
mLogView.setText(""); |
||||||
|
} |
||||||
|
}); |
||||||
|
|
||||||
|
return newView; |
||||||
|
} |
||||||
|
/** A facade for Agnss UI related operations. */ |
||||||
|
public class AgnssUIFragmentComponent { |
||||||
|
|
||||||
|
private static final int MAX_LENGTH = 12000; |
||||||
|
private static final int LOWER_THRESHOLD = (int) (MAX_LENGTH * 0.5); |
||||||
|
|
||||||
|
public synchronized void logTextFragment(final String tag, final String text, int color) { |
||||||
|
final SpannableStringBuilder builder = new SpannableStringBuilder(); |
||||||
|
builder.append(tag).append(" | ").append(text).append("\n"); |
||||||
|
builder.setSpan( |
||||||
|
new ForegroundColorSpan(color), |
||||||
|
0 /* start */, |
||||||
|
builder.length(), |
||||||
|
SpannableStringBuilder.SPAN_INCLUSIVE_EXCLUSIVE); |
||||||
|
|
||||||
|
Activity activity = getActivity(); |
||||||
|
if (activity == null) { |
||||||
|
return; |
||||||
|
} |
||||||
|
activity.runOnUiThread( |
||||||
|
new Runnable() { |
||||||
|
@Override |
||||||
|
public void run() { |
||||||
|
mLogView.append(builder); |
||||||
|
Editable editable = mLogView.getEditableText(); |
||||||
|
int length = editable.length(); |
||||||
|
if (length > MAX_LENGTH) { |
||||||
|
editable.delete(0, length - LOWER_THRESHOLD); |
||||||
|
} |
||||||
|
} |
||||||
|
}); |
||||||
|
} |
||||||
|
|
||||||
|
public void startActivity(Intent intent) { |
||||||
|
getActivity().startActivity(intent); |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,111 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.android.apps.location.gps.gnsslogger; |
||||||
|
|
||||||
|
import android.graphics.Color; |
||||||
|
import android.location.GnssMeasurementsEvent; |
||||||
|
import android.location.GnssNavigationMessage; |
||||||
|
import android.location.GnssStatus; |
||||||
|
import android.location.Location; |
||||||
|
import android.os.Bundle; |
||||||
|
import android.util.Log; |
||||||
|
import com.google.android.apps.location.gps.gnsslogger.AgnssFragment.AgnssUIFragmentComponent; |
||||||
|
import java.util.concurrent.TimeUnit; |
||||||
|
|
||||||
|
/** |
||||||
|
* A class representing a UI logger for the application. Its responsibility is show information in |
||||||
|
* the UI. |
||||||
|
*/ |
||||||
|
public class AgnssUiLogger implements GnssListener { |
||||||
|
|
||||||
|
private static final int USED_COLOR = Color.rgb(0x4a, 0x5f, 0x70); |
||||||
|
|
||||||
|
public AgnssUiLogger() {} |
||||||
|
|
||||||
|
private AgnssUIFragmentComponent mUiFragmentComponent; |
||||||
|
|
||||||
|
public synchronized AgnssUIFragmentComponent getUiFragmentComponent() { |
||||||
|
return mUiFragmentComponent; |
||||||
|
} |
||||||
|
|
||||||
|
public synchronized void setUiFragmentComponent(AgnssUIFragmentComponent value) { |
||||||
|
mUiFragmentComponent = value; |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onProviderEnabled(String provider) { |
||||||
|
logLocationEvent("onProviderEnabled: " + provider); |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onTTFFReceived(long l) { |
||||||
|
logLocationEvent("timeToFirstFix: " + TimeUnit.NANOSECONDS.toMillis(l) + "millis"); |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onProviderDisabled(String provider) { |
||||||
|
logLocationEvent("onProviderDisabled: " + provider); |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onLocationChanged(Location location) { |
||||||
|
logLocationEvent("onLocationChanged: " + location); |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onLocationStatusChanged(String provider, int status, Bundle extras) {} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onGnssMeasurementsReceived(GnssMeasurementsEvent event) {} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onGnssMeasurementsStatusChanged(int status) {} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onGnssNavigationMessageReceived(GnssNavigationMessage event) {} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onGnssNavigationMessageStatusChanged(int status) {} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onGnssStatusChanged(GnssStatus gnssStatus) {} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onNmeaReceived(long timestamp, String s) {} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onListenerRegistration(String listener, boolean result) { |
||||||
|
logEvent("Registration", String.format("add%sListener: %b", listener, result), USED_COLOR); |
||||||
|
} |
||||||
|
|
||||||
|
private void logEvent(String tag, String message, int color) { |
||||||
|
String composedTag = GnssContainer.TAG + tag; |
||||||
|
Log.d(composedTag, message); |
||||||
|
logText(tag, message, color); |
||||||
|
} |
||||||
|
|
||||||
|
private void logText(String tag, String text, int color) { |
||||||
|
AgnssUIFragmentComponent component = getUiFragmentComponent(); |
||||||
|
if (component != null) { |
||||||
|
component.logTextFragment(tag, text, color); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
private void logLocationEvent(String event) { |
||||||
|
logEvent("Location", event, USED_COLOR); |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,48 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.android.apps.location.gps.gnsslogger; |
||||||
|
|
||||||
|
import android.content.BroadcastReceiver; |
||||||
|
import android.content.Context; |
||||||
|
import android.content.Intent; |
||||||
|
import android.support.v4.content.LocalBroadcastManager; |
||||||
|
|
||||||
|
/** |
||||||
|
* A {@link BroadcastReceiver} that receives and broadcasts the result of |
||||||
|
* {@link com.google.android.gms.location.ActivityRecognition |
||||||
|
* #ActivityRecognitionApi#requestActivityUpdates()} |
||||||
|
* to {@link MainActivity} to be further analyzed. |
||||||
|
*/ |
||||||
|
public class DetectedActivitiesIntentReceiver extends BroadcastReceiver { |
||||||
|
public static String AR_RESULT_BROADCAST_ACTION = |
||||||
|
"com.google.android.apps.location.gps.gnsslogger.AR_RESULT_BROADCAST_ACTION"; |
||||||
|
|
||||||
|
/** |
||||||
|
* Gets called when the result of {@link com.google.android.gms.location.ActivityRecognition |
||||||
|
* #ActivityRecognitionApi#requestActivityUpdates()} is available and handles |
||||||
|
* incoming intents. |
||||||
|
* |
||||||
|
* @param intent The Intent is provided (inside a {@link android.app.PendingIntent}) |
||||||
|
* when {@link com.google.android.gms.location.ActivityRecognition |
||||||
|
* #ActivityRecognitionApi#requestActivityUpdates()} is called. |
||||||
|
*/ |
||||||
|
public void onReceive(Context context, Intent intent) { |
||||||
|
|
||||||
|
intent.setAction(AR_RESULT_BROADCAST_ACTION); |
||||||
|
LocalBroadcastManager.getInstance(context).sendBroadcast(intent); |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,27 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.android.apps.location.gps.gnsslogger; |
||||||
|
|
||||||
|
/** |
||||||
|
* A class representing the interface toggling auto ground truth mode switch |
||||||
|
*/ |
||||||
|
public interface GroundTruthModeSwitcher { |
||||||
|
/** |
||||||
|
* Gets called to enable auto switch ground truth mode |
||||||
|
*/ |
||||||
|
void setAutoSwitchGroundTruthModeEnabled(boolean enabled); |
||||||
|
} |
||||||
@ -0,0 +1,190 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.android.apps.location.gps.gnsslogger; |
||||||
|
|
||||||
|
import android.app.Activity; |
||||||
|
import android.app.Fragment; |
||||||
|
import android.os.Bundle; |
||||||
|
import android.util.Log; |
||||||
|
import android.view.LayoutInflater; |
||||||
|
import android.view.View; |
||||||
|
import android.view.ViewGroup; |
||||||
|
import com.google.android.gms.maps.CameraUpdateFactory; |
||||||
|
import com.google.android.gms.maps.GoogleMap; |
||||||
|
import com.google.android.gms.maps.MapView; |
||||||
|
import com.google.android.gms.maps.MapsInitializer; |
||||||
|
import com.google.android.gms.maps.OnMapReadyCallback; |
||||||
|
import com.google.android.gms.maps.model.BitmapDescriptorFactory; |
||||||
|
import com.google.android.gms.maps.model.LatLng; |
||||||
|
import com.google.android.gms.maps.model.Marker; |
||||||
|
import com.google.android.gms.maps.model.MarkerOptions; |
||||||
|
import java.text.SimpleDateFormat; |
||||||
|
import java.util.Date; |
||||||
|
import java.util.HashSet; |
||||||
|
import java.util.Set; |
||||||
|
|
||||||
|
/** |
||||||
|
* A map fragment to show the computed least square position and the device computed position on |
||||||
|
* Google map. |
||||||
|
*/ |
||||||
|
public class MapFragment extends Fragment implements OnMapReadyCallback { |
||||||
|
private static final float ZOOM_LEVEL = 15; |
||||||
|
private static final String TAG = "MapFragment"; |
||||||
|
private RealTimePositionVelocityCalculator mPositionVelocityCalculator; |
||||||
|
|
||||||
|
private static final SimpleDateFormat DATE_SDF = new SimpleDateFormat("HH:mm:ss"); |
||||||
|
|
||||||
|
// UI members
|
||||||
|
private GoogleMap mMap; // Might be null if Google Play services APK is not available.
|
||||||
|
private MapView mMapView; |
||||||
|
private final Set<Object> mSetOfFeatures = new HashSet<Object>(); |
||||||
|
|
||||||
|
private Marker mLastLocationMarkerRaw = null; |
||||||
|
private Marker mLastLocationMarkerDevice = null; |
||||||
|
|
||||||
|
@Override |
||||||
|
public View onCreateView( |
||||||
|
LayoutInflater inflater, ViewGroup container, Bundle savedInstanceState) { |
||||||
|
View rootView = inflater.inflate(R.layout.map_fragment, container, false); |
||||||
|
mMapView = ((MapView) rootView.findViewById(R.id.map)); |
||||||
|
mMapView.onCreate(savedInstanceState); |
||||||
|
mMapView.getMapAsync(this); |
||||||
|
MapsInitializer.initialize(getActivity()); |
||||||
|
|
||||||
|
RealTimePositionVelocityCalculator currentPositionVelocityCalculator = |
||||||
|
mPositionVelocityCalculator; |
||||||
|
if (currentPositionVelocityCalculator != null) { |
||||||
|
currentPositionVelocityCalculator.setMapFragment(this); |
||||||
|
} |
||||||
|
|
||||||
|
return rootView; |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onResume() { |
||||||
|
super.onResume(); |
||||||
|
mMapView.onResume(); |
||||||
|
if (mMap != null) { |
||||||
|
mMap.clear(); |
||||||
|
} |
||||||
|
mLastLocationMarkerRaw = null; |
||||||
|
mLastLocationMarkerDevice = null; |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onPause() { |
||||||
|
mMapView.onPause(); |
||||||
|
super.onPause(); |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onDestroy() { |
||||||
|
mMapView.onDestroy(); |
||||||
|
super.onDestroy(); |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onMapReady(GoogleMap googleMap) { |
||||||
|
mMap = googleMap; |
||||||
|
mMap.setMyLocationEnabled(false); |
||||||
|
mMap.getUiSettings().setZoomControlsEnabled(true); |
||||||
|
mMap.getUiSettings().setZoomGesturesEnabled(true); |
||||||
|
mMap.getUiSettings().setMapToolbarEnabled(false); |
||||||
|
} |
||||||
|
|
||||||
|
public void setPositionVelocityCalculator(RealTimePositionVelocityCalculator value) { |
||||||
|
mPositionVelocityCalculator = value; |
||||||
|
} |
||||||
|
|
||||||
|
public void updateMapViewWithPostions( |
||||||
|
final double latDegRaw, |
||||||
|
final double lngDegRaw, |
||||||
|
final double latDegDevice, |
||||||
|
final double lngDegDevice, |
||||||
|
final long timeMillis) { |
||||||
|
Activity activity = getActivity(); |
||||||
|
if (activity == null) { |
||||||
|
return; |
||||||
|
} |
||||||
|
activity.runOnUiThread( |
||||||
|
new Runnable() { |
||||||
|
@Override |
||||||
|
public void run() { |
||||||
|
Log.i(TAG, "onLocationChanged"); |
||||||
|
LatLng latLngRaw = new LatLng(latDegRaw, lngDegRaw); |
||||||
|
LatLng latLngDevice = new LatLng(latDegDevice, lngDegDevice); |
||||||
|
if (mLastLocationMarkerRaw == null && mLastLocationMarkerDevice == null) { |
||||||
|
if (mMap != null) { |
||||||
|
mLastLocationMarkerDevice = |
||||||
|
mMap.addMarker( |
||||||
|
new MarkerOptions() |
||||||
|
.position(latLngDevice) |
||||||
|
.title(getResources().getString(R.string.title_device)) |
||||||
|
.icon( |
||||||
|
BitmapDescriptorFactory.defaultMarker( |
||||||
|
BitmapDescriptorFactory.HUE_BLUE))); |
||||||
|
mLastLocationMarkerDevice.showInfoWindow(); |
||||||
|
|
||||||
|
mLastLocationMarkerRaw = |
||||||
|
mMap.addMarker( |
||||||
|
new MarkerOptions() |
||||||
|
.position(latLngRaw) |
||||||
|
.title(getResources().getString(R.string.title_wls)) |
||||||
|
.icon( |
||||||
|
BitmapDescriptorFactory.defaultMarker( |
||||||
|
BitmapDescriptorFactory.HUE_GREEN))); |
||||||
|
mLastLocationMarkerRaw.showInfoWindow(); |
||||||
|
|
||||||
|
mMap.moveCamera(CameraUpdateFactory.newLatLngZoom(latLngRaw, ZOOM_LEVEL)); |
||||||
|
} |
||||||
|
} else { |
||||||
|
mLastLocationMarkerRaw.setPosition(latLngRaw); |
||||||
|
mLastLocationMarkerDevice.setPosition(latLngDevice); |
||||||
|
} |
||||||
|
if (mLastLocationMarkerRaw == null && mLastLocationMarkerDevice == null) { |
||||||
|
String formattedDate = DATE_SDF.format(new Date(timeMillis)); |
||||||
|
mLastLocationMarkerRaw.setTitle("time: " + formattedDate); |
||||||
|
mLastLocationMarkerDevice.showInfoWindow(); |
||||||
|
|
||||||
|
mLastLocationMarkerRaw.setTitle("time: " + formattedDate); |
||||||
|
mLastLocationMarkerDevice.showInfoWindow(); |
||||||
|
} |
||||||
|
} |
||||||
|
}); |
||||||
|
} |
||||||
|
|
||||||
|
public void clearMarkers() { |
||||||
|
Activity activity = getActivity(); |
||||||
|
if (activity == null) { |
||||||
|
return; |
||||||
|
} |
||||||
|
activity.runOnUiThread( |
||||||
|
new Runnable() { |
||||||
|
@Override |
||||||
|
public void run() { |
||||||
|
if (mLastLocationMarkerRaw != null) { |
||||||
|
mLastLocationMarkerRaw.remove(); |
||||||
|
mLastLocationMarkerRaw = null; |
||||||
|
} |
||||||
|
if (mLastLocationMarkerDevice != null) { |
||||||
|
mLastLocationMarkerDevice.remove(); |
||||||
|
mLastLocationMarkerDevice = null; |
||||||
|
} |
||||||
|
} |
||||||
|
}); |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,464 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.android.apps.location.gps.gnsslogger; |
||||||
|
|
||||||
|
import android.app.Fragment; |
||||||
|
import android.content.Context; |
||||||
|
import android.graphics.Color; |
||||||
|
import android.graphics.Paint.Align; |
||||||
|
import android.location.GnssMeasurement; |
||||||
|
import android.location.GnssMeasurementsEvent; |
||||||
|
import android.location.GnssStatus; |
||||||
|
import android.os.Bundle; |
||||||
|
import android.support.v4.util.ArrayMap; |
||||||
|
import android.text.Spannable; |
||||||
|
import android.text.SpannableStringBuilder; |
||||||
|
import android.text.style.ForegroundColorSpan; |
||||||
|
import android.view.LayoutInflater; |
||||||
|
import android.view.View; |
||||||
|
import android.view.ViewGroup; |
||||||
|
import android.widget.AdapterView; |
||||||
|
import android.widget.AdapterView.OnItemSelectedListener; |
||||||
|
import android.widget.LinearLayout; |
||||||
|
import android.widget.Spinner; |
||||||
|
import android.widget.TextView; |
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.GpsNavigationMessageStore; |
||||||
|
import java.text.DecimalFormat; |
||||||
|
import java.util.ArrayList; |
||||||
|
import java.util.Collections; |
||||||
|
import java.util.Comparator; |
||||||
|
import java.util.List; |
||||||
|
import java.util.Random; |
||||||
|
import java.util.concurrent.TimeUnit; |
||||||
|
import org.achartengine.ChartFactory; |
||||||
|
import org.achartengine.GraphicalView; |
||||||
|
import org.achartengine.model.XYMultipleSeriesDataset; |
||||||
|
import org.achartengine.model.XYSeries; |
||||||
|
import org.achartengine.renderer.XYMultipleSeriesRenderer; |
||||||
|
import org.achartengine.renderer.XYSeriesRenderer; |
||||||
|
import org.achartengine.util.MathHelper; |
||||||
|
|
||||||
|
/** A plot fragment to show real-time Gnss analysis migrated from GnssAnalysis Tool. */ |
||||||
|
public class PlotFragment extends Fragment { |
||||||
|
|
||||||
|
/** Total number of kinds of plot tabs */ |
||||||
|
private static final int NUMBER_OF_TABS = 2; |
||||||
|
|
||||||
|
/** The position of the CN0 over time plot tab */ |
||||||
|
private static final int CN0_TAB = 0; |
||||||
|
|
||||||
|
/** The position of the prearrange residual plot tab*/ |
||||||
|
private static final int PR_RESIDUAL_TAB = 1; |
||||||
|
|
||||||
|
/** The number of Gnss constellations */ |
||||||
|
private static final int NUMBER_OF_CONSTELLATIONS = 6; |
||||||
|
|
||||||
|
/** The X range of the plot, we are keeping the latest one minute visible */ |
||||||
|
private static final double TIME_INTERVAL_SECONDS = 60; |
||||||
|
|
||||||
|
/** The index in data set we reserved for the plot containing all constellations */ |
||||||
|
private static final int DATA_SET_INDEX_ALL = 0; |
||||||
|
|
||||||
|
/** The number of satellites we pick for the strongest satellite signal strength calculation */ |
||||||
|
private static final int NUMBER_OF_STRONGEST_SATELLITES = 4; |
||||||
|
|
||||||
|
/** Data format used to format the data in the text view */ |
||||||
|
private static final DecimalFormat sDataFormat = new DecimalFormat("##.#"); |
||||||
|
|
||||||
|
private GraphicalView mChartView; |
||||||
|
|
||||||
|
/** The average of the average of strongest satellite signal strength over history */ |
||||||
|
private double mAverageCn0 = 0; |
||||||
|
|
||||||
|
/** Total number of {@link GnssMeasurementsEvent} has been recieved*/ |
||||||
|
private int mMeasurementCount = 0; |
||||||
|
private double mInitialTimeSeconds = -1; |
||||||
|
private TextView mAnalysisView; |
||||||
|
private double mLastTimeReceivedSeconds = 0; |
||||||
|
private final ColorMap mColorMap = new ColorMap(); |
||||||
|
private DataSetManager mDataSetManager; |
||||||
|
private XYMultipleSeriesRenderer mCurrentRenderer; |
||||||
|
private LinearLayout mLayout; |
||||||
|
private int mCurrentTab = 0; |
||||||
|
|
||||||
|
@Override |
||||||
|
public View onCreateView( |
||||||
|
LayoutInflater inflater, ViewGroup container, Bundle savedInstanceState) { |
||||||
|
View plotView = inflater.inflate(R.layout.fragment_plot, container, false /* attachToRoot */); |
||||||
|
|
||||||
|
mDataSetManager |
||||||
|
= new DataSetManager(NUMBER_OF_TABS, NUMBER_OF_CONSTELLATIONS, getContext(), mColorMap); |
||||||
|
|
||||||
|
// Set UI elements handlers
|
||||||
|
final Spinner spinner = plotView.findViewById(R.id.constellation_spinner); |
||||||
|
final Spinner tabSpinner = plotView.findViewById(R.id.tab_spinner); |
||||||
|
|
||||||
|
OnItemSelectedListener spinnerOnSelectedListener = new OnItemSelectedListener() { |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onItemSelected(AdapterView<?> parent, View view, int position, long id) { |
||||||
|
mCurrentTab = tabSpinner.getSelectedItemPosition(); |
||||||
|
XYMultipleSeriesRenderer renderer |
||||||
|
= mDataSetManager.getRenderer(mCurrentTab, spinner.getSelectedItemPosition()); |
||||||
|
XYMultipleSeriesDataset dataSet |
||||||
|
= mDataSetManager.getDataSet(mCurrentTab, spinner.getSelectedItemPosition()); |
||||||
|
if (mLastTimeReceivedSeconds > TIME_INTERVAL_SECONDS) { |
||||||
|
renderer.setXAxisMax(mLastTimeReceivedSeconds); |
||||||
|
renderer.setXAxisMin(mLastTimeReceivedSeconds - TIME_INTERVAL_SECONDS); |
||||||
|
} |
||||||
|
mCurrentRenderer = renderer; |
||||||
|
mLayout.removeAllViews(); |
||||||
|
mChartView = ChartFactory.getLineChartView(getContext(), dataSet, renderer); |
||||||
|
mLayout.addView(mChartView); |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onNothingSelected(AdapterView<?> parent) {} |
||||||
|
}; |
||||||
|
|
||||||
|
spinner.setOnItemSelectedListener(spinnerOnSelectedListener); |
||||||
|
tabSpinner.setOnItemSelectedListener(spinnerOnSelectedListener); |
||||||
|
|
||||||
|
// Set up the Graph View
|
||||||
|
mCurrentRenderer = mDataSetManager.getRenderer(mCurrentTab, DATA_SET_INDEX_ALL); |
||||||
|
XYMultipleSeriesDataset currentDataSet |
||||||
|
= mDataSetManager.getDataSet(mCurrentTab, DATA_SET_INDEX_ALL); |
||||||
|
mChartView = ChartFactory.getLineChartView(getContext(), currentDataSet, mCurrentRenderer); |
||||||
|
mAnalysisView = plotView.findViewById(R.id.analysis); |
||||||
|
mAnalysisView.setTextColor(Color.BLACK); |
||||||
|
mLayout = plotView.findViewById(R.id.plot); |
||||||
|
mLayout.addView(mChartView); |
||||||
|
return plotView; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Updates the CN0 versus Time plot data from a {@link GnssMeasurement} |
||||||
|
*/ |
||||||
|
protected void updateCnoTab(GnssMeasurementsEvent event) { |
||||||
|
long timeInSeconds = |
||||||
|
TimeUnit.NANOSECONDS.toSeconds(event.getClock().getTimeNanos()); |
||||||
|
if (mInitialTimeSeconds < 0) { |
||||||
|
mInitialTimeSeconds = timeInSeconds; |
||||||
|
} |
||||||
|
|
||||||
|
// Building the texts message in analysis text view
|
||||||
|
List<GnssMeasurement> measurements = |
||||||
|
sortByCarrierToNoiseRatio(new ArrayList<>(event.getMeasurements())); |
||||||
|
SpannableStringBuilder builder = new SpannableStringBuilder(); |
||||||
|
double currentAverage = 0; |
||||||
|
if (measurements.size() >= NUMBER_OF_STRONGEST_SATELLITES) { |
||||||
|
mAverageCn0 = |
||||||
|
(mAverageCn0 * mMeasurementCount |
||||||
|
+ (measurements.get(0).getCn0DbHz() |
||||||
|
+ measurements.get(1).getCn0DbHz() |
||||||
|
+ measurements.get(2).getCn0DbHz() |
||||||
|
+ measurements.get(3).getCn0DbHz()) |
||||||
|
/ NUMBER_OF_STRONGEST_SATELLITES) |
||||||
|
/ (++mMeasurementCount); |
||||||
|
currentAverage = |
||||||
|
(measurements.get(0).getCn0DbHz() |
||||||
|
+ measurements.get(1).getCn0DbHz() |
||||||
|
+ measurements.get(2).getCn0DbHz() |
||||||
|
+ measurements.get(3).getCn0DbHz()) |
||||||
|
/ NUMBER_OF_STRONGEST_SATELLITES; |
||||||
|
} |
||||||
|
builder.append(getString(R.string.history_average_hint, |
||||||
|
sDataFormat.format(mAverageCn0) + "\n")); |
||||||
|
builder.append(getString(R.string.current_average_hint, |
||||||
|
sDataFormat.format(currentAverage) + "\n")); |
||||||
|
for (int i = 0; i < NUMBER_OF_STRONGEST_SATELLITES && i < measurements.size(); i++) { |
||||||
|
int start = builder.length(); |
||||||
|
builder.append( |
||||||
|
mDataSetManager.getConstellationPrefix(measurements.get(i).getConstellationType()) |
||||||
|
+ measurements.get(i).getSvid() |
||||||
|
+ ": " |
||||||
|
+ sDataFormat.format(measurements.get(i).getCn0DbHz()) |
||||||
|
+ "\n"); |
||||||
|
int end = builder.length(); |
||||||
|
builder.setSpan( |
||||||
|
new ForegroundColorSpan( |
||||||
|
mColorMap.getColor( |
||||||
|
measurements.get(i).getSvid(), measurements.get(i).getConstellationType())), |
||||||
|
start, |
||||||
|
end, |
||||||
|
Spannable.SPAN_INCLUSIVE_EXCLUSIVE); |
||||||
|
} |
||||||
|
builder.append(getString(R.string.satellite_number_sum_hint, measurements.size())); |
||||||
|
mAnalysisView.setText(builder); |
||||||
|
|
||||||
|
// Adding incoming data into Dataset
|
||||||
|
mLastTimeReceivedSeconds = timeInSeconds - mInitialTimeSeconds; |
||||||
|
for (GnssMeasurement measurement : measurements) { |
||||||
|
int constellationType = measurement.getConstellationType(); |
||||||
|
int svID = measurement.getSvid(); |
||||||
|
if (constellationType != GnssStatus.CONSTELLATION_UNKNOWN) { |
||||||
|
mDataSetManager.addValue( |
||||||
|
CN0_TAB, |
||||||
|
constellationType, |
||||||
|
svID, |
||||||
|
mLastTimeReceivedSeconds, |
||||||
|
measurement.getCn0DbHz()); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
mDataSetManager.fillInDiscontinuity(CN0_TAB, mLastTimeReceivedSeconds); |
||||||
|
|
||||||
|
// Checks if the plot has reached the end of frame and resize
|
||||||
|
if (mLastTimeReceivedSeconds > mCurrentRenderer.getXAxisMax()) { |
||||||
|
mCurrentRenderer.setXAxisMax(mLastTimeReceivedSeconds); |
||||||
|
mCurrentRenderer.setXAxisMin(mLastTimeReceivedSeconds - TIME_INTERVAL_SECONDS); |
||||||
|
} |
||||||
|
|
||||||
|
mChartView.invalidate(); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Updates the pseudorange residual plot from residual results calculated by |
||||||
|
* {@link RealTimePositionVelocityCalculator} |
||||||
|
* |
||||||
|
* @param residuals An array of MAX_NUMBER_OF_SATELLITES elements where indexes of satellites was |
||||||
|
* not seen are fixed with {@code Double.NaN} and indexes of satellites what were seen |
||||||
|
* are filled with pseudorange residual in meters |
||||||
|
* @param timeInSeconds the time at which measurements are received |
||||||
|
*/ |
||||||
|
protected void updatePseudorangeResidualTab(double[] residuals, double timeInSeconds) { |
||||||
|
double timeSinceLastMeasurement = timeInSeconds - mInitialTimeSeconds; |
||||||
|
for (int i = 1; i <= GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES; i++) { |
||||||
|
if (!Double.isNaN(residuals[i - 1])) { |
||||||
|
mDataSetManager.addValue( |
||||||
|
PR_RESIDUAL_TAB, |
||||||
|
GnssStatus.CONSTELLATION_GPS, |
||||||
|
i, |
||||||
|
timeSinceLastMeasurement, |
||||||
|
residuals[i - 1]); |
||||||
|
} |
||||||
|
} |
||||||
|
mDataSetManager.fillInDiscontinuity(PR_RESIDUAL_TAB, timeSinceLastMeasurement); |
||||||
|
} |
||||||
|
|
||||||
|
private List<GnssMeasurement> sortByCarrierToNoiseRatio(List<GnssMeasurement> measurements) { |
||||||
|
Collections.sort( |
||||||
|
measurements, |
||||||
|
new Comparator<GnssMeasurement>() { |
||||||
|
@Override |
||||||
|
public int compare(GnssMeasurement o1, GnssMeasurement o2) { |
||||||
|
return Double.compare(o2.getCn0DbHz(), o1.getCn0DbHz()); |
||||||
|
} |
||||||
|
}); |
||||||
|
return measurements; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* An utility class provides and keeps record of all color assignments to the satellite in the |
||||||
|
* plots. Each satellite will receive a unique color assignment through out every graph. |
||||||
|
*/ |
||||||
|
private static class ColorMap { |
||||||
|
|
||||||
|
private ArrayMap<Integer, Integer> mColorMap = new ArrayMap<>(); |
||||||
|
private int mColorsAssigned = 0; |
||||||
|
/** |
||||||
|
* Source of Kelly's contrasting colors: |
||||||
|
* https://medium.com/@rjurney/kellys-22-colours-of-maximum-contrast-58edb70c90d1
|
||||||
|
*/ |
||||||
|
private static final String[] CONTRASTING_COLORS = { |
||||||
|
"#222222", "#F3C300", "#875692", "#F38400", "#A1CAF1", "#BE0032", "#C2B280", "#848482", |
||||||
|
"#008856", "#E68FAC", "#0067A5", "#F99379", "#604E97", "#F6A600", "#B3446C", "#DCD300", |
||||||
|
"#882D17", "#8DB600", "#654522", "#E25822", "#2B3D26" |
||||||
|
}; |
||||||
|
private final Random mRandom = new Random(); |
||||||
|
|
||||||
|
private int getColor(int svId, int constellationType) { |
||||||
|
// Assign the color from Kelly's 21 contrasting colors to satellites first, if all color
|
||||||
|
// has been assigned, use a random color and record in {@link mColorMap}.
|
||||||
|
if (mColorMap.containsKey(constellationType * 1000 + svId)) { |
||||||
|
return mColorMap.get(getUniqueSatelliteIdentifier(constellationType, svId)); |
||||||
|
} |
||||||
|
if (this.mColorsAssigned < CONTRASTING_COLORS.length) { |
||||||
|
int color = Color.parseColor(CONTRASTING_COLORS[mColorsAssigned++]); |
||||||
|
mColorMap.put(getUniqueSatelliteIdentifier(constellationType, svId), color); |
||||||
|
return color; |
||||||
|
} |
||||||
|
int color = Color.argb(255, mRandom.nextInt(256), mRandom.nextInt(256), mRandom.nextInt(256)); |
||||||
|
mColorMap.put(getUniqueSatelliteIdentifier(constellationType, svId), color); |
||||||
|
return color; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
private static int getUniqueSatelliteIdentifier(int constellationType, int svID){ |
||||||
|
return constellationType * 1000 + svID; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* An utility class stores and maintains all the data sets and corresponding renders. |
||||||
|
* We use 0 as the {@code dataSetIndex} of all constellations and 1 - 6 as the |
||||||
|
* {@code dataSetIndex} of each satellite constellations |
||||||
|
*/ |
||||||
|
private static class DataSetManager { |
||||||
|
/** The Y min and max of each plot */ |
||||||
|
private static final int[][] RENDER_HEIGHTS = {{5, 45}, {-60, 60}}; |
||||||
|
/** |
||||||
|
* <ul> |
||||||
|
* <li>A list of constellation prefix</li> |
||||||
|
* <li>G : GPS, US Constellation</li> |
||||||
|
* <li>S : Satellite-based Augmentation System</li> |
||||||
|
* <li>R : GLONASS, Russia Constellation</li> |
||||||
|
* <li>J : QZSS, Japan Constellation</li> |
||||||
|
* <li>C : BEIDOU China Constellation</li> |
||||||
|
* <li>E : GALILEO EU Constellation</li> |
||||||
|
* </ul> |
||||||
|
*/ |
||||||
|
private static final String[] CONSTELLATION_PREFIX = {"G", "S", "R", "J", "C", "E"}; |
||||||
|
|
||||||
|
private final List<ArrayMap<Integer, Integer>>[] mSatelliteIndex; |
||||||
|
private final List<ArrayMap<Integer, Integer>>[] mSatelliteConstellationIndex; |
||||||
|
private final List<XYMultipleSeriesDataset>[] mDataSetList; |
||||||
|
private final List<XYMultipleSeriesRenderer>[] mRendererList; |
||||||
|
private final Context mContext; |
||||||
|
private final ColorMap mColorMap; |
||||||
|
|
||||||
|
public DataSetManager(int numberOfTabs, int numberOfConstellations, |
||||||
|
Context context, ColorMap colorMap) { |
||||||
|
mDataSetList = new ArrayList[numberOfTabs]; |
||||||
|
mRendererList = new ArrayList[numberOfTabs]; |
||||||
|
mSatelliteIndex = new ArrayList[numberOfTabs]; |
||||||
|
mSatelliteConstellationIndex = new ArrayList[numberOfTabs]; |
||||||
|
mContext = context; |
||||||
|
mColorMap = colorMap; |
||||||
|
|
||||||
|
// Preparing data sets and renderer for all six constellations
|
||||||
|
for (int i = 0; i < numberOfTabs; i++) { |
||||||
|
mDataSetList[i] = new ArrayList<>(); |
||||||
|
mRendererList[i] = new ArrayList<>(); |
||||||
|
mSatelliteIndex[i] = new ArrayList<>(); |
||||||
|
mSatelliteConstellationIndex[i] = new ArrayList<>(); |
||||||
|
for (int k = 0; k <= numberOfConstellations; k++) { |
||||||
|
mSatelliteIndex[i].add(new ArrayMap<Integer, Integer>()); |
||||||
|
mSatelliteConstellationIndex[i].add(new ArrayMap<Integer, Integer>()); |
||||||
|
XYMultipleSeriesRenderer tempRenderer = new XYMultipleSeriesRenderer(); |
||||||
|
setUpRenderer(tempRenderer, i); |
||||||
|
mRendererList[i].add(tempRenderer); |
||||||
|
XYMultipleSeriesDataset tempDataSet = new XYMultipleSeriesDataset(); |
||||||
|
mDataSetList[i].add(tempDataSet); |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
// The constellation type should range from 1 to 6
|
||||||
|
private String getConstellationPrefix(int constellationType) { |
||||||
|
if (constellationType <= GnssStatus.CONSTELLATION_UNKNOWN |
||||||
|
|| constellationType > NUMBER_OF_CONSTELLATIONS) { |
||||||
|
return ""; |
||||||
|
} |
||||||
|
return CONSTELLATION_PREFIX[constellationType - 1]; |
||||||
|
} |
||||||
|
|
||||||
|
/** Returns the multiple series data set at specific tab and index */ |
||||||
|
private XYMultipleSeriesDataset getDataSet(int tab, int dataSetIndex) { |
||||||
|
return mDataSetList[tab].get(dataSetIndex); |
||||||
|
} |
||||||
|
|
||||||
|
/** Returns the multiple series renderer set at specific tab and index */ |
||||||
|
private XYMultipleSeriesRenderer getRenderer(int tab, int dataSetIndex) { |
||||||
|
return mRendererList[tab].get(dataSetIndex); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Adds a value into the both the data set containing all constellations and individual data set |
||||||
|
* of the constellation of the satellite |
||||||
|
*/ |
||||||
|
private void addValue(int tab, int constellationType, int svID, |
||||||
|
double timeInSeconds, double value) { |
||||||
|
XYMultipleSeriesDataset dataSetAll = getDataSet(tab, DATA_SET_INDEX_ALL); |
||||||
|
XYMultipleSeriesRenderer rendererAll = getRenderer(tab, DATA_SET_INDEX_ALL); |
||||||
|
value = Double.parseDouble(sDataFormat.format(value)); |
||||||
|
if (hasSeen(constellationType, svID, tab)) { |
||||||
|
// If the satellite has been seen before, we retrieve the dataseries it is add and add new
|
||||||
|
// data
|
||||||
|
dataSetAll |
||||||
|
.getSeriesAt(mSatelliteIndex[tab].get(constellationType).get(svID)) |
||||||
|
.add(timeInSeconds, value); |
||||||
|
mDataSetList[tab] |
||||||
|
.get(constellationType) |
||||||
|
.getSeriesAt(mSatelliteConstellationIndex[tab].get(constellationType).get(svID)) |
||||||
|
.add(timeInSeconds, value); |
||||||
|
} else { |
||||||
|
// If the satellite has not been seen before, we create new dataset and renderer before
|
||||||
|
// adding data
|
||||||
|
mSatelliteIndex[tab].get(constellationType).put(svID, dataSetAll.getSeriesCount()); |
||||||
|
mSatelliteConstellationIndex[tab] |
||||||
|
.get(constellationType) |
||||||
|
.put(svID, mDataSetList[tab].get(constellationType).getSeriesCount()); |
||||||
|
XYSeries tempSeries = new XYSeries(CONSTELLATION_PREFIX[constellationType - 1] + svID); |
||||||
|
tempSeries.add(timeInSeconds, value); |
||||||
|
dataSetAll.addSeries(tempSeries); |
||||||
|
mDataSetList[tab].get(constellationType).addSeries(tempSeries); |
||||||
|
XYSeriesRenderer tempRenderer = new XYSeriesRenderer(); |
||||||
|
tempRenderer.setLineWidth(5); |
||||||
|
tempRenderer.setColor(mColorMap.getColor(svID, constellationType)); |
||||||
|
rendererAll.addSeriesRenderer(tempRenderer); |
||||||
|
mRendererList[tab].get(constellationType).addSeriesRenderer(tempRenderer); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Creates a discontinuity of the satellites that has been seen but not reported in this batch |
||||||
|
* of measurements |
||||||
|
*/ |
||||||
|
private void fillInDiscontinuity(int tab, double referenceTimeSeconds) { |
||||||
|
for (XYMultipleSeriesDataset dataSet : mDataSetList[tab]) { |
||||||
|
for (int i = 0; i < dataSet.getSeriesCount(); i++) { |
||||||
|
if (dataSet.getSeriesAt(i).getMaxX() < referenceTimeSeconds) { |
||||||
|
dataSet.getSeriesAt(i).add(referenceTimeSeconds, MathHelper.NULL_VALUE); |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Returns a boolean indicating whether the input satellite has been seen. |
||||||
|
*/ |
||||||
|
private boolean hasSeen(int constellationType, int svID, int tab) { |
||||||
|
return mSatelliteIndex[tab].get(constellationType).containsKey(svID); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Set up a {@link XYMultipleSeriesRenderer} with the specs customized per plot tab. |
||||||
|
*/ |
||||||
|
private void setUpRenderer(XYMultipleSeriesRenderer renderer, int tabNumber) { |
||||||
|
renderer.setXAxisMin(0); |
||||||
|
renderer.setXAxisMax(60); |
||||||
|
renderer.setYAxisMin(RENDER_HEIGHTS[tabNumber][0]); |
||||||
|
renderer.setYAxisMax(RENDER_HEIGHTS[tabNumber][1]); |
||||||
|
renderer.setYAxisAlign(Align.RIGHT, 0); |
||||||
|
renderer.setLegendTextSize(30); |
||||||
|
renderer.setLabelsTextSize(30); |
||||||
|
renderer.setYLabelsColor(0, Color.BLACK); |
||||||
|
renderer.setXLabelsColor(Color.BLACK); |
||||||
|
renderer.setFitLegend(true); |
||||||
|
renderer.setShowGridX(true); |
||||||
|
renderer.setMargins(new int[] {10, 10, 30, 10}); |
||||||
|
// setting the plot untouchable
|
||||||
|
renderer.setZoomEnabled(false, false); |
||||||
|
renderer.setPanEnabled(false, true); |
||||||
|
renderer.setClickEnabled(false); |
||||||
|
renderer.setMarginsColor(Color.WHITE); |
||||||
|
renderer.setChartTitle(mContext.getResources() |
||||||
|
.getStringArray(R.array.plot_titles)[tabNumber]); |
||||||
|
renderer.setChartTitleTextSize(50); |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,544 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.android.apps.location.gps.gnsslogger; |
||||||
|
|
||||||
|
import android.graphics.Color; |
||||||
|
import android.location.GnssMeasurementsEvent; |
||||||
|
import android.location.GnssNavigationMessage; |
||||||
|
import android.location.GnssStatus; |
||||||
|
import android.location.Location; |
||||||
|
import android.location.LocationManager; |
||||||
|
import android.os.Bundle; |
||||||
|
import android.os.Handler; |
||||||
|
import android.os.HandlerThread; |
||||||
|
import android.util.Log; |
||||||
|
import com.google.android.apps.location.gps.gnsslogger.ResultFragment.UIResultComponent; |
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.GpsMathOperations; |
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.GpsNavigationMessageStore; |
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.PseudorangePositionVelocityFromRealTimeEvents; |
||||||
|
import java.text.DecimalFormat; |
||||||
|
import java.util.concurrent.TimeUnit; |
||||||
|
|
||||||
|
/** |
||||||
|
* A class that handles real time position and velocity calculation, passing {@link |
||||||
|
* GnssMeasurementsEvent} instances to the {@link PseudorangePositionVelocityFromRealTimeEvents} |
||||||
|
* whenever a new raw measurement is received in order to compute a new position solution. The |
||||||
|
* computed position and velocity solutions are passed to the {@link ResultFragment} to be |
||||||
|
* visualized. |
||||||
|
*/ |
||||||
|
public class RealTimePositionVelocityCalculator implements GnssListener { |
||||||
|
/** Residual analysis where user disabled residual plots */ |
||||||
|
public static final int RESIDUAL_MODE_DISABLED = -1; |
||||||
|
|
||||||
|
/** Residual analysis where the user is not moving */ |
||||||
|
public static final int RESIDUAL_MODE_STILL = 0; |
||||||
|
|
||||||
|
/** Residual analysis where the user is moving */ |
||||||
|
public static final int RESIDUAL_MODE_MOVING = 1; |
||||||
|
|
||||||
|
/** |
||||||
|
* Residual analysis where the user chose to enter a LLA input as their position |
||||||
|
*/ |
||||||
|
public static final int RESIDUAL_MODE_AT_INPUT_LOCATION = 2; |
||||||
|
|
||||||
|
private static final long EARTH_RADIUS_METERS = 6371000; |
||||||
|
private PseudorangePositionVelocityFromRealTimeEvents |
||||||
|
mPseudorangePositionVelocityFromRealTimeEvents; |
||||||
|
private HandlerThread mPositionVelocityCalculationHandlerThread; |
||||||
|
private Handler mMyPositionVelocityCalculationHandler; |
||||||
|
private int mCurrentColor = Color.rgb(0x4a, 0x5f, 0x70); |
||||||
|
private int mCurrentColorIndex = 0; |
||||||
|
private boolean mAllowShowingRawResults = false; |
||||||
|
private MapFragment mMapFragement; |
||||||
|
private MainActivity mMainActivity; |
||||||
|
private PlotFragment mPlotFragment; |
||||||
|
private int[] mRgbColorArray = { |
||||||
|
Color.rgb(0x4a, 0x5f, 0x70), |
||||||
|
Color.rgb(0x7f, 0x82, 0x5f), |
||||||
|
Color.rgb(0xbf, 0x90, 0x76), |
||||||
|
Color.rgb(0x82, 0x4e, 0x4e), |
||||||
|
Color.rgb(0x66, 0x77, 0x7d) |
||||||
|
}; |
||||||
|
private int mResidualPlotStatus; |
||||||
|
private double[] mGroundTruth = null; |
||||||
|
private int mPositionSolutionCount = 0; |
||||||
|
|
||||||
|
public RealTimePositionVelocityCalculator() { |
||||||
|
mPositionVelocityCalculationHandlerThread = |
||||||
|
new HandlerThread("Position From Realtime Pseudoranges"); |
||||||
|
mPositionVelocityCalculationHandlerThread.start(); |
||||||
|
mMyPositionVelocityCalculationHandler = |
||||||
|
new Handler(mPositionVelocityCalculationHandlerThread.getLooper()); |
||||||
|
|
||||||
|
final Runnable r = |
||||||
|
new Runnable() { |
||||||
|
@Override |
||||||
|
public void run() { |
||||||
|
try { |
||||||
|
mPseudorangePositionVelocityFromRealTimeEvents = |
||||||
|
new PseudorangePositionVelocityFromRealTimeEvents(); |
||||||
|
} catch (Exception e) { |
||||||
|
Log.e( |
||||||
|
GnssContainer.TAG, |
||||||
|
" Exception in constructing PseudorangePositionFromRealTimeEvents : ", |
||||||
|
e); |
||||||
|
} |
||||||
|
} |
||||||
|
}; |
||||||
|
|
||||||
|
mMyPositionVelocityCalculationHandler.post(r); |
||||||
|
} |
||||||
|
|
||||||
|
private UIResultComponent uiResultComponent; |
||||||
|
|
||||||
|
public synchronized UIResultComponent getUiResultComponent() { |
||||||
|
return uiResultComponent; |
||||||
|
} |
||||||
|
|
||||||
|
public synchronized void setUiResultComponent(UIResultComponent value) { |
||||||
|
uiResultComponent = value; |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onProviderEnabled(String provider) {} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onProviderDisabled(String provider) {} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onGnssStatusChanged(GnssStatus gnssStatus) {} |
||||||
|
|
||||||
|
/** |
||||||
|
* Update the reference location in {@link PseudorangePositionVelocityFromRealTimeEvents} if the |
||||||
|
* received location is a network location. Otherwise, update the {@link ResultFragment} to |
||||||
|
* visualize both GPS location computed by the device and the one computed from the raw data. |
||||||
|
*/ |
||||||
|
@Override |
||||||
|
public void onLocationChanged(final Location location) { |
||||||
|
if (location.getProvider().equals(LocationManager.NETWORK_PROVIDER)) { |
||||||
|
final Runnable r = |
||||||
|
new Runnable() { |
||||||
|
@Override |
||||||
|
public void run() { |
||||||
|
if (mPseudorangePositionVelocityFromRealTimeEvents == null) { |
||||||
|
return; |
||||||
|
} |
||||||
|
try { |
||||||
|
mPseudorangePositionVelocityFromRealTimeEvents.setReferencePosition( |
||||||
|
(int) (location.getLatitude() * 1E7), |
||||||
|
(int) (location.getLongitude() * 1E7), |
||||||
|
(int) (location.getAltitude() * 1E7)); |
||||||
|
} catch (Exception e) { |
||||||
|
Log.e(GnssContainer.TAG, " Exception setting reference location : ", e); |
||||||
|
} |
||||||
|
} |
||||||
|
}; |
||||||
|
|
||||||
|
mMyPositionVelocityCalculationHandler.post(r); |
||||||
|
|
||||||
|
} else if (location.getProvider().equals(LocationManager.GPS_PROVIDER)) { |
||||||
|
if (mAllowShowingRawResults) { |
||||||
|
final Runnable r = |
||||||
|
new Runnable() { |
||||||
|
@Override |
||||||
|
public void run() { |
||||||
|
if (mPseudorangePositionVelocityFromRealTimeEvents == null) { |
||||||
|
return; |
||||||
|
} |
||||||
|
double[] posSolution = |
||||||
|
mPseudorangePositionVelocityFromRealTimeEvents.getPositionSolutionLatLngDeg(); |
||||||
|
double[] velSolution = |
||||||
|
mPseudorangePositionVelocityFromRealTimeEvents.getVelocitySolutionEnuMps(); |
||||||
|
double[] pvUncertainty = |
||||||
|
mPseudorangePositionVelocityFromRealTimeEvents |
||||||
|
.getPositionVelocityUncertaintyEnu(); |
||||||
|
if (Double.isNaN(posSolution[0])) { |
||||||
|
logPositionFromRawDataEvent("No Position Calculated Yet"); |
||||||
|
logPositionError("And no offset calculated yet..."); |
||||||
|
} else { |
||||||
|
if (mResidualPlotStatus != RESIDUAL_MODE_DISABLED |
||||||
|
&& mResidualPlotStatus != RESIDUAL_MODE_AT_INPUT_LOCATION) { |
||||||
|
updateGroundTruth(posSolution); |
||||||
|
} |
||||||
|
String formattedLatDegree = new DecimalFormat("##.######").format(posSolution[0]); |
||||||
|
String formattedLngDegree = new DecimalFormat("##.######").format(posSolution[1]); |
||||||
|
String formattedAltMeters = new DecimalFormat("##.#").format(posSolution[2]); |
||||||
|
logPositionFromRawDataEvent( |
||||||
|
"latDegrees = " |
||||||
|
+ formattedLatDegree |
||||||
|
+ " lngDegrees = " |
||||||
|
+ formattedLngDegree |
||||||
|
+ "altMeters = " |
||||||
|
+ formattedAltMeters); |
||||||
|
String formattedVelocityEastMps = |
||||||
|
new DecimalFormat("##.###").format(velSolution[0]); |
||||||
|
String formattedVelocityNorthMps = |
||||||
|
new DecimalFormat("##.###").format(velSolution[1]); |
||||||
|
String formattedVelocityUpMps = |
||||||
|
new DecimalFormat("##.###").format(velSolution[2]); |
||||||
|
logVelocityFromRawDataEvent( |
||||||
|
"Velocity East = " |
||||||
|
+ formattedVelocityEastMps |
||||||
|
+ "mps" |
||||||
|
+ " Velocity North = " |
||||||
|
+ formattedVelocityNorthMps |
||||||
|
+ "mps" |
||||||
|
+ "Velocity Up = " |
||||||
|
+ formattedVelocityUpMps |
||||||
|
+ "mps"); |
||||||
|
|
||||||
|
String formattedPosUncertaintyEastMeters = |
||||||
|
new DecimalFormat("##.###").format(pvUncertainty[0]); |
||||||
|
String formattedPosUncertaintyNorthMeters = |
||||||
|
new DecimalFormat("##.###").format(pvUncertainty[1]); |
||||||
|
String formattedPosUncertaintyUpMeters = |
||||||
|
new DecimalFormat("##.###").format(pvUncertainty[2]); |
||||||
|
logPositionUncertainty( |
||||||
|
"East = " |
||||||
|
+ formattedPosUncertaintyEastMeters |
||||||
|
+ "m North = " |
||||||
|
+ formattedPosUncertaintyNorthMeters |
||||||
|
+ "m Up = " |
||||||
|
+ formattedPosUncertaintyUpMeters |
||||||
|
+ "m"); |
||||||
|
String formattedVelUncertaintyEastMeters = |
||||||
|
new DecimalFormat("##.###").format(pvUncertainty[3]); |
||||||
|
String formattedVelUncertaintyNorthMeters = |
||||||
|
new DecimalFormat("##.###").format(pvUncertainty[4]); |
||||||
|
String formattedVelUncertaintyUpMeters = |
||||||
|
new DecimalFormat("##.###").format(pvUncertainty[5]); |
||||||
|
logVelocityUncertainty( |
||||||
|
"East = " |
||||||
|
+ formattedVelUncertaintyEastMeters |
||||||
|
+ "mps North = " |
||||||
|
+ formattedVelUncertaintyNorthMeters |
||||||
|
+ "mps Up = " |
||||||
|
+ formattedVelUncertaintyUpMeters |
||||||
|
+ "mps"); |
||||||
|
String formattedOffsetMeters = |
||||||
|
new DecimalFormat("##.######") |
||||||
|
.format( |
||||||
|
getDistanceMeters( |
||||||
|
location.getLatitude(), |
||||||
|
location.getLongitude(), |
||||||
|
posSolution[0], |
||||||
|
posSolution[1])); |
||||||
|
logPositionError("position offset = " + formattedOffsetMeters + " meters"); |
||||||
|
String formattedSpeedOffsetMps = |
||||||
|
new DecimalFormat("##.###") |
||||||
|
.format( |
||||||
|
Math.abs( |
||||||
|
location.getSpeed() |
||||||
|
- Math.sqrt( |
||||||
|
Math.pow(velSolution[0], 2) |
||||||
|
+ Math.pow(velSolution[1], 2)))); |
||||||
|
logVelocityError("speed offset = " + formattedSpeedOffsetMps + " mps"); |
||||||
|
} |
||||||
|
logLocationEvent("onLocationChanged: " + location); |
||||||
|
if (!Double.isNaN(posSolution[0])) { |
||||||
|
updateMapViewWithPostions( |
||||||
|
posSolution[0], |
||||||
|
posSolution[1], |
||||||
|
location.getLatitude(), |
||||||
|
location.getLongitude(), |
||||||
|
location.getTime()); |
||||||
|
} else { |
||||||
|
clearMapMarkers(); |
||||||
|
} |
||||||
|
} |
||||||
|
}; |
||||||
|
mMyPositionVelocityCalculationHandler.post(r); |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
private void clearMapMarkers() { |
||||||
|
mMapFragement.clearMarkers(); |
||||||
|
} |
||||||
|
|
||||||
|
private void updateMapViewWithPostions( |
||||||
|
double latDegRaw, |
||||||
|
double lngDegRaw, |
||||||
|
double latDegDevice, |
||||||
|
double lngDegDevice, |
||||||
|
long timeMillis) { |
||||||
|
mMapFragement.updateMapViewWithPostions( |
||||||
|
latDegRaw, lngDegRaw, latDegDevice, lngDegDevice, timeMillis); |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onLocationStatusChanged(String provider, int status, Bundle extras) {} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onGnssMeasurementsReceived(final GnssMeasurementsEvent event) { |
||||||
|
mAllowShowingRawResults = true; |
||||||
|
final Runnable r = |
||||||
|
new Runnable() { |
||||||
|
@Override |
||||||
|
public void run() { |
||||||
|
mMainActivity.runOnUiThread( |
||||||
|
new Runnable() { |
||||||
|
@Override |
||||||
|
public void run() { |
||||||
|
mPlotFragment.updateCnoTab(event); |
||||||
|
} |
||||||
|
}); |
||||||
|
if (mPseudorangePositionVelocityFromRealTimeEvents == null) { |
||||||
|
return; |
||||||
|
} |
||||||
|
try { |
||||||
|
if (mResidualPlotStatus != RESIDUAL_MODE_DISABLED |
||||||
|
&& mResidualPlotStatus != RESIDUAL_MODE_AT_INPUT_LOCATION) { |
||||||
|
// The position at last epoch is used for the residual analysis.
|
||||||
|
// This is happening by updating the ground truth for pseudorange before using the
|
||||||
|
// new arriving pseudoranges to compute a new position.
|
||||||
|
mPseudorangePositionVelocityFromRealTimeEvents |
||||||
|
.setCorrectedResidualComputationTruthLocationLla(mGroundTruth); |
||||||
|
} |
||||||
|
mPseudorangePositionVelocityFromRealTimeEvents |
||||||
|
.computePositionVelocitySolutionsFromRawMeas(event); |
||||||
|
// Running on main thread instead of in parallel will improve the thread safety
|
||||||
|
if (mResidualPlotStatus != RESIDUAL_MODE_DISABLED) { |
||||||
|
mMainActivity.runOnUiThread( |
||||||
|
new Runnable() { |
||||||
|
@Override |
||||||
|
public void run() { |
||||||
|
mPlotFragment.updatePseudorangeResidualTab( |
||||||
|
mPseudorangePositionVelocityFromRealTimeEvents |
||||||
|
.getPseudorangeResidualsMeters(), |
||||||
|
TimeUnit.NANOSECONDS.toSeconds( |
||||||
|
event.getClock().getTimeNanos())); |
||||||
|
} |
||||||
|
} |
||||||
|
); |
||||||
|
} else { |
||||||
|
mMainActivity.runOnUiThread( |
||||||
|
new Runnable() { |
||||||
|
@Override |
||||||
|
public void run() { |
||||||
|
// Here we create gaps when the residual plot is disabled
|
||||||
|
mPlotFragment.updatePseudorangeResidualTab( |
||||||
|
GpsMathOperations.createAndFillArray( |
||||||
|
GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES, Double.NaN), |
||||||
|
TimeUnit.NANOSECONDS.toSeconds( |
||||||
|
event.getClock().getTimeNanos())); |
||||||
|
} |
||||||
|
} |
||||||
|
); |
||||||
|
} |
||||||
|
} catch (Exception e) { |
||||||
|
e.printStackTrace(); |
||||||
|
} |
||||||
|
} |
||||||
|
}; |
||||||
|
mMyPositionVelocityCalculationHandler.post(r); |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onGnssMeasurementsStatusChanged(int status) {} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onGnssNavigationMessageReceived(GnssNavigationMessage event) { |
||||||
|
if (event.getType() == GnssNavigationMessage.TYPE_GPS_L1CA) { |
||||||
|
mPseudorangePositionVelocityFromRealTimeEvents.parseHwNavigationMessageUpdates(event); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onGnssNavigationMessageStatusChanged(int status) {} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onNmeaReceived(long l, String s) {} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onListenerRegistration(String listener, boolean result) {} |
||||||
|
|
||||||
|
private void logEvent(String tag, String message, int color) { |
||||||
|
String composedTag = GnssContainer.TAG + tag; |
||||||
|
Log.d(composedTag, message); |
||||||
|
logText(tag, message, color); |
||||||
|
} |
||||||
|
|
||||||
|
private void logText(String tag, String text, int color) { |
||||||
|
UIResultComponent component = getUiResultComponent(); |
||||||
|
if (component != null) { |
||||||
|
component.logTextResults(tag, text, color); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
public void logLocationEvent(String event) { |
||||||
|
mCurrentColor = getNextColor(); |
||||||
|
logEvent("Location", event, mCurrentColor); |
||||||
|
} |
||||||
|
|
||||||
|
private void logPositionFromRawDataEvent(String event) { |
||||||
|
logEvent("Calculated Position From Raw Data", event + "\n", mCurrentColor); |
||||||
|
} |
||||||
|
|
||||||
|
private void logVelocityFromRawDataEvent(String event) { |
||||||
|
logEvent("Calculated Velocity From Raw Data", event + "\n", mCurrentColor); |
||||||
|
} |
||||||
|
|
||||||
|
private void logPositionError(String event) { |
||||||
|
logEvent( |
||||||
|
"Offset between the reported position and Google's WLS position based on reported " |
||||||
|
+ "measurements", |
||||||
|
event + "\n", |
||||||
|
mCurrentColor); |
||||||
|
} |
||||||
|
|
||||||
|
private void logVelocityError(String event) { |
||||||
|
logEvent( |
||||||
|
"Offset between the reported velocity and " |
||||||
|
+ "Google's computed velocity based on reported measurements ", |
||||||
|
event + "\n", |
||||||
|
mCurrentColor); |
||||||
|
} |
||||||
|
|
||||||
|
private void logPositionUncertainty(String event) { |
||||||
|
logEvent("Uncertainty of the calculated position from Raw Data", event + "\n", mCurrentColor); |
||||||
|
} |
||||||
|
|
||||||
|
private void logVelocityUncertainty(String event) { |
||||||
|
logEvent("Uncertainty of the calculated velocity from Raw Data", event + "\n", mCurrentColor); |
||||||
|
} |
||||||
|
|
||||||
|
private synchronized int getNextColor() { |
||||||
|
++mCurrentColorIndex; |
||||||
|
return mRgbColorArray[mCurrentColorIndex % mRgbColorArray.length]; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Return the distance (measured along the surface of the sphere) between 2 points |
||||||
|
*/ |
||||||
|
public double getDistanceMeters( |
||||||
|
double lat1Degree, double lng1Degree, double lat2Degree, double lng2Degree) { |
||||||
|
|
||||||
|
double deltaLatRadian = Math.toRadians(lat2Degree - lat1Degree); |
||||||
|
double deltaLngRadian = Math.toRadians(lng2Degree - lng1Degree); |
||||||
|
|
||||||
|
double a = |
||||||
|
Math.sin(deltaLatRadian / 2) * Math.sin(deltaLatRadian / 2) |
||||||
|
+ Math.cos(Math.toRadians(lat1Degree)) |
||||||
|
* Math.cos(Math.toRadians(lat2Degree)) |
||||||
|
* Math.sin(deltaLngRadian / 2) |
||||||
|
* Math.sin(deltaLngRadian / 2); |
||||||
|
double angularDistanceRad = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a)); |
||||||
|
|
||||||
|
return EARTH_RADIUS_METERS * angularDistanceRad; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Update the ground truth for pseudorange residual analysis based on the user activity. |
||||||
|
*/ |
||||||
|
private synchronized void updateGroundTruth(double[] posSolution) { |
||||||
|
|
||||||
|
// In case of switching between modes, last ground truth from previous mode will be used.
|
||||||
|
if (mGroundTruth == null) { |
||||||
|
// If mGroundTruth has not been initialized, we set it to be the same as position solution
|
||||||
|
mGroundTruth = new double[] {0.0, 0.0, 0.0}; |
||||||
|
mGroundTruth[0] = posSolution[0]; |
||||||
|
mGroundTruth[1] = posSolution[1]; |
||||||
|
mGroundTruth[2] = posSolution[2]; |
||||||
|
} else if (mResidualPlotStatus == RESIDUAL_MODE_STILL) { |
||||||
|
// If the user is standing still, we average our WLS position solution
|
||||||
|
// Reference: https://en.wikipedia.org/wiki/Moving_average#Cumulative_moving_average
|
||||||
|
mGroundTruth[0] = |
||||||
|
(mGroundTruth[0] * mPositionSolutionCount + posSolution[0]) |
||||||
|
/ (mPositionSolutionCount + 1); |
||||||
|
mGroundTruth[1] = |
||||||
|
(mGroundTruth[1] * mPositionSolutionCount + posSolution[1]) |
||||||
|
/ (mPositionSolutionCount + 1); |
||||||
|
mGroundTruth[2] = |
||||||
|
(mGroundTruth[2] * mPositionSolutionCount + posSolution[2]) |
||||||
|
/ (mPositionSolutionCount + 1); |
||||||
|
mPositionSolutionCount++; |
||||||
|
} else if (mResidualPlotStatus == RESIDUAL_MODE_MOVING) { |
||||||
|
// If the user is moving fast, we use single WLS position solution
|
||||||
|
mGroundTruth[0] = posSolution[0]; |
||||||
|
mGroundTruth[1] = posSolution[1]; |
||||||
|
mGroundTruth[2] = posSolution[2]; |
||||||
|
mPositionSolutionCount = 0; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Sets {@link MapFragment} for receiving WLS location update |
||||||
|
*/ |
||||||
|
public void setMapFragment(MapFragment mapFragement) { |
||||||
|
this.mMapFragement = mapFragement; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Sets {@link PlotFragment} for receiving Gnss measurement and residual computation results for |
||||||
|
* plot |
||||||
|
*/ |
||||||
|
public void setPlotFragment(PlotFragment plotFragment) { |
||||||
|
this.mPlotFragment = plotFragment; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Sets {@link MainActivity} for running some UI tasks on UI thread |
||||||
|
*/ |
||||||
|
public void setMainActivity(MainActivity mainActivity) { |
||||||
|
this.mMainActivity = mainActivity; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Sets the ground truth mode in {@link PseudorangePositionVelocityFromRealTimeEvents} |
||||||
|
* for calculating corrected pseudorange residuals, also logs the change in ResultFragment |
||||||
|
*/ |
||||||
|
public void setResidualPlotMode(int residualPlotStatus, double[] fixedGroundTruth) { |
||||||
|
mResidualPlotStatus = residualPlotStatus; |
||||||
|
if (mPseudorangePositionVelocityFromRealTimeEvents == null) { |
||||||
|
return; |
||||||
|
} |
||||||
|
switch (mResidualPlotStatus) { |
||||||
|
case RESIDUAL_MODE_MOVING: |
||||||
|
mPseudorangePositionVelocityFromRealTimeEvents |
||||||
|
.setCorrectedResidualComputationTruthLocationLla(mGroundTruth); |
||||||
|
logEvent("Residual Plot", "Mode is set to moving", mCurrentColor); |
||||||
|
break; |
||||||
|
|
||||||
|
case RESIDUAL_MODE_STILL: |
||||||
|
mPseudorangePositionVelocityFromRealTimeEvents |
||||||
|
.setCorrectedResidualComputationTruthLocationLla(mGroundTruth); |
||||||
|
logEvent("Residual Plot", "Mode is set to still", mCurrentColor); |
||||||
|
break; |
||||||
|
|
||||||
|
case RESIDUAL_MODE_AT_INPUT_LOCATION: |
||||||
|
mPseudorangePositionVelocityFromRealTimeEvents |
||||||
|
.setCorrectedResidualComputationTruthLocationLla(fixedGroundTruth); |
||||||
|
logEvent("Residual Plot", "Mode is set to fixed ground truth", mCurrentColor); |
||||||
|
break; |
||||||
|
|
||||||
|
case RESIDUAL_MODE_DISABLED: |
||||||
|
mGroundTruth = null; |
||||||
|
mPseudorangePositionVelocityFromRealTimeEvents |
||||||
|
.setCorrectedResidualComputationTruthLocationLla(mGroundTruth); |
||||||
|
logEvent("Residual Plot", "Mode is set to Disabled", mCurrentColor); |
||||||
|
break; |
||||||
|
|
||||||
|
default: |
||||||
|
mPseudorangePositionVelocityFromRealTimeEvents |
||||||
|
.setCorrectedResidualComputationTruthLocationLla(null); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onTTFFReceived(long l) {} |
||||||
|
} |
||||||
@ -0,0 +1,143 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.android.apps.location.gps.gnsslogger; |
||||||
|
|
||||||
|
import android.app.Activity; |
||||||
|
import android.app.Fragment; |
||||||
|
import android.content.Intent; |
||||||
|
import android.content.SharedPreferences; |
||||||
|
import android.os.Bundle; |
||||||
|
import android.preference.PreferenceManager; |
||||||
|
import android.text.Editable; |
||||||
|
import android.text.SpannableStringBuilder; |
||||||
|
import android.text.style.ForegroundColorSpan; |
||||||
|
import android.view.LayoutInflater; |
||||||
|
import android.view.View; |
||||||
|
import android.view.View.OnClickListener; |
||||||
|
import android.view.ViewGroup; |
||||||
|
import android.widget.Button; |
||||||
|
import android.widget.ScrollView; |
||||||
|
import android.widget.TextView; |
||||||
|
|
||||||
|
/** The UI fragment that hosts a logging view. */ |
||||||
|
public class ResultFragment extends Fragment { |
||||||
|
|
||||||
|
private TextView mLogView; |
||||||
|
private ScrollView mScrollView; |
||||||
|
|
||||||
|
private RealTimePositionVelocityCalculator mPositionVelocityCalculator; |
||||||
|
|
||||||
|
private final UIResultComponent mUiComponent = new UIResultComponent(); |
||||||
|
|
||||||
|
public void setPositionVelocityCalculator(RealTimePositionVelocityCalculator value) { |
||||||
|
mPositionVelocityCalculator = value; |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public View onCreateView( |
||||||
|
LayoutInflater inflater, ViewGroup container, Bundle savedInstanceState) { |
||||||
|
View newView = inflater.inflate(R.layout.results_log, container, false /* attachToRoot */); |
||||||
|
mLogView = (TextView) newView.findViewById(R.id.log_view); |
||||||
|
mScrollView = (ScrollView) newView.findViewById(R.id.log_scroll); |
||||||
|
|
||||||
|
RealTimePositionVelocityCalculator currentPositionVelocityCalculator = |
||||||
|
mPositionVelocityCalculator; |
||||||
|
if (currentPositionVelocityCalculator != null) { |
||||||
|
currentPositionVelocityCalculator.setUiResultComponent(mUiComponent); |
||||||
|
} |
||||||
|
|
||||||
|
Button start = (Button) newView.findViewById(R.id.start_log); |
||||||
|
start.setOnClickListener( |
||||||
|
new OnClickListener() { |
||||||
|
@Override |
||||||
|
public void onClick(View view) { |
||||||
|
mScrollView.fullScroll(View.FOCUS_UP); |
||||||
|
} |
||||||
|
}); |
||||||
|
|
||||||
|
Button end = (Button) newView.findViewById(R.id.end_log); |
||||||
|
end.setOnClickListener( |
||||||
|
new OnClickListener() { |
||||||
|
@Override |
||||||
|
public void onClick(View view) { |
||||||
|
mScrollView.fullScroll(View.FOCUS_DOWN); |
||||||
|
} |
||||||
|
}); |
||||||
|
|
||||||
|
Button clear = (Button) newView.findViewById(R.id.clear_log); |
||||||
|
clear.setOnClickListener( |
||||||
|
new OnClickListener() { |
||||||
|
@Override |
||||||
|
public void onClick(View view) { |
||||||
|
mLogView.setText(""); |
||||||
|
} |
||||||
|
}); |
||||||
|
return newView; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* A facade for UI and Activity related operations that are required for {@link GnssListener}s. |
||||||
|
*/ |
||||||
|
public class UIResultComponent { |
||||||
|
|
||||||
|
private static final int MAX_LENGTH = 12000; |
||||||
|
private static final int LOWER_THRESHOLD = (int) (MAX_LENGTH * 0.5); |
||||||
|
|
||||||
|
public synchronized void logTextResults(final String tag, final String text, int color) { |
||||||
|
final SpannableStringBuilder builder = new SpannableStringBuilder(); |
||||||
|
builder.append(tag).append(" | ").append(text).append("\n"); |
||||||
|
|
||||||
|
builder.setSpan( |
||||||
|
new ForegroundColorSpan(color), |
||||||
|
0 /* start */, |
||||||
|
builder.length(), |
||||||
|
SpannableStringBuilder.SPAN_INCLUSIVE_EXCLUSIVE); |
||||||
|
|
||||||
|
Activity activity = getActivity(); |
||||||
|
if (activity == null) { |
||||||
|
return; |
||||||
|
} |
||||||
|
activity.runOnUiThread( |
||||||
|
new Runnable() { |
||||||
|
@Override |
||||||
|
public void run() { |
||||||
|
mLogView.append(builder); |
||||||
|
SharedPreferences sharedPreferences = PreferenceManager. |
||||||
|
getDefaultSharedPreferences(getActivity()); |
||||||
|
Editable editable = mLogView.getEditableText(); |
||||||
|
int length = editable.length(); |
||||||
|
if (length > MAX_LENGTH) { |
||||||
|
editable.delete(0, length - LOWER_THRESHOLD); |
||||||
|
} |
||||||
|
if (sharedPreferences.getBoolean( |
||||||
|
SettingsFragment.PREFERENCE_KEY_AUTO_SCROLL, false /*default return value*/)){ |
||||||
|
mScrollView.post(new Runnable() { |
||||||
|
@Override |
||||||
|
public void run() { |
||||||
|
mScrollView.fullScroll(View.FOCUS_DOWN); |
||||||
|
} |
||||||
|
}); |
||||||
|
} |
||||||
|
} |
||||||
|
}); |
||||||
|
} |
||||||
|
|
||||||
|
public void startActivity(Intent intent) { |
||||||
|
getActivity().startActivity(intent); |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,99 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.android.apps.location.gps.gnsslogger; |
||||||
|
|
||||||
|
import static com.google.common.base.Preconditions.checkState; |
||||||
|
|
||||||
|
import android.app.Activity; |
||||||
|
import android.app.AlertDialog; |
||||||
|
import android.app.Dialog; |
||||||
|
import android.app.DialogFragment; |
||||||
|
import android.content.DialogInterface; |
||||||
|
import android.os.Bundle; |
||||||
|
import android.view.View; |
||||||
|
import android.widget.NumberPicker; |
||||||
|
import com.google.android.apps.location.gps.gnsslogger.TimerService.TimerListener; |
||||||
|
|
||||||
|
/** A {@link Dialog} allowing "Hours", "Minutes", and "Seconds" to be selected for a timer */ |
||||||
|
public class TimerFragment extends DialogFragment { |
||||||
|
private TimerListener mListener; |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onAttach(Activity activity) { |
||||||
|
super.onAttach(activity); |
||||||
|
|
||||||
|
checkState( |
||||||
|
getTargetFragment() instanceof TimerListener, |
||||||
|
"Target fragment is not instance of TimerListener"); |
||||||
|
|
||||||
|
mListener = (TimerListener) getTargetFragment(); |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public Dialog onCreateDialog(Bundle savedInstanceState) { |
||||||
|
AlertDialog.Builder builder = new AlertDialog.Builder(getActivity()); |
||||||
|
|
||||||
|
View view = getActivity().getLayoutInflater().inflate(R.layout.timer, null); |
||||||
|
final NumberPicker timerHours = (NumberPicker) view.findViewById(R.id.hours_picker); |
||||||
|
final NumberPicker timerMinutes = (NumberPicker) view.findViewById(R.id.minutes_picker); |
||||||
|
final NumberPicker timerSeconds = (NumberPicker) view.findViewById(R.id.seconds_picker); |
||||||
|
|
||||||
|
final TimerValues values; |
||||||
|
|
||||||
|
if (getArguments() != null) { |
||||||
|
values = TimerValues.fromBundle(getArguments()); |
||||||
|
} else { |
||||||
|
values = new TimerValues(0 /* hours */, 0 /* minutes */, 0 /* seconds */); |
||||||
|
} |
||||||
|
|
||||||
|
values.configureHours(timerHours); |
||||||
|
values.configureMinutes(timerMinutes); |
||||||
|
values.configureSeconds(timerSeconds); |
||||||
|
|
||||||
|
builder.setTitle(R.string.timer_title); |
||||||
|
builder.setView(view); |
||||||
|
builder.setPositiveButton( |
||||||
|
R.string.timer_set, |
||||||
|
new DialogInterface.OnClickListener() { |
||||||
|
@Override |
||||||
|
public void onClick(DialogInterface dialog, int id) { |
||||||
|
mListener.processTimerValues( |
||||||
|
new TimerValues( |
||||||
|
timerHours.getValue(), timerMinutes.getValue(), timerSeconds.getValue())); |
||||||
|
} |
||||||
|
}); |
||||||
|
builder.setNeutralButton( |
||||||
|
R.string.timer_cancel, |
||||||
|
new DialogInterface.OnClickListener() { |
||||||
|
@Override |
||||||
|
public void onClick(DialogInterface dialog, int id) { |
||||||
|
mListener.processTimerValues(values); |
||||||
|
} |
||||||
|
}); |
||||||
|
builder.setNegativeButton( |
||||||
|
R.string.timer_reset, |
||||||
|
new DialogInterface.OnClickListener() { |
||||||
|
@Override |
||||||
|
public void onClick(DialogInterface dialog, int id) { |
||||||
|
mListener.processTimerValues( |
||||||
|
new TimerValues(0 /* hours */, 0 /* minutes */, 0 /* seconds */)); |
||||||
|
} |
||||||
|
}); |
||||||
|
|
||||||
|
return builder.create(); |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,119 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.android.apps.location.gps.gnsslogger; |
||||||
|
|
||||||
|
import android.app.Notification; |
||||||
|
import android.app.Service; |
||||||
|
import android.content.Intent; |
||||||
|
import android.os.Binder; |
||||||
|
import android.os.CountDownTimer; |
||||||
|
import android.os.IBinder; |
||||||
|
import android.support.v4.content.LocalBroadcastManager; |
||||||
|
import java.util.concurrent.TimeUnit; |
||||||
|
|
||||||
|
/** A {@link Service} to be bound to that exposes a timer. */ |
||||||
|
public class TimerService extends Service { |
||||||
|
static final String TIMER_ACTION = |
||||||
|
String.format("%s.TIMER_UPDATE", TimerService.class.getPackage().getName()); |
||||||
|
static final String EXTRA_KEY_TYPE = "type"; |
||||||
|
static final String EXTRA_KEY_UPDATE_REMAINING = "remaining"; |
||||||
|
static final byte TYPE_UNKNOWN = -1; |
||||||
|
static final byte TYPE_UPDATE = 0; |
||||||
|
static final byte TYPE_FINISH = 1; |
||||||
|
static final int NOTIFICATION_ID = 7777; |
||||||
|
|
||||||
|
private final IBinder mBinder = new TimerBinder(); |
||||||
|
private CountDownTimer mCountDownTimer; |
||||||
|
private boolean mTimerStarted; |
||||||
|
|
||||||
|
/** Handles response from {@link TimerFragment} */ |
||||||
|
public interface TimerListener { |
||||||
|
/** |
||||||
|
* Process a {@link TimerValues} result |
||||||
|
* |
||||||
|
* @param values The set {@link TimerValues} |
||||||
|
*/ |
||||||
|
public void processTimerValues(TimerValues values); |
||||||
|
} |
||||||
|
|
||||||
|
/** A {@link Binder} that exposes a {@link TimerService}. */ |
||||||
|
public class TimerBinder extends Binder { |
||||||
|
TimerService getService() { |
||||||
|
return TimerService.this; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onCreate() { |
||||||
|
mTimerStarted = false; |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public IBinder onBind(Intent intent) { |
||||||
|
Notification notification = new Notification(); |
||||||
|
startForeground(NOTIFICATION_ID, notification); |
||||||
|
return mBinder; |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onDestroy() { |
||||||
|
if (mCountDownTimer != null) { |
||||||
|
mCountDownTimer.cancel(); |
||||||
|
} |
||||||
|
mTimerStarted = false; |
||||||
|
} |
||||||
|
|
||||||
|
void setTimer(TimerValues values) { |
||||||
|
// Only allow setting when not already running
|
||||||
|
if (!mTimerStarted) { |
||||||
|
mCountDownTimer = |
||||||
|
new CountDownTimer( |
||||||
|
values.getTotalMilliseconds(), |
||||||
|
TimeUnit.MILLISECONDS.convert(1, TimeUnit.SECONDS) /* countDownInterval */) { |
||||||
|
@Override |
||||||
|
public void onTick(long millisUntilFinished) { |
||||||
|
Intent broadcast = new Intent(TIMER_ACTION); |
||||||
|
broadcast.putExtra(EXTRA_KEY_TYPE, TYPE_UPDATE); |
||||||
|
broadcast.putExtra(EXTRA_KEY_UPDATE_REMAINING, millisUntilFinished); |
||||||
|
LocalBroadcastManager.getInstance(TimerService.this).sendBroadcast(broadcast); |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public void onFinish() { |
||||||
|
mTimerStarted = false; |
||||||
|
Intent broadcast = new Intent(TIMER_ACTION); |
||||||
|
broadcast.putExtra(EXTRA_KEY_TYPE, TYPE_FINISH); |
||||||
|
LocalBroadcastManager.getInstance(TimerService.this).sendBroadcast(broadcast); |
||||||
|
} |
||||||
|
}; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
void startTimer() { |
||||||
|
if ((mCountDownTimer != null) && !mTimerStarted) { |
||||||
|
mCountDownTimer.start(); |
||||||
|
mTimerStarted = true; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
void stopTimer() { |
||||||
|
if (mCountDownTimer != null) { |
||||||
|
mCountDownTimer.cancel(); |
||||||
|
mTimerStarted = false; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,153 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.android.apps.location.gps.gnsslogger; |
||||||
|
|
||||||
|
import static com.google.common.base.Preconditions.checkArgument; |
||||||
|
|
||||||
|
import android.os.Bundle; |
||||||
|
import android.widget.NumberPicker; |
||||||
|
import java.util.concurrent.TimeUnit; |
||||||
|
|
||||||
|
/** A represetation of a time as "hours:minutes:seconds" */ |
||||||
|
public class TimerValues { |
||||||
|
private static final String EMPTY = "N/A"; |
||||||
|
private static final String HOURS = "hours"; |
||||||
|
private static final String MINUTES = "minutes"; |
||||||
|
private static final String SECONDS = "seconds"; |
||||||
|
private int mHours; |
||||||
|
private int mMinutes; |
||||||
|
private int mSeconds; |
||||||
|
|
||||||
|
/** |
||||||
|
* Creates a {@link TimerValues} |
||||||
|
* |
||||||
|
* @param hours The number of hours to represent |
||||||
|
* @param minutes The number of minutes to represent |
||||||
|
* @param seconds The number of seconds to represent |
||||||
|
*/ |
||||||
|
public TimerValues(int hours, int minutes, int seconds) { |
||||||
|
checkArgument(hours >= 0, "Hours is negative: %s", hours); |
||||||
|
checkArgument(minutes >= 0, "Minutes is negative: %s", minutes); |
||||||
|
checkArgument(seconds >= 0, "Seconds is negative: %s", seconds); |
||||||
|
|
||||||
|
mHours = hours; |
||||||
|
mMinutes = minutes; |
||||||
|
mSeconds = seconds; |
||||||
|
|
||||||
|
normalizeValues(); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Creates a {@link TimerValues} |
||||||
|
* |
||||||
|
* @param milliseconds The number of milliseconds to represent |
||||||
|
*/ |
||||||
|
public TimerValues(long milliseconds) { |
||||||
|
this( |
||||||
|
0 /* hours */, |
||||||
|
0 /* minutes */, |
||||||
|
(int) TimeUnit.SECONDS.convert(milliseconds, TimeUnit.MILLISECONDS)); |
||||||
|
} |
||||||
|
|
||||||
|
/** Creates a {@link TimerValues} from a {@link Bundle} */ |
||||||
|
public static TimerValues fromBundle(Bundle bundle) { |
||||||
|
checkArgument(bundle != null, "Bundle is null"); |
||||||
|
|
||||||
|
return new TimerValues( |
||||||
|
bundle.getInt(HOURS, 0), bundle.getInt(MINUTES, 0), bundle.getInt(SECONDS, 0)); |
||||||
|
} |
||||||
|
|
||||||
|
/** Returns a {@link Bundle} from the {@link TimerValues} */ |
||||||
|
public Bundle toBundle() { |
||||||
|
Bundle content = new Bundle(); |
||||||
|
content.putInt(HOURS, mHours); |
||||||
|
content.putInt(MINUTES, mMinutes); |
||||||
|
content.putInt(SECONDS, mSeconds); |
||||||
|
|
||||||
|
return content; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Configures a {@link NumberPicker} with appropriate bounds and initial value for displaying |
||||||
|
* "Hours" |
||||||
|
*/ |
||||||
|
public void configureHours(NumberPicker picker) { |
||||||
|
picker.setMinValue(0); |
||||||
|
picker.setMaxValue((int) TimeUnit.HOURS.convert(1, TimeUnit.DAYS) - 1); |
||||||
|
picker.setValue(mHours); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Configures a {@link NumberPicker} with appropriate bounds and initial value for displaying |
||||||
|
* "Minutes" |
||||||
|
*/ |
||||||
|
public void configureMinutes(NumberPicker picker) { |
||||||
|
picker.setMinValue(0); |
||||||
|
picker.setMaxValue((int) TimeUnit.MINUTES.convert(1, TimeUnit.HOURS) - 1); |
||||||
|
picker.setValue(mMinutes); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Configures a {@link NumberPicker} with appropriate bounds and initial value for displaying |
||||||
|
* "Seconds" |
||||||
|
*/ |
||||||
|
public void configureSeconds(NumberPicker picker) { |
||||||
|
picker.setMinValue(0); |
||||||
|
picker.setMaxValue((int) TimeUnit.SECONDS.convert(1, TimeUnit.MINUTES) - 1); |
||||||
|
picker.setValue(mSeconds); |
||||||
|
} |
||||||
|
|
||||||
|
/** Returns the {@link TimerValues} in milliseconds */ |
||||||
|
public long getTotalMilliseconds() { |
||||||
|
return (TimeUnit.MILLISECONDS.convert(mHours, TimeUnit.HOURS) |
||||||
|
+ TimeUnit.MILLISECONDS.convert(mMinutes, TimeUnit.MINUTES) |
||||||
|
+ TimeUnit.MILLISECONDS.convert(mSeconds, TimeUnit.SECONDS)); |
||||||
|
} |
||||||
|
|
||||||
|
/** Returns {@code true} if {@link TimerValues} is zero. */ |
||||||
|
public boolean isZero() { |
||||||
|
return ((mHours == 0) && (mMinutes == 0) && (mSeconds == 0)); |
||||||
|
} |
||||||
|
|
||||||
|
/** Returns string representation that includes "00:00:00" */ |
||||||
|
public String toCountdownString() { |
||||||
|
return String.format("%02d:%02d:%02d", mHours, mMinutes, mSeconds); |
||||||
|
} |
||||||
|
|
||||||
|
/** Normalize seconds and minutes */ |
||||||
|
private void normalizeValues() { |
||||||
|
long minuteOverflow = TimeUnit.MINUTES.convert(mSeconds, TimeUnit.SECONDS); |
||||||
|
long hourOverflow = TimeUnit.HOURS.convert(mMinutes, TimeUnit.MINUTES); |
||||||
|
|
||||||
|
// Apply overflow
|
||||||
|
mMinutes += minuteOverflow; |
||||||
|
mHours += hourOverflow; |
||||||
|
|
||||||
|
// Apply bounds
|
||||||
|
mSeconds -= TimeUnit.SECONDS.convert(minuteOverflow, TimeUnit.MINUTES); |
||||||
|
mMinutes -= TimeUnit.MINUTES.convert(hourOverflow, TimeUnit.HOURS); |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public String toString() { |
||||||
|
if (isZero()) { |
||||||
|
return EMPTY; |
||||||
|
} else { |
||||||
|
return toCountdownString(); |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
After Width: | Height: | Size: 1.8 KiB |
@ -0,0 +1,85 @@ |
|||||||
|
<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android" |
||||||
|
xmlns:tools="http://schemas.android.com/tools" |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="match_parent" |
||||||
|
android:orientation="vertical" |
||||||
|
android:paddingLeft="@dimen/activity_horizontal_margin" |
||||||
|
android:paddingRight="@dimen/activity_horizontal_margin" |
||||||
|
android:paddingTop="@dimen/activity_vertical_margin" |
||||||
|
android:paddingBottom="@dimen/activity_vertical_margin" |
||||||
|
tools:context=".MainActivity"> |
||||||
|
|
||||||
|
<LinearLayout |
||||||
|
android:orientation="horizontal" |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="wrap_content"> |
||||||
|
<Button |
||||||
|
android:layout_width="0dp" |
||||||
|
android:layout_weight="1" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:text="@string/clearAgps" |
||||||
|
android:layout_marginTop="0dp" |
||||||
|
android:id="@+id/clearAgps" |
||||||
|
android:singleLine="true" /> |
||||||
|
<Button |
||||||
|
android:layout_width="0dp" |
||||||
|
android:layout_weight="1" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:text="@string/fetchExtraData" |
||||||
|
android:layout_marginTop="0dp" |
||||||
|
android:id="@+id/fetchExtraData" |
||||||
|
android:singleLine="true" /> |
||||||
|
<Button |
||||||
|
android:layout_width="0dp" |
||||||
|
android:layout_weight="1" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:text="@string/fetchTimeData" |
||||||
|
android:layout_marginTop="0dp" |
||||||
|
android:id="@+id/fetchTimeData" |
||||||
|
android:singleLine="true" /> |
||||||
|
</LinearLayout> |
||||||
|
|
||||||
|
<LinearLayout |
||||||
|
android:orientation="horizontal" |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="wrap_content"> |
||||||
|
<Button |
||||||
|
android:layout_width="0dp" |
||||||
|
android:layout_weight="1" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:text="@string/requestSingleNlp" |
||||||
|
android:layout_marginTop="0dp" |
||||||
|
android:id="@+id/requestSingleNlp" |
||||||
|
android:textSize="10.5sp" |
||||||
|
android:singleLine="true" /> |
||||||
|
<Button |
||||||
|
android:id="@+id/clear_log" |
||||||
|
android:layout_marginTop="0dp" |
||||||
|
android:text="Clear" |
||||||
|
android:layout_width="0dp" |
||||||
|
android:layout_weight="1" |
||||||
|
android:layout_height="wrap_content" /> |
||||||
|
<Button |
||||||
|
android:layout_width="0dp" |
||||||
|
android:layout_weight="1" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:text="@string/requestSingleGps" |
||||||
|
android:layout_marginTop="0dp" |
||||||
|
android:id="@+id/requestSingleGps" |
||||||
|
android:singleLine="true" /> |
||||||
|
</LinearLayout> |
||||||
|
|
||||||
|
<ScrollView |
||||||
|
android:id="@+id/log_scroll" |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="0dp" |
||||||
|
android:layout_weight="1"> |
||||||
|
|
||||||
|
<TextView |
||||||
|
android:id="@+id/log_view" |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="wrap_content" /> |
||||||
|
|
||||||
|
</ScrollView> |
||||||
|
|
||||||
|
</LinearLayout> |
||||||
@ -0,0 +1,38 @@ |
|||||||
|
<?xml version="1.0" encoding="utf-8"?> |
||||||
|
<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android" |
||||||
|
android:orientation="vertical" |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="match_parent"> |
||||||
|
<LinearLayout |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="0dp" |
||||||
|
android:orientation="vertical" |
||||||
|
android:id="@+id/plot" |
||||||
|
android:layout_weight="3"> |
||||||
|
</LinearLayout> |
||||||
|
<LinearLayout |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:orientation = "horizontal"> |
||||||
|
<Spinner |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:gravity="start" |
||||||
|
android:id = "@+id/constellation_spinner" |
||||||
|
android:entries="@array/constellation_arrays"> |
||||||
|
</Spinner> |
||||||
|
<Spinner |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:gravity="end" |
||||||
|
android:id = "@+id/tab_spinner" |
||||||
|
android:entries="@array/tab_arrays"> |
||||||
|
</Spinner> |
||||||
|
</LinearLayout> |
||||||
|
<TextView |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="0dp" |
||||||
|
android:id="@+id/analysis" |
||||||
|
android:text="@string/turn_on_location_measurement" |
||||||
|
android:layout_weight="1"/> |
||||||
|
</LinearLayout> |
||||||
@ -0,0 +1,12 @@ |
|||||||
|
<?xml version="1.0" encoding="utf-8"?> |
||||||
|
<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android" |
||||||
|
android:orientation="vertical" |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="match_parent"> |
||||||
|
<com.google.android.gms.maps.MapView |
||||||
|
android:id="@+id/map" |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="0dp" |
||||||
|
android:layout_weight="9" /> |
||||||
|
|
||||||
|
</LinearLayout> |
||||||
@ -0,0 +1,65 @@ |
|||||||
|
<?xml version="1.0" encoding="utf-8"?> |
||||||
|
<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android" |
||||||
|
android:orientation="vertical" |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:background="@color/background_material_light" |
||||||
|
android:id="@+id/pop"> |
||||||
|
<TextView |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:textSize="@dimen/abc_text_size_title_material_toolbar" |
||||||
|
android:textColor="@color/background_material_dark" |
||||||
|
android:text="@string/please_select_ground_truth_mode"/> |
||||||
|
<LinearLayout |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="wrap_content"> |
||||||
|
<TextView |
||||||
|
android:layout_width="0dp" |
||||||
|
android:layout_weight="1" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:gravity="start" |
||||||
|
android:text="@string/ground_solution_mode"/> |
||||||
|
<Spinner |
||||||
|
android:layout_width="0dp" |
||||||
|
android:layout_weight="1" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:gravity="end" |
||||||
|
android:id="@+id/residual_spinner" |
||||||
|
android:entries="@array/residual_options"> |
||||||
|
</Spinner> |
||||||
|
</LinearLayout> |
||||||
|
<EditText |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:id="@+id/latitude_input" |
||||||
|
android:inputType="numberDecimal|numberSigned" |
||||||
|
android:hint="@string/latitude_in_degrees"/> |
||||||
|
<EditText |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:id="@+id/longitude_input" |
||||||
|
android:inputType="numberDecimal|numberSigned" |
||||||
|
android:hint="@string/longitude_in_degrees"/> |
||||||
|
<EditText |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:id="@+id/altitude_input" |
||||||
|
android:inputType="numberDecimal|numberSigned" |
||||||
|
android:hint="@string/altitude_in_meters"/> |
||||||
|
<LinearLayout |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:orientation="horizontal"> |
||||||
|
<Button |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:id="@+id/popup_button_ok" |
||||||
|
android:text="@string/ok"/> |
||||||
|
<Button |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:id="@+id/popup_button_cancel" |
||||||
|
android:text="@string/cancel"/> |
||||||
|
</LinearLayout> |
||||||
|
</LinearLayout> |
||||||
@ -0,0 +1,46 @@ |
|||||||
|
<?xml version="1.0" encoding="utf-8"?> |
||||||
|
<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android" |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="match_parent" |
||||||
|
android:orientation="vertical" > |
||||||
|
|
||||||
|
<LinearLayout |
||||||
|
android:orientation="horizontal" |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="wrap_content"> |
||||||
|
|
||||||
|
<Button |
||||||
|
android:id="@+id/start_log" |
||||||
|
android:text="\u2770 Start" |
||||||
|
android:layout_width="0dp" |
||||||
|
android:layout_weight="1" |
||||||
|
android:layout_height="wrap_content" /> |
||||||
|
<Button |
||||||
|
android:id="@+id/clear_log" |
||||||
|
android:text="Clear" |
||||||
|
android:layout_width="0dp" |
||||||
|
android:layout_weight="1" |
||||||
|
android:layout_height="wrap_content" /> |
||||||
|
<Button |
||||||
|
android:id="@+id/end_log" |
||||||
|
android:text="End \u2771" |
||||||
|
android:layout_width="0dp" |
||||||
|
android:layout_weight="1" |
||||||
|
android:layout_height="wrap_content" /> |
||||||
|
|
||||||
|
</LinearLayout> |
||||||
|
|
||||||
|
<ScrollView |
||||||
|
android:id="@+id/log_scroll" |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="0dp" |
||||||
|
android:layout_weight="1"> |
||||||
|
|
||||||
|
<TextView |
||||||
|
android:id="@+id/log_view" |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="wrap_content" /> |
||||||
|
|
||||||
|
</ScrollView> |
||||||
|
|
||||||
|
</LinearLayout> |
||||||
@ -0,0 +1,69 @@ |
|||||||
|
<?xml version="1.0" encoding="utf-8"?> |
||||||
|
<RelativeLayout xmlns:android="http://schemas.android.com/apk/res/android" |
||||||
|
android:orientation="vertical" |
||||||
|
android:layout_width="match_parent" |
||||||
|
android:layout_height="match_parent"> |
||||||
|
|
||||||
|
<!-- Center landmark --> |
||||||
|
<RelativeLayout |
||||||
|
android:id="@+id/minutes_container" |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:layout_centerVertical="true" |
||||||
|
android:layout_centerHorizontal="true"> |
||||||
|
<NumberPicker |
||||||
|
android:id="@+id/minutes_picker" |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:layout_centerVertical="true"/> |
||||||
|
<TextView |
||||||
|
android:id="@+id/minutes_text" |
||||||
|
android:text="@string/timer_minutes" |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:layout_centerHorizontal="true" |
||||||
|
android:layout_below="@id/minutes_picker"/> |
||||||
|
|
||||||
|
</RelativeLayout> |
||||||
|
|
||||||
|
<RelativeLayout |
||||||
|
android:id="@+id/hours_container" |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:layout_toLeftOf="@id/minutes_container"> |
||||||
|
<NumberPicker |
||||||
|
android:id="@+id/hours_picker" |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:layout_centerVertical="true"/> |
||||||
|
<TextView |
||||||
|
android:id="@+id/hours_text" |
||||||
|
android:text="@string/timer_hours" |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:layout_centerHorizontal="true" |
||||||
|
android:layout_below="@id/hours_picker"/> |
||||||
|
|
||||||
|
</RelativeLayout> |
||||||
|
|
||||||
|
<RelativeLayout |
||||||
|
android:id="@+id/seconds_container" |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:layout_toRightOf="@+id/minutes_container"> |
||||||
|
<NumberPicker |
||||||
|
android:id="@+id/seconds_picker" |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:layout_centerVertical="true"/> |
||||||
|
<TextView |
||||||
|
android:id="@+id/seconds_text" |
||||||
|
android:text="@string/timer_seconds" |
||||||
|
android:layout_width="wrap_content" |
||||||
|
android:layout_height="wrap_content" |
||||||
|
android:layout_centerHorizontal="true" |
||||||
|
android:layout_below="@+id/seconds_picker"/> |
||||||
|
|
||||||
|
</RelativeLayout> |
||||||
|
|
||||||
|
</RelativeLayout> |
||||||
|
Before Width: | Height: | Size: 3.3 KiB After Width: | Height: | Size: 2.5 KiB |
|
Before Width: | Height: | Size: 2.1 KiB After Width: | Height: | Size: 1.7 KiB |
|
Before Width: | Height: | Size: 4.7 KiB After Width: | Height: | Size: 3.5 KiB |
|
Before Width: | Height: | Size: 7.5 KiB After Width: | Height: | Size: 5.8 KiB |
|
Before Width: | Height: | Size: 10 KiB After Width: | Height: | Size: 8.5 KiB |
@ -1,19 +1,82 @@ |
|||||||
<?xml version="1.0" encoding="utf-8"?> |
<?xml version="1.0" encoding="utf-8"?> |
||||||
<resources> |
<resources xmlns:xliff="urn:oasis:names:tc:xliff:document:1.2"> |
||||||
|
|
||||||
<string name="app_name">GnssLogger</string> |
<string name="app_name">GnssLogger</string> |
||||||
<string name="app_version">v1.4.0.0</string> |
<string name="app_version">v2.0.0.0</string> |
||||||
|
|
||||||
<string name="title_settings">Settings</string> |
<string name="title_settings">Settings</string> |
||||||
<string name="title_log">Log</string> |
<string name="title_log">Log</string> |
||||||
|
<string name="title_offset">Position Offset</string> |
||||||
|
<string name="title_map">Map</string> |
||||||
|
<string name="title_agnss">AGNSS</string> |
||||||
<string name="title_device">Device</string> |
<string name="title_device">Device</string> |
||||||
<integer name="google_play_services_version">8487000</integer> |
<string name="title_wls">WLS</string> |
||||||
|
<string name="title_plot">Plot</string> |
||||||
|
|
||||||
<string name="location_label">Location</string> |
<string name="location_label">Location</string> |
||||||
<string name="measurements_label">Measurements</string> |
<string name="measurements_label">Measurements</string> |
||||||
<string name="nav_msg_label">Navigation Messages</string> |
<string name="nav_msg_label">Navigation Messages</string> |
||||||
<string name="gnss_status_label">GnssStatus</string> |
<string name="gnss_status_label">GnssStatus</string> |
||||||
<string name="nmea_label">Nmea</string> |
<string name="nmea_label">Nmea</string> |
||||||
|
<string name="auto_scroll">Auto Scroll</string> |
||||||
|
<string name="residual_plot">Residual Plot</string> |
||||||
|
|
||||||
<string name="help">HELP</string> |
<string name="help">HELP</string> |
||||||
<string name="exit">Exit</string> |
<string name="exit">Exit</string> |
||||||
|
<string name="clearAgps">Clear AGPS</string> |
||||||
|
<string name="fetchExtraData">Get Xtra</string> |
||||||
|
<string name="fetchTimeData">Inject Time</string> |
||||||
|
<string name="requestSingleNlp">Get Network Loc.</string> |
||||||
|
<string name="requestSingleGps">Get GPS Loc.</string> |
||||||
|
<string name="ttff">TTFF</string> |
||||||
|
|
||||||
|
<string name="timer_service_name">GNSS Logger timer</string> |
||||||
|
<string name="timer_title">Timer Settings</string> |
||||||
|
<string name="timer_hours">Hours</string> |
||||||
|
<string name="timer_minutes">Minutes</string> |
||||||
|
<string name="timer_seconds">Seconds</string> |
||||||
|
<string name="timer_set">Set</string> |
||||||
|
<string name="timer_reset">Reset</string> |
||||||
|
<string name="timer_cancel">Cancel</string> |
||||||
|
<string name="timer_display">Time Remaining</string> |
||||||
|
<string name="start_message">Starting log...</string> |
||||||
|
<string name="stop_message">Sending file...</string> |
||||||
|
<string name="longitude_in_degrees">Longitude in decimal degrees</string> |
||||||
|
<string name="ground_solution_mode">Ground Truth Mode:</string> |
||||||
|
<string name="please_select_ground_truth_mode">Please Select Ground Truth Mode</string> |
||||||
|
<string name="latitude_in_degrees">Latitude in decimal degrees</string> |
||||||
|
<string name="altitude_in_meters">Altitude in meters</string> |
||||||
|
<string name="turn_on_location_measurement"> |
||||||
|
No Measurement has been received yet. Please turn on Measurement and Location in Settings. |
||||||
|
</string> |
||||||
|
<string name="ok">OK</string> |
||||||
|
<string name="cancel">Cancel</string> |
||||||
|
<string name="current_average_hint">Current Average Of Strongest 4 Satellites: |
||||||
|
<xliff:g id="currentAverage">%s</xliff:g></string> |
||||||
|
<string name="history_average_hint">History Average of Strongest 4 Satellites: |
||||||
|
<xliff:g id="historyAverage">%s</xliff:g></string> |
||||||
|
<string name="satellite_number_sum_hint">Total Number of Visible Satellites: |
||||||
|
<xliff:g id="totalNumber">%d</xliff:g></string> |
||||||
|
<string-array name="constellation_arrays"> |
||||||
|
<item>All</item> |
||||||
|
<item>GPS</item> |
||||||
|
<item>SBAS</item> |
||||||
|
<item>GLONASS</item> |
||||||
|
<item>QZSS</item> |
||||||
|
<item>BEIDOU</item> |
||||||
|
<item>GALILEO</item> |
||||||
|
</string-array> |
||||||
|
<string-array name="tab_arrays"> |
||||||
|
<item>C/N0</item> |
||||||
|
<item>PR Residual</item> |
||||||
|
</string-array> |
||||||
|
<string-array name="residual_options"> |
||||||
|
<item>Manual - Still</item> |
||||||
|
<item>Manual - Moving</item> |
||||||
|
<item>Manual - Use LLA input</item> |
||||||
|
<item>Automatic - AR Based</item> |
||||||
|
</string-array> |
||||||
|
<string-array name="plot_titles"> |
||||||
|
<item>CN0(dB.Hz) vs Time(s)</item> |
||||||
|
<item>Pseudorange residual(m) vs Time(s)</item> |
||||||
|
</string-array> |
||||||
</resources> |
</resources> |
||||||
|
|||||||
@ -1,10 +1,12 @@ |
|||||||
## This file is automatically generated by Android Studio. |
## This file is automatically generated by Android Studio. |
||||||
# Do not modify this file -- YOUR CHANGES WILL BE ERASED! |
# Do not modify this file -- YOUR CHANGES WILL BE ERASED! |
||||||
# |
# |
||||||
# This file should *NOT* be checked into Version Control Systems, |
# This file must *NOT* be checked into Version Control Systems, |
||||||
# as it contains information specific to your local configuration. |
# as it contains information specific to your local configuration. |
||||||
# |
# |
||||||
# Location of the SDK. This is only used by Gradle. |
# Location of the SDK. This is only used by Gradle. |
||||||
# For customization when using a Version Control System, please read the |
# For customization when using a Version Control System, please read the |
||||||
# header note. |
# header note. |
||||||
sdk.dir=/media/build/master/prebuilts/fullsdk/linux |
#Mon Jul 31 15:20:15 PDT 2017 |
||||||
|
ndk.dir=/usr/local/google/home/seangao/Android/Sdk/ndk-bundle |
||||||
|
sdk.dir=/usr/local/google/home/seangao/Android/Sdk |
||||||
|
|||||||
@ -0,0 +1,53 @@ |
|||||||
|
apply plugin: 'com.android.library' |
||||||
|
|
||||||
|
android { |
||||||
|
compileSdkVersion 26 |
||||||
|
buildToolsVersion "26.0.0" |
||||||
|
useLibrary 'org.apache.http.legacy' |
||||||
|
defaultConfig { |
||||||
|
minSdkVersion 24 |
||||||
|
targetSdkVersion 25 |
||||||
|
versionCode 1 |
||||||
|
versionName "1.0" |
||||||
|
|
||||||
|
testInstrumentationRunner "android.support.test.runner.AndroidJUnitRunner" |
||||||
|
} |
||||||
|
compileOptions { |
||||||
|
sourceCompatibility JavaVersion.VERSION_1_7 |
||||||
|
targetCompatibility JavaVersion.VERSION_1_7 |
||||||
|
} |
||||||
|
dexOptions { |
||||||
|
preDexLibraries = false |
||||||
|
javaMaxHeapSize "4g" // 2g should be also OK |
||||||
|
} |
||||||
|
|
||||||
|
packagingOptions { |
||||||
|
exclude 'META-INF/DEPENDENCIES.txt' |
||||||
|
exclude 'META-INF/LICENSE.txt' |
||||||
|
exclude 'META-INF/NOTICE.txt' |
||||||
|
exclude 'META-INF/NOTICE' |
||||||
|
exclude 'META-INF/LICENSE' |
||||||
|
exclude 'META-INF/DEPENDENCIES' |
||||||
|
exclude 'META-INF/notice.txt' |
||||||
|
exclude 'META-INF/license.txt' |
||||||
|
exclude 'META-INF/dependencies.txt' |
||||||
|
exclude 'META-INF/LGPL2.1' |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
dependencies { |
||||||
|
compile fileTree(include: ['*.jar'], dir: 'libs') |
||||||
|
androidTestCompile('com.android.support.test.espresso:espresso-core:2.2.2', { |
||||||
|
exclude group: 'com.android.support', module: 'support-annotations' |
||||||
|
}) |
||||||
|
compile 'com.android.support:appcompat-v7:26.+' |
||||||
|
testCompile 'junit:junit:4.12' |
||||||
|
compile files('libs/guava-22.0-android.jar') |
||||||
|
compile files('libs/commons-math3-3.6.1.jar') |
||||||
|
compile 'com.google.android.gms:play-services-location:11.0.2' |
||||||
|
compile files('libs/commons-codec-1.10.jar') |
||||||
|
compile files('libs/asn1-supl2.jar') |
||||||
|
compile files('libs/asn1-base.jar') |
||||||
|
compile files('libs/suplClient.jar') |
||||||
|
compile files('libs/protobuf-nano.jar') |
||||||
|
} |
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -0,0 +1,4 @@ |
|||||||
|
<manifest xmlns:android="http://schemas.android.com/apk/res/android" |
||||||
|
package="com.google.location.lbs.gnss.gps.pseudorange"> |
||||||
|
|
||||||
|
</manifest> |
||||||
@ -0,0 +1,119 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
import org.apache.commons.math3.linear.Array2DRowRealMatrix; |
||||||
|
import org.apache.commons.math3.linear.RealMatrix; |
||||||
|
|
||||||
|
/** |
||||||
|
* Converts ECEF (Earth Centered Earth Fixed) Cartesian coordinates to local ENU (East, North, |
||||||
|
* and Up). |
||||||
|
* |
||||||
|
* <p> Source: reference from Navipedia: |
||||||
|
* http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
|
||||||
|
*/ |
||||||
|
|
||||||
|
public class Ecef2EnuConverter { |
||||||
|
|
||||||
|
/** |
||||||
|
* Converts a vector represented by coordinates ecefX, ecefY, ecefZ in an |
||||||
|
* Earth-Centered Earth-Fixed (ECEF) Cartesian system into a vector in a |
||||||
|
* local east-north-up (ENU) Cartesian system. |
||||||
|
* |
||||||
|
* <p> For example it can be used to rotate a speed vector or position offset vector to ENU. |
||||||
|
* |
||||||
|
* @param ecefX X coordinates in ECEF |
||||||
|
* @param ecefY Y coordinates in ECEF |
||||||
|
* @param ecefZ Z coordinates in ECEF |
||||||
|
* @param refLat Latitude in Radians of the Reference Position |
||||||
|
* @param refLng Longitude in Radians of the Reference Position |
||||||
|
* @return the converted values in {@code EnuValues} |
||||||
|
*/ |
||||||
|
public static EnuValues convertEcefToEnu(double ecefX, double ecefY, double ecefZ, |
||||||
|
double refLat, double refLng){ |
||||||
|
|
||||||
|
RealMatrix rotationMatrix = getRotationMatrix(refLat, refLng); |
||||||
|
RealMatrix ecefCoordinates = new Array2DRowRealMatrix(new double[]{ecefX, ecefY, ecefZ}); |
||||||
|
|
||||||
|
RealMatrix enuResult = rotationMatrix.multiply(ecefCoordinates); |
||||||
|
return new EnuValues(enuResult.getEntry(0, 0), |
||||||
|
enuResult.getEntry(1, 0), enuResult.getEntry(2 , 0)); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Computes a rotation matrix for converting a vector in Earth-Centered Earth-Fixed (ECEF) |
||||||
|
* Cartesian system into a vector in local east-north-up (ENU) Cartesian system with respect to |
||||||
|
* a reference location. The matrix has the following content: |
||||||
|
* |
||||||
|
* - sinLng cosLng 0 |
||||||
|
* - sinLat * cosLng - sinLat * sinLng cosLat |
||||||
|
* cosLat * cosLng cosLat * sinLng sinLat |
||||||
|
* |
||||||
|
* <p> Reference: Pratap Misra and Per Enge |
||||||
|
* "Global Positioning System: Signals, Measurements, and Performance" Page 137. |
||||||
|
* |
||||||
|
* @param refLat Latitude of reference location |
||||||
|
* @param refLng Longitude of reference location |
||||||
|
* @return the Ecef to Enu rotation matrix |
||||||
|
*/ |
||||||
|
public static RealMatrix getRotationMatrix(double refLat, double refLng){ |
||||||
|
RealMatrix rotationMatrix = new Array2DRowRealMatrix(3, 3); |
||||||
|
|
||||||
|
// Fill in the rotation Matrix
|
||||||
|
rotationMatrix.setEntry(0, 0, -1 * Math.sin(refLng)); |
||||||
|
rotationMatrix.setEntry(1, 0, -1 * Math.cos(refLng) * Math.sin(refLat)); |
||||||
|
rotationMatrix.setEntry(2, 0, Math.cos(refLng) * Math.cos(refLat)); |
||||||
|
rotationMatrix.setEntry(0, 1, Math.cos(refLng)); |
||||||
|
rotationMatrix.setEntry(1, 1, -1 * Math.sin(refLat) * Math.sin(refLng)); |
||||||
|
rotationMatrix.setEntry(2, 1, Math.cos(refLat) * Math.sin(refLng)); |
||||||
|
rotationMatrix.setEntry(0, 2, 0); |
||||||
|
rotationMatrix.setEntry(1, 2, Math.cos(refLat)); |
||||||
|
rotationMatrix.setEntry(2, 2, Math.sin(refLat)); |
||||||
|
return rotationMatrix; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* A container for values in ENU (East, North, Up) coordination system. |
||||||
|
*/ |
||||||
|
public static class EnuValues { |
||||||
|
|
||||||
|
/** |
||||||
|
* East Coordinates in local ENU |
||||||
|
*/ |
||||||
|
public final double enuEast; |
||||||
|
|
||||||
|
/** |
||||||
|
* North Coordinates in local ENU |
||||||
|
*/ |
||||||
|
public final double enuNorth; |
||||||
|
|
||||||
|
/** |
||||||
|
* Up Coordinates in local ENU |
||||||
|
*/ |
||||||
|
public final double enuUP; |
||||||
|
|
||||||
|
/** |
||||||
|
* Constructor |
||||||
|
*/ |
||||||
|
public EnuValues(double enuEast, double enuNorth, double enuUP){ |
||||||
|
this.enuEast = enuEast; |
||||||
|
this.enuNorth = enuNorth; |
||||||
|
this.enuUP = enuUP; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
@ -0,0 +1,177 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
/** |
||||||
|
* Converts ECEF (Earth Centered Earth Fixed) Cartesian coordinates to LLA (latitude, longitude, |
||||||
|
* and altitude). |
||||||
|
* |
||||||
|
* <p> Source: reference from Mathworks: https://microem.ru/files/2012/08/GPS.G1-X-00006.pdf
|
||||||
|
* http://www.mathworks.com/help/aeroblks/ecefpositiontolla.html
|
||||||
|
*/ |
||||||
|
|
||||||
|
public class Ecef2LlaConverter { |
||||||
|
// WGS84 Ellipsoid Parameters
|
||||||
|
private static final double EARTH_SEMI_MAJOR_AXIS_METERS = 6378137.0; |
||||||
|
private static final double ECCENTRICITY = 8.1819190842622e-2; |
||||||
|
private static final double INVERSE_FLATENNING = 298.257223563; |
||||||
|
private static final double MIN_MAGNITUDE_METERS = 1.0e-22; |
||||||
|
private static final double MAX_ITERATIONS = 15; |
||||||
|
private static final double RESIDUAL_TOLERANCE = 1.0e-6; |
||||||
|
private static final double SEMI_MINOR_AXIS_METERS = |
||||||
|
Math.sqrt(Math.pow(EARTH_SEMI_MAJOR_AXIS_METERS, 2) * (1 - Math.pow(ECCENTRICITY, 2))); |
||||||
|
private static final double SECOND_ECCENTRICITY = Math.sqrt( |
||||||
|
(Math.pow(EARTH_SEMI_MAJOR_AXIS_METERS, 2) - Math.pow(SEMI_MINOR_AXIS_METERS, 2)) |
||||||
|
/ Math.pow(SEMI_MINOR_AXIS_METERS, 2)); |
||||||
|
private static final double ECEF_NEAR_POLE_THRESHOLD_METERS = 1.0; |
||||||
|
|
||||||
|
/** |
||||||
|
* Converts ECEF (Earth Centered Earth Fixed) Cartesian coordinates to LLA (latitude, |
||||||
|
* longitude, and altitude) using the close form approach |
||||||
|
* |
||||||
|
* <p>Inputs are cartesian coordinates x,y,z |
||||||
|
* |
||||||
|
* <p>Output is GeodeticLlaValues class containing geodetic latitude (radians), geodetic longitude |
||||||
|
* (radians), height above WGS84 ellipsoid (m)} |
||||||
|
*/ |
||||||
|
public static GeodeticLlaValues convertECEFToLLACloseForm(double ecefXMeters, double ecefYMeters, |
||||||
|
double ecefZMeters) { |
||||||
|
|
||||||
|
// Auxiliary parameters
|
||||||
|
double pMeters = Math.sqrt(Math.pow(ecefXMeters, 2) + Math.pow(ecefYMeters, 2)); |
||||||
|
double thetaRadians = |
||||||
|
Math.atan2(EARTH_SEMI_MAJOR_AXIS_METERS * ecefZMeters, SEMI_MINOR_AXIS_METERS * pMeters); |
||||||
|
|
||||||
|
double lngRadians = Math.atan2(ecefYMeters, ecefXMeters); |
||||||
|
// limit longitude to range of 0 to 2Pi
|
||||||
|
lngRadians = lngRadians % (2 * Math.PI); |
||||||
|
|
||||||
|
final double sinTheta = Math.sin(thetaRadians); |
||||||
|
final double cosTheta = Math.cos(thetaRadians); |
||||||
|
final double tempY = ecefZMeters |
||||||
|
+ Math.pow(SECOND_ECCENTRICITY, 2) * SEMI_MINOR_AXIS_METERS * Math.pow(sinTheta, 3); |
||||||
|
final double tempX = pMeters |
||||||
|
- Math.pow(ECCENTRICITY, 2) * EARTH_SEMI_MAJOR_AXIS_METERS * (Math.pow(cosTheta, 3)); |
||||||
|
double latRadians = Math.atan2(tempY, tempX); |
||||||
|
// Radius of curvature in the vertical prime
|
||||||
|
double curvatureRadius = EARTH_SEMI_MAJOR_AXIS_METERS |
||||||
|
/ Math.sqrt(1 - Math.pow(ECCENTRICITY, 2) * (Math.pow(Math.sin(latRadians), 2))); |
||||||
|
double altMeters = (pMeters / Math.cos(latRadians)) - curvatureRadius; |
||||||
|
|
||||||
|
// Correct for numerical instability in altitude near poles
|
||||||
|
boolean polesCheck = Math.abs(ecefXMeters) < ECEF_NEAR_POLE_THRESHOLD_METERS |
||||||
|
&& Math.abs(ecefYMeters) < ECEF_NEAR_POLE_THRESHOLD_METERS; |
||||||
|
if (polesCheck) { |
||||||
|
altMeters = Math.abs(ecefZMeters) - SEMI_MINOR_AXIS_METERS; |
||||||
|
} |
||||||
|
|
||||||
|
return new GeodeticLlaValues(latRadians, lngRadians, altMeters); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Converts ECEF (Earth Centered Earth Fixed) Cartesian coordinates to LLA (latitude, |
||||||
|
* longitude, and altitude) using iteration approach |
||||||
|
* |
||||||
|
* <p>Inputs are cartesian coordinates x,y,z. |
||||||
|
* |
||||||
|
* <p>Outputs is GeodeticLlaValues containing geodetic latitude (radians), geodetic longitude |
||||||
|
* (radians), height above WGS84 ellipsoid (m)} |
||||||
|
*/ |
||||||
|
public static GeodeticLlaValues convertECEFToLLAByIterations(double ecefXMeters, |
||||||
|
double ecefYMeters, double ecefZMeters) { |
||||||
|
|
||||||
|
double xyLengthMeters = Math.sqrt(Math.pow(ecefXMeters, 2) + Math.pow(ecefYMeters, 2)); |
||||||
|
double xyzLengthMeters = Math.sqrt(Math.pow(xyLengthMeters, 2) + Math.pow(ecefZMeters, 2)); |
||||||
|
|
||||||
|
double lngRad; |
||||||
|
if (xyLengthMeters > MIN_MAGNITUDE_METERS) { |
||||||
|
lngRad = Math.atan2(ecefYMeters, ecefXMeters); |
||||||
|
} else { |
||||||
|
lngRad = 0; |
||||||
|
} |
||||||
|
|
||||||
|
double sinPhi; |
||||||
|
if (xyzLengthMeters > MIN_MAGNITUDE_METERS) { |
||||||
|
sinPhi = ecefZMeters / xyzLengthMeters; |
||||||
|
} else { |
||||||
|
sinPhi = 0; |
||||||
|
} |
||||||
|
// initial latitude (iterate next to improve accuracy)
|
||||||
|
double latRad = Math.asin(sinPhi); |
||||||
|
double altMeters; |
||||||
|
if (xyzLengthMeters > MIN_MAGNITUDE_METERS) { |
||||||
|
double ni; |
||||||
|
double pResidual; |
||||||
|
double ecefZMetersResidual; |
||||||
|
// initial height (iterate next to improve accuracy)
|
||||||
|
altMeters = xyzLengthMeters - EARTH_SEMI_MAJOR_AXIS_METERS |
||||||
|
* (1 - sinPhi * sinPhi / INVERSE_FLATENNING); |
||||||
|
|
||||||
|
for (int i = 1; i <= MAX_ITERATIONS; i++) { |
||||||
|
sinPhi = Math.sin(latRad); |
||||||
|
|
||||||
|
// calculate radius of curvature in prime vertical direction
|
||||||
|
ni = EARTH_SEMI_MAJOR_AXIS_METERS / Math.sqrt(1 - (2 - 1 / INVERSE_FLATENNING) |
||||||
|
/ INVERSE_FLATENNING * Math.sin(latRad) * Math.sin(latRad)); |
||||||
|
|
||||||
|
// calculate residuals in p and ecefZMeters
|
||||||
|
pResidual = xyLengthMeters - (ni + altMeters) * Math.cos(latRad); |
||||||
|
ecefZMetersResidual = ecefZMeters |
||||||
|
- (ni * (1 - (2 - 1 / INVERSE_FLATENNING) / INVERSE_FLATENNING) + altMeters) |
||||||
|
* Math.sin(latRad); |
||||||
|
|
||||||
|
// update height and latitude
|
||||||
|
altMeters += Math.sin(latRad) * ecefZMetersResidual + Math.cos(latRad) * pResidual; |
||||||
|
latRad += (Math.cos(latRad) * ecefZMetersResidual - Math.sin(latRad) * pResidual) |
||||||
|
/ (ni + altMeters); |
||||||
|
|
||||||
|
if (Math.sqrt((pResidual * pResidual + ecefZMetersResidual * ecefZMetersResidual)) |
||||||
|
< RESIDUAL_TOLERANCE) { |
||||||
|
break; |
||||||
|
} |
||||||
|
|
||||||
|
if (i == MAX_ITERATIONS) { |
||||||
|
System.err.println( |
||||||
|
"Geodetic coordinate calculation did not converge in " + i + " iterations"); |
||||||
|
} |
||||||
|
} |
||||||
|
} else { |
||||||
|
altMeters = 0; |
||||||
|
} |
||||||
|
return new GeodeticLlaValues(latRad, lngRad, altMeters); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* |
||||||
|
* Class containing geodetic coordinates: latitude in radians, geodetic longitude in radians |
||||||
|
* and altitude in meters |
||||||
|
*/ |
||||||
|
public static class GeodeticLlaValues { |
||||||
|
|
||||||
|
public final double latitudeRadians; |
||||||
|
public final double longitudeRadians; |
||||||
|
public final double altitudeMeters; |
||||||
|
|
||||||
|
public GeodeticLlaValues(double latitudeRadians, |
||||||
|
double longitudeRadians, double altitudeMeters) { |
||||||
|
this.latitudeRadians = latitudeRadians; |
||||||
|
this.longitudeRadians = longitudeRadians; |
||||||
|
this.altitudeMeters = altitudeMeters; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
@ -0,0 +1,107 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.Ecef2LlaConverter.GeodeticLlaValues; |
||||||
|
import org.apache.commons.math3.linear.RealMatrix; |
||||||
|
|
||||||
|
/** |
||||||
|
* Transformations from ECEF coordiantes to Topocentric coordinates |
||||||
|
*/ |
||||||
|
public class EcefToTopocentricConverter { |
||||||
|
private static final double MIN_DISTANCE_MAGNITUDE_METERS = 1.0e-22; |
||||||
|
private static final int EAST_IDX = 0; |
||||||
|
private static final int NORTH_IDX = 1; |
||||||
|
private static final int UP_IDX = 2; |
||||||
|
|
||||||
|
/** |
||||||
|
* Transformation of {@code inputVectorMeters} with origin at {@code originECEFMeters} into |
||||||
|
* topocentric coordinate system. The result is {@code TopocentricAEDValues} containing azimuth |
||||||
|
* from north +ve clockwise, radians; elevation angle, radians; distance, vector length meters |
||||||
|
* |
||||||
|
* <p>Source: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
|
||||||
|
* http://kom.aau.dk/~borre/life-l99/topocent.m
|
||||||
|
* |
||||||
|
*/ |
||||||
|
public static TopocentricAEDValues convertCartesianToTopocentericRadMeters( |
||||||
|
final double[] originECEFMeters, final double[] inputVectorMeters) { |
||||||
|
|
||||||
|
GeodeticLlaValues latLngAlt = Ecef2LlaConverter.convertECEFToLLACloseForm(originECEFMeters[0], |
||||||
|
originECEFMeters[1], originECEFMeters[2]); |
||||||
|
|
||||||
|
RealMatrix rotationMatrix = |
||||||
|
Ecef2EnuConverter. |
||||||
|
getRotationMatrix(latLngAlt.latitudeRadians, latLngAlt.longitudeRadians).transpose(); |
||||||
|
double[] eastNorthUpVectorMeters = GpsMathOperations.matrixByColVectMultiplication( |
||||||
|
rotationMatrix.transpose().getData(), inputVectorMeters); |
||||||
|
double eastMeters = eastNorthUpVectorMeters[EAST_IDX]; |
||||||
|
double northMeters = eastNorthUpVectorMeters[NORTH_IDX]; |
||||||
|
double upMeters = eastNorthUpVectorMeters[UP_IDX]; |
||||||
|
|
||||||
|
// calculate azimuth, elevation and height from the ENU values
|
||||||
|
double horizontalDistanceMeters = Math.hypot(eastMeters, northMeters); |
||||||
|
double azimuthRadians; |
||||||
|
double elevationRadians; |
||||||
|
|
||||||
|
if (horizontalDistanceMeters < MIN_DISTANCE_MAGNITUDE_METERS) { |
||||||
|
elevationRadians = Math.PI / 2.0; |
||||||
|
azimuthRadians = 0; |
||||||
|
} else { |
||||||
|
elevationRadians = Math.atan2(upMeters, horizontalDistanceMeters); |
||||||
|
azimuthRadians = Math.atan2(eastMeters, northMeters); |
||||||
|
} |
||||||
|
if (azimuthRadians < 0) { |
||||||
|
azimuthRadians += 2 * Math.PI; |
||||||
|
} |
||||||
|
|
||||||
|
double distanceMeters = Math.sqrt(Math.pow(inputVectorMeters[0], 2) |
||||||
|
+ Math.pow(inputVectorMeters[1], 2) + Math.pow(inputVectorMeters[2], 2)); |
||||||
|
return new TopocentricAEDValues(elevationRadians, azimuthRadians, distanceMeters); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculates azimuth, elevation in radians,and distance in meters between the user position in |
||||||
|
* ECEF meters {@code userPositionECEFMeters} and the satellite position in ECEF meters |
||||||
|
* {@code satPositionECEFMeters} |
||||||
|
*/ |
||||||
|
public static TopocentricAEDValues calculateElAzDistBetween2Points( |
||||||
|
double[] userPositionECEFMeters, double[] satPositionECEFMeters) { |
||||||
|
|
||||||
|
return convertCartesianToTopocentericRadMeters(userPositionECEFMeters, |
||||||
|
GpsMathOperations.subtractTwoVectors(satPositionECEFMeters, userPositionECEFMeters)); |
||||||
|
|
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* |
||||||
|
* Class containing topocenter coordinates: azimuth in radians, elevation in radians, and distance |
||||||
|
* in meters |
||||||
|
*/ |
||||||
|
public static class TopocentricAEDValues { |
||||||
|
|
||||||
|
public final double elevationRadians; |
||||||
|
public final double azimuthRadians; |
||||||
|
public final double distanceMeters; |
||||||
|
|
||||||
|
public TopocentricAEDValues(double elevationRadians, double azimuthRadians, |
||||||
|
double distanceMeters) { |
||||||
|
this.elevationRadians = elevationRadians; |
||||||
|
this.azimuthRadians = azimuthRadians; |
||||||
|
this.distanceMeters = distanceMeters; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,100 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
import static java.nio.charset.StandardCharsets.UTF_8; |
||||||
|
|
||||||
|
import com.google.common.base.Preconditions; |
||||||
|
import java.io.BufferedReader; |
||||||
|
import java.io.InputStream; |
||||||
|
import java.io.InputStreamReader; |
||||||
|
import java.net.HttpURLConnection; |
||||||
|
import java.net.URL; |
||||||
|
|
||||||
|
/** |
||||||
|
* A helper class to access the Google Elevation API for computing the Terrain Elevation Above Sea |
||||||
|
* level at a given location (lat, lng). An Elevation API key is required for getting elevation |
||||||
|
* above sea level from Google server. |
||||||
|
* |
||||||
|
* <p> For more information please see: |
||||||
|
* https://developers.google.com/maps/documentation/elevation/start
|
||||||
|
* |
||||||
|
* <p> A key can be conveniently acquired from: |
||||||
|
* https://developers.google.com/maps/documentation/elevation/get-api-key
|
||||||
|
*/ |
||||||
|
|
||||||
|
public class ElevationApiHelper { |
||||||
|
|
||||||
|
private static final String ELEVATION_XML_STRING = "<elevation>"; |
||||||
|
private static final String GOOGLE_ELEVATION_API_HTTP_ADDRESS = |
||||||
|
"https://maps.googleapis.com/maps/api/elevation/xml?locations="; |
||||||
|
private String elevationApiKey = ""; |
||||||
|
|
||||||
|
/** |
||||||
|
* A constructor that passes the {@code elevationApiKey}. If the user pass an empty string for |
||||||
|
* API Key, an {@code IllegalArgumentException} will be thrown. |
||||||
|
*/ |
||||||
|
public ElevationApiHelper(String elevationApiKey){ |
||||||
|
// An Elevation API key must be provided for getting elevation from Google Server.
|
||||||
|
Preconditions.checkArgument(!elevationApiKey.isEmpty()); |
||||||
|
this.elevationApiKey = elevationApiKey; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculates the geoid height by subtracting the elevation above sea level from the ellipsoid |
||||||
|
* height in altitude meters. |
||||||
|
*/ |
||||||
|
public static double calculateGeoidHeightMeters(double altitudeMeters, |
||||||
|
double elevationAboveSeaLevelMeters){ |
||||||
|
return altitudeMeters - elevationAboveSeaLevelMeters; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Gets elevation (height above sea level) via the Google elevation API by requesting |
||||||
|
* elevation for a given latitude and longitude. Longitude and latitude should be in decimal |
||||||
|
* degrees and the returned elevation will be in meters. |
||||||
|
*/ |
||||||
|
public double getElevationAboveSeaLevelMeters(double latitudeDegrees, |
||||||
|
double longitudeDegrees) throws Exception{ |
||||||
|
|
||||||
|
String url = |
||||||
|
GOOGLE_ELEVATION_API_HTTP_ADDRESS |
||||||
|
+ latitudeDegrees |
||||||
|
+ "," |
||||||
|
+ longitudeDegrees |
||||||
|
+ "&key=" |
||||||
|
+ elevationApiKey; |
||||||
|
String elevationMeters = "0.0"; |
||||||
|
|
||||||
|
HttpURLConnection urlConnection = (HttpURLConnection) new URL(url).openConnection(); |
||||||
|
InputStream content = urlConnection.getInputStream(); |
||||||
|
BufferedReader buffer = new BufferedReader(new InputStreamReader(content, UTF_8)); |
||||||
|
String line; |
||||||
|
while ((line = buffer.readLine()) != null) { |
||||||
|
line = line.trim(); |
||||||
|
if (line.startsWith(ELEVATION_XML_STRING)) { |
||||||
|
// read the part of the line after the opening tag <elevation>
|
||||||
|
String substring = line.substring(ELEVATION_XML_STRING.length(), line.length()); |
||||||
|
// read the part of the line until before the closing tag <elevation>
|
||||||
|
elevationMeters = |
||||||
|
substring.substring(0, substring.length() - ELEVATION_XML_STRING.length() - 1); |
||||||
|
} |
||||||
|
} |
||||||
|
return Double.parseDouble(elevationMeters); |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
@ -0,0 +1,156 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
import java.util.Arrays; |
||||||
|
|
||||||
|
/** |
||||||
|
* Helper class containing the basic vector and matrix operations used for calculating the position |
||||||
|
* solution from pseudoranges |
||||||
|
* |
||||||
|
*/ |
||||||
|
public class GpsMathOperations { |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculates the norm of a vector |
||||||
|
*/ |
||||||
|
public static double vectorNorm(double[] inputVector) { |
||||||
|
double normSqured = 0; |
||||||
|
for (int i = 0; i < inputVector.length; i++) { |
||||||
|
normSqured = Math.pow(inputVector[i], 2) + normSqured; |
||||||
|
} |
||||||
|
|
||||||
|
return Math.sqrt(normSqured); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Subtract two vectors {@code firstVector} - {@code secondVector}. Both vectors should be of the |
||||||
|
* same length. |
||||||
|
*/ |
||||||
|
public static double[] subtractTwoVectors(double[] firstVector, double[] secondVector) |
||||||
|
throws ArithmeticException { |
||||||
|
double[] result = new double[firstVector.length]; |
||||||
|
if (firstVector.length != secondVector.length) { |
||||||
|
throw new ArithmeticException("Input vectors are of different lengths"); |
||||||
|
} |
||||||
|
|
||||||
|
for (int i = 0; i < firstVector.length; i++) { |
||||||
|
result[i] = firstVector[i] - secondVector[i]; |
||||||
|
} |
||||||
|
|
||||||
|
return result; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Multiply a matrix {@code matrix} by a column vector {@code vector} |
||||||
|
* ({@code matrix} * {@code vector}) and return the resulting vector {@resultVector}. |
||||||
|
* {@code matrix} and {@resultVector} dimensions must match. |
||||||
|
*/ |
||||||
|
public static double[] matrixByColVectMultiplication(double[][] matrix, double[] resultVector) |
||||||
|
throws ArithmeticException { |
||||||
|
double[] result = new double[matrix.length]; |
||||||
|
int matrixLength = matrix.length; |
||||||
|
int vectorLength = resultVector.length; |
||||||
|
if (vectorLength != matrix[0].length) { |
||||||
|
throw new ArithmeticException("Matrix and vector dimensions do not match"); |
||||||
|
} |
||||||
|
|
||||||
|
for (int i = 0; i < matrixLength; i++) { |
||||||
|
for (int j = 0; j < vectorLength; j++) { |
||||||
|
result[i] += matrix[i][j] * resultVector[j]; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
return result; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Dot product of a raw vector {@code firstVector} and a column vector {@code secondVector}. |
||||||
|
* Both vectors should be of the same length. |
||||||
|
*/ |
||||||
|
public static double dotProduct(double[] firstVector, double[] secondVector) |
||||||
|
throws ArithmeticException { |
||||||
|
if (firstVector.length != secondVector.length) { |
||||||
|
throw new ArithmeticException("Input vectors are of different lengths"); |
||||||
|
} |
||||||
|
double result = 0; |
||||||
|
for (int i = 0; i < firstVector.length; i++) { |
||||||
|
result = firstVector[i] * secondVector[i] + result; |
||||||
|
} |
||||||
|
return result; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Finds the index of max value in a vector {@code vector} filtering out NaNs, return -1 if |
||||||
|
* the vector is empty or only contain NaNs. |
||||||
|
*/ |
||||||
|
public static int maxIndexOfVector(double[] vector) { |
||||||
|
double max = Double.NEGATIVE_INFINITY; |
||||||
|
int index = -1; |
||||||
|
|
||||||
|
for (int i = 0; i < vector.length; i++) { |
||||||
|
if (!Double.isNaN(vector[i])) { |
||||||
|
if (vector[i] > max) { |
||||||
|
index = i; |
||||||
|
max = vector[i]; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
return index; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Subtracts every element in a vector {@code vector} by a scalar {@code scalar}. We do not need |
||||||
|
* to filter out NaN in this case because NaN subtract by a real number will still be NaN. |
||||||
|
*/ |
||||||
|
public static double[] subtractByScalar(double[] vector, double scalar) { |
||||||
|
double[] result = new double[vector.length]; |
||||||
|
|
||||||
|
for (int i = 0; i < vector.length; i++) { |
||||||
|
result[i] = vector[i] - scalar; |
||||||
|
} |
||||||
|
|
||||||
|
return result; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Computes the mean value of a vector {@code vector}, filtering out NaNs. If no non-NaN exists, |
||||||
|
* return Double.NaN {@link Double#NaN} |
||||||
|
*/ |
||||||
|
public static double meanOfVector(double[] vector) { |
||||||
|
double sum = 0; |
||||||
|
double size = 0; |
||||||
|
|
||||||
|
for (int i = 0; i < vector.length; i++) { |
||||||
|
if (!Double.isNaN(vector[i])) { |
||||||
|
sum += vector[i]; |
||||||
|
size++; |
||||||
|
} |
||||||
|
} |
||||||
|
return size == 0 ? Double.NaN : sum / size; |
||||||
|
} |
||||||
|
|
||||||
|
/** Creates a numeric array of size {@code size} and fills it with the value {@code value} */ |
||||||
|
public static double[] createAndFillArray(int size, double value) { |
||||||
|
double[] vector = new double[size]; |
||||||
|
|
||||||
|
Arrays.fill(vector, value); |
||||||
|
|
||||||
|
return vector; |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,68 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
/** |
||||||
|
* A container for the received GPS measurements for a single satellite. |
||||||
|
* |
||||||
|
* <p>The GPS receiver measurements includes: satellite PRN, accumulated delta range in meters, |
||||||
|
* accumulated delta range state (boolean), pseudorange rate in meters per second, received signal |
||||||
|
* to noise ratio dB, accumulated delta range uncertainty in meters, pseudorange rate uncertainty in |
||||||
|
* meters per second. |
||||||
|
*/ |
||||||
|
class GpsMeasurement { |
||||||
|
/** Time since GPS week start (Nano seconds) */ |
||||||
|
public final long arrivalTimeSinceGpsWeekNs; |
||||||
|
|
||||||
|
/** Accumulated delta range (meters) */ |
||||||
|
public final double accumulatedDeltaRangeMeters; |
||||||
|
|
||||||
|
/** Accumulated delta range state */ |
||||||
|
public final boolean validAccumulatedDeltaRangeMeters; |
||||||
|
|
||||||
|
/** Pseudorange rate measurement (meters per second) */ |
||||||
|
public final double pseudorangeRateMps; |
||||||
|
|
||||||
|
/** Signal to noise ratio (dB) */ |
||||||
|
public final double signalToNoiseRatioDb; |
||||||
|
|
||||||
|
/** Accumulated Delta Range Uncertainty (meters) */ |
||||||
|
public final double accumulatedDeltaRangeUncertaintyMeters; |
||||||
|
|
||||||
|
/** Pseudorange rate uncertainty (meter per seconds) */ |
||||||
|
public final double pseudorangeRateUncertaintyMps; |
||||||
|
|
||||||
|
public GpsMeasurement(long arrivalTimeSinceGpsWeekNs, double accumulatedDeltaRangeMeters, |
||||||
|
boolean validAccumulatedDeltaRangeMeters, double pseudorangeRateMps, |
||||||
|
double signalToNoiseRatioDb, double accumulatedDeltaRangeUncertaintyMeters, |
||||||
|
double pseudorangeRateUncertaintyMps) { |
||||||
|
this.arrivalTimeSinceGpsWeekNs = arrivalTimeSinceGpsWeekNs; |
||||||
|
this.accumulatedDeltaRangeMeters = accumulatedDeltaRangeMeters; |
||||||
|
this.validAccumulatedDeltaRangeMeters = validAccumulatedDeltaRangeMeters; |
||||||
|
this.pseudorangeRateMps = pseudorangeRateMps; |
||||||
|
this.signalToNoiseRatioDb = signalToNoiseRatioDb; |
||||||
|
this.accumulatedDeltaRangeUncertaintyMeters = accumulatedDeltaRangeUncertaintyMeters; |
||||||
|
this.pseudorangeRateUncertaintyMps = pseudorangeRateUncertaintyMps; |
||||||
|
} |
||||||
|
|
||||||
|
protected GpsMeasurement(GpsMeasurement another) { |
||||||
|
this(another.arrivalTimeSinceGpsWeekNs, another.accumulatedDeltaRangeMeters, |
||||||
|
another.validAccumulatedDeltaRangeMeters, another.pseudorangeRateMps, |
||||||
|
another.signalToNoiseRatioDb, another.accumulatedDeltaRangeUncertaintyMeters, |
||||||
|
another.pseudorangeRateUncertaintyMps); |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,40 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
/** |
||||||
|
* A container for the received GPS measurements for a single satellite. |
||||||
|
* |
||||||
|
* <p>The container extends {@link GpsMeasurement} to additionally include |
||||||
|
* {@link #pseudorangeMeters} and {@link #pseudorangeUncertaintyMeters}. |
||||||
|
*/ |
||||||
|
class GpsMeasurementWithRangeAndUncertainty extends GpsMeasurement { |
||||||
|
|
||||||
|
/** Pseudorange measurement (meters) */ |
||||||
|
public final double pseudorangeMeters; |
||||||
|
|
||||||
|
/** Pseudorange uncertainty (meters) */ |
||||||
|
public final double pseudorangeUncertaintyMeters; |
||||||
|
|
||||||
|
public GpsMeasurementWithRangeAndUncertainty(GpsMeasurement another, double pseudorangeMeters, |
||||||
|
double pseudorangeUncertaintyMeters) { |
||||||
|
super(another); |
||||||
|
this.pseudorangeMeters = pseudorangeMeters; |
||||||
|
this.pseudorangeUncertaintyMeters = pseudorangeUncertaintyMeters; |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
@ -0,0 +1,761 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
import com.google.common.base.Preconditions; |
||||||
|
import android.location.cts.nano.Ephemeris.GpsEphemerisProto; |
||||||
|
import android.location.cts.nano.Ephemeris.GpsNavMessageProto; |
||||||
|
import android.location.cts.nano.Ephemeris.IonosphericModelProto; |
||||||
|
import android.support.annotation.NonNull; |
||||||
|
|
||||||
|
import java.util.ArrayList; |
||||||
|
import java.util.concurrent.TimeUnit; |
||||||
|
|
||||||
|
/** |
||||||
|
* A class to extract the fields of the GPS navigation message from the raw bytes received from the |
||||||
|
* GPS receiver. |
||||||
|
* |
||||||
|
* <p>Raw bytes are passed by calling the method |
||||||
|
* {@link #onNavMessageReported(byte, byte, short, byte[])} |
||||||
|
* |
||||||
|
* <p>A {@link GpsNavMessageProto} containing the extracted field is obtained by calling the method |
||||||
|
* {@link #createDecodedNavMessage()} |
||||||
|
* |
||||||
|
* <p>References: |
||||||
|
* http://www.gps.gov/technical/icwg/IS-GPS-200D.pdf and
|
||||||
|
* http://www.gps.gov/technical/ps/1995-SPS-signal-specification.pdf
|
||||||
|
* |
||||||
|
*/ |
||||||
|
public class GpsNavigationMessageStore { |
||||||
|
|
||||||
|
private static final byte IONOSPHERIC_PARAMETERS_PAGE_18_SV_ID = 56; |
||||||
|
|
||||||
|
private static final int WORD_SIZE_BITS = 30; |
||||||
|
private static final int WORD_PADDING_BITS = 2; |
||||||
|
private static final int BYTE_AS_BITS = 8; |
||||||
|
private static final int GPS_CYCLE_WEEKS = 1024; |
||||||
|
private static final int IODE_TO_IODC_MASK = 0xFF; |
||||||
|
|
||||||
|
public static final int SUBFRAME_1 = (1 << 0); |
||||||
|
public static final int SUBFRAME_2 = (1 << 1); |
||||||
|
public static final int SUBFRAME_3 = (1 << 2); |
||||||
|
public static final int SUBFRAME_4 = (1 << 3); |
||||||
|
public static final int SUBFRAME_5 = (1 << 4); |
||||||
|
|
||||||
|
private static final double POW_2_4 = Math.pow(2, 4); |
||||||
|
private static final double POW_2_11 = Math.pow(2, 11); |
||||||
|
private static final double POW_2_12 = Math.pow(2, 12); |
||||||
|
private static final double POW_2_14 = Math.pow(2, 14); |
||||||
|
private static final double POW_2_16 = Math.pow(2, 16); |
||||||
|
private static final double POW_2_NEG_5 = Math.pow(2, -5); |
||||||
|
private static final double POW_2_NEG_19 = Math.pow(2, -19); |
||||||
|
private static final double POW_2_NEG_24 = Math.pow(2, -24); |
||||||
|
private static final double POW_2_NEG_27 = Math.pow(2, -27); |
||||||
|
private static final double POW_2_NEG_29 = Math.pow(2, -29); |
||||||
|
private static final double POW_2_NEG_30 = Math.pow(2, -30); |
||||||
|
private static final double POW_2_NEG_31 = Math.pow(2, -31); |
||||||
|
private static final double POW_2_NEG_33 = Math.pow(2, -33); |
||||||
|
private static final double POW_2_NEG_43 = Math.pow(2, -43); |
||||||
|
private static final double POW_2_NEG_55 = Math.pow(2, -55); |
||||||
|
|
||||||
|
private static final long INTEGER_RANGE = 0xFFFFFFFFL; |
||||||
|
// 3657 is the number of days between the unix epoch and GPS epoch as the GPS epoch started on
|
||||||
|
// Jan 6, 1980
|
||||||
|
private static final long GPS_EPOCH_AS_UNIX_EPOCH_MS = TimeUnit.DAYS.toMillis(3657); |
||||||
|
// A GPS Cycle is 1024 weeks, or 7168 days
|
||||||
|
private static final long GPS_CYCLE_MS = TimeUnit.DAYS.toMillis(7168); |
||||||
|
|
||||||
|
/** Maximum possible number of GPS satellites */ |
||||||
|
public static final int MAX_NUMBER_OF_SATELLITES = 32; |
||||||
|
|
||||||
|
private static final int L1_CA_MESSAGE_LENGTH_BYTES = 40; |
||||||
|
|
||||||
|
private static final int IODC1_INDEX = 82; |
||||||
|
private static final int IODC1_LENGTH = 2; |
||||||
|
private static final int IODC2_INDEX = 210; |
||||||
|
private static final int IODC2_LENGTH = 8; |
||||||
|
private static final int WEEK_INDEX = 60; |
||||||
|
private static final int WEEK_LENGTH = 10; |
||||||
|
private static final int URA_INDEX = 72; |
||||||
|
private static final int URA_LENGTH = 4; |
||||||
|
private static final int SV_HEALTH_INDEX = 76; |
||||||
|
private static final int SV_HEALTH_LENGTH = 6; |
||||||
|
private static final int TGD_INDEX = 196; |
||||||
|
private static final int TGD_LENGTH = 8; |
||||||
|
private static final int AF2_INDEX = 240; |
||||||
|
private static final int AF2_LENGTH = 8; |
||||||
|
private static final int AF1_INDEX = 248; |
||||||
|
private static final int AF1_LENGTH = 16; |
||||||
|
private static final int AF0_INDEX = 270; |
||||||
|
private static final int AF0_LENGTH = 22; |
||||||
|
private static final int IODE1_INDEX = 60; |
||||||
|
private static final int IODE_LENGTH = 8; |
||||||
|
private static final int TOC_INDEX = 218; |
||||||
|
private static final int TOC_LENGTH = 16; |
||||||
|
private static final int CRS_INDEX = 68; |
||||||
|
private static final int CRS_LENGTH = 16; |
||||||
|
private static final int DELTA_N_INDEX = 90; |
||||||
|
private static final int DELTA_N_LENGTH = 16; |
||||||
|
private static final int M0_INDEX8 = 106; |
||||||
|
private static final int M0_INDEX24 = 120; |
||||||
|
private static final int CUC_INDEX = 150; |
||||||
|
private static final int CUC_LENGTH = 16; |
||||||
|
private static final int E_INDEX8 = 166; |
||||||
|
private static final int E_INDEX24 = 180; |
||||||
|
private static final int CUS_INDEX = 210; |
||||||
|
private static final int CUS_LENGTH = 16; |
||||||
|
private static final int A_INDEX8 = 226; |
||||||
|
private static final int A_INDEX24 = 240; |
||||||
|
private static final int TOE_INDEX = 270; |
||||||
|
private static final int TOE_LENGTH = 16; |
||||||
|
private static final int IODE2_INDEX = 270; |
||||||
|
private static final int CIC_INDEX = 60; |
||||||
|
private static final int CIC_LENGTH = 16; |
||||||
|
private static final int O0_INDEX8 = 76; |
||||||
|
private static final int O0_INDEX24 = 90; |
||||||
|
private static final int O_INDEX8 = 196; |
||||||
|
private static final int O_INDEX24 = 210; |
||||||
|
private static final int ODOT_INDEX = 240; |
||||||
|
private static final int ODOT_LENGTH = 24; |
||||||
|
private static final int CIS_INDEX = 120; |
||||||
|
private static final int CIS_LENGTH = 16; |
||||||
|
private static final int I0_INDEX8 = 136; |
||||||
|
private static final int I0_INDEX24 = 150; |
||||||
|
private static final int CRC_INDEX = 180; |
||||||
|
private static final int CRC_LENGTH = 16; |
||||||
|
private static final int IDOT_INDEX = 278; |
||||||
|
private static final int IDOT_LENGTH = 14; |
||||||
|
private static final int A0_INDEX = 68; |
||||||
|
private static final int A_B_LENGTH = 8; |
||||||
|
private static final int A1_INDEX = 76; |
||||||
|
private static final int A2_INDEX = 90; |
||||||
|
private static final int A3_INDEX = 98; |
||||||
|
private static final int B0_INDEX = 106; |
||||||
|
private static final int B1_INDEX = 120; |
||||||
|
private static final int B2_INDEX = 128; |
||||||
|
private static final int B3_INDEX = 136; |
||||||
|
private static final int WN_LS_INDEX = 226; |
||||||
|
private static final int DELTA_T_LS_INDEX = 240; |
||||||
|
private static final int TOT_LS_INDEX = 218; |
||||||
|
private static final int DN_LS_INDEX = 256; |
||||||
|
private static final int WNF_LS_INDEX = 248; |
||||||
|
private static final int DELTA_TF_LS_INDEX = 270; |
||||||
|
private static final int I0UTC_INDEX8 = 210; |
||||||
|
private static final int I0UTC_INDEX24 = 180; |
||||||
|
private static final int I1UTC_INDEX = 150; |
||||||
|
|
||||||
|
/** Partially decoded intermediate ephemerides */ |
||||||
|
private final IntermediateEphemeris[] partiallyDecodedIntermediateEphemerides = |
||||||
|
new IntermediateEphemeris[MAX_NUMBER_OF_SATELLITES]; |
||||||
|
|
||||||
|
/** Fully decoded intermediate ephemerides */ |
||||||
|
private final IntermediateEphemeris[] fullyDecodedIntermediateEphemerides = |
||||||
|
new IntermediateEphemeris[MAX_NUMBER_OF_SATELLITES]; |
||||||
|
|
||||||
|
|
||||||
|
private IonosphericModelProto decodedIonosphericObj; |
||||||
|
|
||||||
|
/** |
||||||
|
* Builds and returns the current {@link GpsNavMessageProto} filling the different ephemeris for |
||||||
|
* the different satellites and setting the ionospheric model parameters. |
||||||
|
*/ |
||||||
|
@NonNull |
||||||
|
public GpsNavMessageProto createDecodedNavMessage() { |
||||||
|
synchronized (fullyDecodedIntermediateEphemerides) {; |
||||||
|
GpsNavMessageProto gpsNavMessageProto = new GpsNavMessageProto(); |
||||||
|
ArrayList<GpsEphemerisProto> gpsEphemerisProtoList = new ArrayList<>(); |
||||||
|
for (int i = 0; i < MAX_NUMBER_OF_SATELLITES; i++) { |
||||||
|
if (fullyDecodedIntermediateEphemerides[i] != null) { |
||||||
|
gpsEphemerisProtoList.add(fullyDecodedIntermediateEphemerides[i].getEphemerisObj()); |
||||||
|
} |
||||||
|
} |
||||||
|
if (decodedIonosphericObj != null) { |
||||||
|
gpsNavMessageProto.iono = decodedIonosphericObj; |
||||||
|
} |
||||||
|
gpsNavMessageProto.ephemerids = |
||||||
|
gpsEphemerisProtoList.toArray(new GpsEphemerisProto[gpsEphemerisProtoList.size()]); |
||||||
|
return gpsNavMessageProto; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Handles a fresh Navigation Message. The message is in its raw format. |
||||||
|
*/ |
||||||
|
public void onNavMessageReported(byte prn, byte type, short id, byte[] rawData) { |
||||||
|
Preconditions.checkArgument(type == 1, "Unsupported NavigationMessage Type: " + type); |
||||||
|
Preconditions.checkArgument( |
||||||
|
rawData != null && rawData.length == L1_CA_MESSAGE_LENGTH_BYTES, |
||||||
|
"Invalid length of rawData for L1 C/A"); |
||||||
|
synchronized (fullyDecodedIntermediateEphemerides) { |
||||||
|
switch (id) { |
||||||
|
case 1: |
||||||
|
handleFirstSubframe(prn, rawData); |
||||||
|
break; |
||||||
|
case 2: |
||||||
|
handleSecondSubframe(prn, rawData); |
||||||
|
break; |
||||||
|
case 3: |
||||||
|
handleThirdSubframe(prn, rawData); |
||||||
|
break; |
||||||
|
case 4: |
||||||
|
handleFourthSubframe(rawData); |
||||||
|
break; |
||||||
|
case 5: |
||||||
|
break; |
||||||
|
default: |
||||||
|
// invalid message id
|
||||||
|
throw new IllegalArgumentException("Invalid Subframe ID: " + id); |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Handles the first navigation message subframe which contains satellite clock correction |
||||||
|
* parameters, GPS date (week number) plus satellite status and health. |
||||||
|
*/ |
||||||
|
private void handleFirstSubframe(byte prn, byte[] rawData) { |
||||||
|
int iodc = extractBits(IODC1_INDEX, IODC1_LENGTH, rawData) << 8; |
||||||
|
iodc |= extractBits(IODC2_INDEX, IODC2_LENGTH, rawData); |
||||||
|
|
||||||
|
IntermediateEphemeris intermediateEphemeris = |
||||||
|
findIntermediateEphemerisToUpdate(prn, SUBFRAME_1, iodc); |
||||||
|
if (intermediateEphemeris == null) { |
||||||
|
// we are up-to-date
|
||||||
|
return; |
||||||
|
} |
||||||
|
|
||||||
|
GpsEphemerisProto gpsEphemerisProto = intermediateEphemeris.getEphemerisObj(); |
||||||
|
gpsEphemerisProto.iodc = iodc; |
||||||
|
|
||||||
|
|
||||||
|
// the navigation message contains a modulo-1023 week number
|
||||||
|
int week = extractBits(WEEK_INDEX, WEEK_LENGTH, rawData); |
||||||
|
week = getGpsWeekWithRollover(week); |
||||||
|
gpsEphemerisProto.week = week; |
||||||
|
|
||||||
|
int uraIndex = extractBits(URA_INDEX, URA_LENGTH, rawData); |
||||||
|
double svAccuracy = computeNominalSvAccuracy(uraIndex); |
||||||
|
gpsEphemerisProto.svAccuracyM = svAccuracy; |
||||||
|
|
||||||
|
int svHealth = extractBits(SV_HEALTH_INDEX, SV_HEALTH_LENGTH, rawData); |
||||||
|
gpsEphemerisProto.svHealth = svHealth; |
||||||
|
|
||||||
|
byte tgd = (byte) extractBits(TGD_INDEX, TGD_LENGTH, rawData); |
||||||
|
gpsEphemerisProto.tgd = tgd * POW_2_NEG_31; |
||||||
|
|
||||||
|
int toc = extractBits(TOC_INDEX, TOC_LENGTH, rawData); |
||||||
|
double tocScaled = toc * POW_2_4; |
||||||
|
gpsEphemerisProto.toc = tocScaled; |
||||||
|
|
||||||
|
byte af2 = (byte) extractBits(AF2_INDEX, AF2_LENGTH, rawData); |
||||||
|
gpsEphemerisProto.af2 = af2 * POW_2_NEG_55; |
||||||
|
|
||||||
|
short af1 = (short) extractBits(AF1_INDEX, AF1_LENGTH, rawData); |
||||||
|
gpsEphemerisProto.af1 = af1 * POW_2_NEG_43; |
||||||
|
|
||||||
|
// a 22-bit two's complement number
|
||||||
|
int af0 = extractBits(AF0_INDEX, AF0_LENGTH, rawData); |
||||||
|
af0 = getTwoComplement(af0, AF0_LENGTH); |
||||||
|
gpsEphemerisProto.af0 = af0 * POW_2_NEG_31; |
||||||
|
|
||||||
|
updateDecodedState(prn, SUBFRAME_1, intermediateEphemeris); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Handles the second navigation message subframe which contains satellite ephemeris |
||||||
|
*/ |
||||||
|
private void handleSecondSubframe(byte prn, byte[] rawData) { |
||||||
|
int iode = extractBits(IODE1_INDEX, IODE_LENGTH, rawData); |
||||||
|
|
||||||
|
IntermediateEphemeris intermediateEphemeris = |
||||||
|
findIntermediateEphemerisToUpdate(prn, SUBFRAME_2, iode); |
||||||
|
if (intermediateEphemeris == null) { |
||||||
|
// nothing to update
|
||||||
|
return; |
||||||
|
} |
||||||
|
|
||||||
|
GpsEphemerisProto gpsEphemerisProto = intermediateEphemeris.getEphemerisObj(); |
||||||
|
|
||||||
|
gpsEphemerisProto.iode = iode; |
||||||
|
|
||||||
|
short crs = (short) extractBits(CRS_INDEX, CRS_LENGTH, rawData); |
||||||
|
gpsEphemerisProto.crc = crs * POW_2_NEG_5; |
||||||
|
|
||||||
|
short deltaN = (short) extractBits(DELTA_N_INDEX, DELTA_N_LENGTH, rawData); |
||||||
|
gpsEphemerisProto.deltaN = deltaN * POW_2_NEG_43 * Math.PI; |
||||||
|
|
||||||
|
int m0 = (int) buildUnsigned32BitsWordFrom8And24Words(M0_INDEX8, M0_INDEX24, rawData); |
||||||
|
gpsEphemerisProto.m0 = m0 * POW_2_NEG_31 * Math.PI; |
||||||
|
|
||||||
|
short cuc = (short) extractBits(CUC_INDEX, CUC_LENGTH, rawData); |
||||||
|
gpsEphemerisProto.cuc = cuc * POW_2_NEG_29; |
||||||
|
|
||||||
|
// an unsigned 32 bit value
|
||||||
|
long e = buildUnsigned32BitsWordFrom8And24Words(E_INDEX8, E_INDEX24, rawData); |
||||||
|
gpsEphemerisProto.e = e * POW_2_NEG_33; |
||||||
|
|
||||||
|
short cus = (short) extractBits(CUS_INDEX, CUS_LENGTH, rawData); |
||||||
|
gpsEphemerisProto.cus = cus * POW_2_NEG_29; |
||||||
|
|
||||||
|
// an unsigned 32 bit value
|
||||||
|
long a = buildUnsigned32BitsWordFrom8And24Words(A_INDEX8, A_INDEX24, rawData); |
||||||
|
gpsEphemerisProto.rootOfA = a * POW_2_NEG_19; |
||||||
|
|
||||||
|
int toe = extractBits(TOE_INDEX, TOE_LENGTH, rawData); |
||||||
|
double toeScaled = toe * POW_2_4; |
||||||
|
gpsEphemerisProto.toe = toe * POW_2_4; |
||||||
|
|
||||||
|
updateDecodedState(prn, SUBFRAME_2, intermediateEphemeris); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Handles the third navigation message subframe which contains satellite ephemeris |
||||||
|
*/ |
||||||
|
private void handleThirdSubframe(byte prn, byte[] rawData) { |
||||||
|
|
||||||
|
int iode = extractBits(IODE2_INDEX, IODE_LENGTH, rawData); |
||||||
|
|
||||||
|
IntermediateEphemeris intermediateEphemeris = |
||||||
|
findIntermediateEphemerisToUpdate(prn, SUBFRAME_3, iode); |
||||||
|
if (intermediateEphemeris == null) { |
||||||
|
// A fully or partially decoded message is available , hence nothing to update
|
||||||
|
return; |
||||||
|
} |
||||||
|
|
||||||
|
GpsEphemerisProto gpsEphemerisProto = intermediateEphemeris.getEphemerisObj(); |
||||||
|
gpsEphemerisProto.iode = iode; |
||||||
|
|
||||||
|
short cic = (short) extractBits(CIC_INDEX, CIC_LENGTH, rawData); |
||||||
|
gpsEphemerisProto.cic = cic * POW_2_NEG_29; |
||||||
|
|
||||||
|
int o0 = (int) buildUnsigned32BitsWordFrom8And24Words(O0_INDEX8, O0_INDEX24, rawData); |
||||||
|
gpsEphemerisProto.omega0 = o0 * POW_2_NEG_31 * Math.PI; |
||||||
|
|
||||||
|
int o = (int) buildUnsigned32BitsWordFrom8And24Words(O_INDEX8, O_INDEX24, rawData); |
||||||
|
gpsEphemerisProto.omega = o * POW_2_NEG_31 * Math.PI; |
||||||
|
|
||||||
|
int odot = extractBits(ODOT_INDEX, ODOT_LENGTH, rawData); |
||||||
|
odot = getTwoComplement(odot, ODOT_LENGTH);; |
||||||
|
gpsEphemerisProto.omegaDot = o * POW_2_NEG_43 * Math.PI; |
||||||
|
|
||||||
|
short cis = (short) extractBits(CIS_INDEX, CIS_LENGTH, rawData); |
||||||
|
gpsEphemerisProto.cis = cis * POW_2_NEG_29; |
||||||
|
|
||||||
|
int i0 = (int) buildUnsigned32BitsWordFrom8And24Words(I0_INDEX8, I0_INDEX24, rawData); |
||||||
|
gpsEphemerisProto.i0 = i0 * POW_2_NEG_31 * Math.PI; |
||||||
|
|
||||||
|
short crc = (short) extractBits(CRC_INDEX, CRC_LENGTH, rawData); |
||||||
|
gpsEphemerisProto.crc = crc * POW_2_NEG_5; |
||||||
|
|
||||||
|
|
||||||
|
// a 14-bit two's complement number
|
||||||
|
int idot = extractBits(IDOT_INDEX, IDOT_LENGTH, rawData); |
||||||
|
idot = getTwoComplement(idot, IDOT_LENGTH); |
||||||
|
gpsEphemerisProto.iDot = idot * POW_2_NEG_43 * Math.PI; |
||||||
|
|
||||||
|
updateDecodedState(prn, SUBFRAME_3, intermediateEphemeris); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Subframe four provides ionospheric model parameters , UTC information, part of the almanac, and |
||||||
|
* indications whether the Anti-Spoofing, is activated or not. |
||||||
|
* |
||||||
|
* <p>For now, only the ionospheric parameters are parsed. |
||||||
|
*/ |
||||||
|
private void handleFourthSubframe(byte[] rawData) { |
||||||
|
byte pageId = (byte) extractBits(62, 6, rawData); |
||||||
|
if (pageId != IONOSPHERIC_PARAMETERS_PAGE_18_SV_ID) { |
||||||
|
// We only care to decode ionospheric parameters for now
|
||||||
|
return; |
||||||
|
} |
||||||
|
|
||||||
|
IonosphericModelProto ionosphericModelProto = new IonosphericModelProto(); |
||||||
|
|
||||||
|
double[] alpha = new double[4]; |
||||||
|
byte a0 = (byte) extractBits(A0_INDEX, A_B_LENGTH, rawData); |
||||||
|
alpha[0] = a0 * POW_2_NEG_30; |
||||||
|
byte a1 = (byte) extractBits(A1_INDEX, A_B_LENGTH, rawData); |
||||||
|
alpha[1] = a1 * POW_2_NEG_27; |
||||||
|
byte a2 = (byte) extractBits(A2_INDEX, A_B_LENGTH, rawData); |
||||||
|
alpha[2] = a2 * POW_2_NEG_24; |
||||||
|
byte a3 = (byte) extractBits(A3_INDEX, A_B_LENGTH, rawData); |
||||||
|
alpha[3] = a3 * POW_2_NEG_24; |
||||||
|
ionosphericModelProto.alpha = alpha; |
||||||
|
|
||||||
|
double[] beta = new double[4]; |
||||||
|
byte b0 = (byte) extractBits(B0_INDEX, A_B_LENGTH, rawData); |
||||||
|
beta[0] = b0 * POW_2_11; |
||||||
|
byte b1 = (byte) extractBits(B1_INDEX, A_B_LENGTH, rawData); |
||||||
|
beta[1] = b1 * POW_2_14; |
||||||
|
byte b2 = (byte) extractBits(B2_INDEX, A_B_LENGTH, rawData); |
||||||
|
beta[2] = b2 * POW_2_16; |
||||||
|
byte b3 = (byte) extractBits(B3_INDEX, A_B_LENGTH, rawData); |
||||||
|
beta[3] = b3 * POW_2_16; |
||||||
|
ionosphericModelProto.beta = beta; |
||||||
|
|
||||||
|
|
||||||
|
double a0UTC = |
||||||
|
buildSigned32BitsWordFrom8And24WordsWith8bitslsb(I0UTC_INDEX8, I0UTC_INDEX24, rawData) |
||||||
|
* Math.pow(2, -30); |
||||||
|
|
||||||
|
double a1UTC = getTwoComplement(extractBits(I1UTC_INDEX, 24, rawData), 24) * Math.pow(2, -50); |
||||||
|
|
||||||
|
short tot = (short) (extractBits(TOT_LS_INDEX, A_B_LENGTH, rawData) * POW_2_12); |
||||||
|
|
||||||
|
short wnt = (short) extractBits(WN_LS_INDEX, A_B_LENGTH, rawData); |
||||||
|
|
||||||
|
short tls = (short) extractBits(DELTA_T_LS_INDEX, A_B_LENGTH, rawData); |
||||||
|
|
||||||
|
short wnlsf = (short) extractBits(WNF_LS_INDEX, A_B_LENGTH, rawData); |
||||||
|
|
||||||
|
short dn = (short) extractBits(DN_LS_INDEX, A_B_LENGTH, rawData); |
||||||
|
|
||||||
|
short tlsf = (short) extractBits(DELTA_TF_LS_INDEX, A_B_LENGTH, rawData); |
||||||
|
|
||||||
|
decodedIonosphericObj = ionosphericModelProto; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Updates the {@link IntermediateEphemeris} with the decoded status of the current subframe. |
||||||
|
* Moreover, update the {@code partiallyDecodedIntermediateEphemerides} list and |
||||||
|
* {@code fullyDecodedIntermediateEphemerides} list |
||||||
|
*/ |
||||||
|
private void updateDecodedState(byte prn, int decodedSubframeNumber, |
||||||
|
IntermediateEphemeris intermediateEphemeris) { |
||||||
|
intermediateEphemeris.reportDecodedSubframe(decodedSubframeNumber); |
||||||
|
if (intermediateEphemeris.isFullyDecoded()) { |
||||||
|
partiallyDecodedIntermediateEphemerides[prn - 1] = null; |
||||||
|
fullyDecodedIntermediateEphemerides[prn - 1] = intermediateEphemeris; |
||||||
|
} else { |
||||||
|
partiallyDecodedIntermediateEphemerides[prn - 1] = intermediateEphemeris; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Extracts the requested bits from the raw stream. |
||||||
|
* |
||||||
|
* @param index Zero-based index of the first bit to extract. |
||||||
|
* @param length The length of the stream of bits to extract. |
||||||
|
* @param rawData The stream to extract data from. |
||||||
|
* |
||||||
|
* @return The bits requested always shifted to the least significant positions. |
||||||
|
*/ |
||||||
|
private static int extractBits(int index, int length, byte[] rawData) { |
||||||
|
int result = 0; |
||||||
|
|
||||||
|
for (int i = 0; i < length; ++i) { |
||||||
|
int workingIndex = index + i; |
||||||
|
|
||||||
|
int wordIndex = workingIndex / WORD_SIZE_BITS; |
||||||
|
// account for 2 bit padding for every 30bit word
|
||||||
|
workingIndex += (wordIndex + 1) * WORD_PADDING_BITS; |
||||||
|
int byteIndex = workingIndex / BYTE_AS_BITS; |
||||||
|
int byteOffset = workingIndex % BYTE_AS_BITS; |
||||||
|
|
||||||
|
byte raw = rawData[byteIndex]; |
||||||
|
// account for zero-based indexing
|
||||||
|
int shiftOffset = BYTE_AS_BITS - 1 - byteOffset; |
||||||
|
int mask = 1 << shiftOffset; |
||||||
|
int bit = raw & mask; |
||||||
|
bit >>= shiftOffset; |
||||||
|
|
||||||
|
// account for zero-based indexing
|
||||||
|
result |= bit << length - 1 - i; |
||||||
|
} |
||||||
|
return result; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Extracts an unsigned 32 bit word where the word is partitioned 8/24 bits. |
||||||
|
* |
||||||
|
* @param index8 The index of the first 8 bits used. |
||||||
|
* @param index24 The index of the last 24 bits used. |
||||||
|
* @param rawData The stream to extract data from. |
||||||
|
* |
||||||
|
* @return The bits requested represented as a long and stored in the least significant positions. |
||||||
|
*/ |
||||||
|
private static long buildUnsigned32BitsWordFrom8And24Words(int index8, int index24, |
||||||
|
byte[] rawData) { |
||||||
|
long result = (long) extractBits(index8, 8, rawData) << 24; |
||||||
|
result |= extractBits(index24, 24, rawData); |
||||||
|
return result; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Extracts a signed 32 bit word where the word is partitioned 8/24 bits with LSB first. |
||||||
|
* |
||||||
|
* @param index8 The index of the first 8 bits used. |
||||||
|
* @param index24 The index of the last 24 bits used. |
||||||
|
* @param rawData The stream to extract data from. |
||||||
|
* @return The bits requested represented as an int and stored in the least significant positions. |
||||||
|
*/ |
||||||
|
private static int buildSigned32BitsWordFrom8And24WordsWith8bitslsb( |
||||||
|
int index8, int index24, byte[] rawData) { |
||||||
|
int result = extractBits(index24, 24, rawData) << 8; |
||||||
|
result |= extractBits(index8, 8, rawData); |
||||||
|
return result; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculates the 2s complement for a specific number of bits of a given value |
||||||
|
* |
||||||
|
* @param value The set of bits to translate. |
||||||
|
* @param bits The number of bits to consider. |
||||||
|
* |
||||||
|
* @return The calculated 2s complement. |
||||||
|
*/ |
||||||
|
private static int getTwoComplement(int value, int bits) { |
||||||
|
int msbMask = 1 << bits - 1; |
||||||
|
int msb = value & msbMask; |
||||||
|
if (msb == 0) { |
||||||
|
// the value is positive
|
||||||
|
return value; |
||||||
|
} |
||||||
|
|
||||||
|
int valueBitMask = (1 << bits) - 1; |
||||||
|
int extendedSignMask = (int) INTEGER_RANGE - valueBitMask; |
||||||
|
return value | extendedSignMask; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculates the GPS week with rollovers. A rollover happens every 1024 weeks, beginning from GPS |
||||||
|
* epoch (January 6, 1980). |
||||||
|
* |
||||||
|
* @param gpsWeek The modulo-1024 GPS week. |
||||||
|
* |
||||||
|
* @return The absolute GPS week. |
||||||
|
*/ |
||||||
|
private static int getGpsWeekWithRollover(int gpsWeek) { |
||||||
|
long nowMs = System.currentTimeMillis(); |
||||||
|
long elapsedTimeFromGpsEpochMs = nowMs - GPS_EPOCH_AS_UNIX_EPOCH_MS; |
||||||
|
long rolloverCycles = elapsedTimeFromGpsEpochMs / GPS_CYCLE_MS; |
||||||
|
int rolloverWeeks = (int) rolloverCycles * GPS_CYCLE_WEEKS; |
||||||
|
return gpsWeek + rolloverWeeks; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Computes a nominal Sv Accuracy based on the URA index. This implementation is taken from |
||||||
|
* http://www.gps.gov/technical/icwg/IS-GPS-200D.pdf, section '20.3.3.3.1.3 Sv Accuracy'.
|
||||||
|
* |
||||||
|
* @param uraIndex The URA Index |
||||||
|
* |
||||||
|
* @return A computed nominal Sv accuracy. |
||||||
|
*/ |
||||||
|
private static double computeNominalSvAccuracy(int uraIndex) { |
||||||
|
if (uraIndex < 0 || uraIndex >= 15) { |
||||||
|
return Double.NaN; |
||||||
|
} else if (uraIndex == 1) { |
||||||
|
return 2.8; |
||||||
|
} else if (uraIndex == 3) { |
||||||
|
return 5.7; |
||||||
|
} else if (uraIndex == 5) { |
||||||
|
return 11.3; |
||||||
|
} |
||||||
|
|
||||||
|
int exponent; |
||||||
|
if (uraIndex < 6) { |
||||||
|
exponent = 1 + (uraIndex / 2); |
||||||
|
} else { |
||||||
|
exponent = uraIndex - 2; |
||||||
|
} |
||||||
|
return Math.pow(2, exponent); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Finds an {@link IntermediateEphemeris} that can be updated by the given data. The pseudocode is |
||||||
|
* as follows: |
||||||
|
* <p>if a fully decoded message is available and matches, there is no need to update |
||||||
|
* <p>if a partially decoded message is available and matches, there is no need to update |
||||||
|
* <p>if the provided issueOfData matches intermediate partially decoded state, update in place |
||||||
|
* <p>otherwise, start a new decoding 'session' for the prn |
||||||
|
* |
||||||
|
* @param prn The prn to update |
||||||
|
* @param subframe The subframe available to update |
||||||
|
* @param issueOfData The issueOfData associated with the given subframe |
||||||
|
* @return a {@link IntermediateEphemeris} to update with the available data, {@code null} if |
||||||
|
* there is no need to update a {@link IntermediateEphemeris}. |
||||||
|
*/ |
||||||
|
private IntermediateEphemeris findIntermediateEphemerisToUpdate(byte prn, int subframe, |
||||||
|
int issueOfData) { |
||||||
|
// find out if we have fully decoded up-to-date ephemeris first
|
||||||
|
IntermediateEphemeris fullyDecodedIntermediateEphemeris = |
||||||
|
this.fullyDecodedIntermediateEphemerides[prn - 1]; |
||||||
|
if (fullyDecodedIntermediateEphemeris != null |
||||||
|
&& fullyDecodedIntermediateEphemeris.findSubframeInfo(prn, subframe, issueOfData) |
||||||
|
.isSubframeDecoded()) { |
||||||
|
return null; |
||||||
|
} |
||||||
|
|
||||||
|
// find out next if there is a partially decoded intermediate state we can continue working on
|
||||||
|
IntermediateEphemeris partiallyDecodedIntermediateEphemeris = |
||||||
|
this.partiallyDecodedIntermediateEphemerides[prn - 1]; |
||||||
|
if (partiallyDecodedIntermediateEphemeris == null) { |
||||||
|
// no intermediate partially decoded state, we need to start a decoding 'session'
|
||||||
|
return new IntermediateEphemeris(prn); |
||||||
|
} |
||||||
|
SubframeCheckResult subframeCheckResult = partiallyDecodedIntermediateEphemeris |
||||||
|
.findSubframeInfo(prn, subframe, issueOfData); |
||||||
|
if (subframeCheckResult.isSubframeDecoded()) { |
||||||
|
return null; |
||||||
|
} |
||||||
|
if (subframeCheckResult.hasSubframe && !subframeCheckResult.issueOfDataMatches) { |
||||||
|
// the navigation message has changed, we need to start over
|
||||||
|
return new IntermediateEphemeris(prn); |
||||||
|
} |
||||||
|
|
||||||
|
int intermediateIode = Integer.MAX_VALUE; |
||||||
|
boolean intermediateHasIode = false; |
||||||
|
GpsEphemerisProto gpsEphemerisProto = partiallyDecodedIntermediateEphemeris.getEphemerisObj(); |
||||||
|
|
||||||
|
if (partiallyDecodedIntermediateEphemeris.hasDecodedSubframe(SUBFRAME_1)) { |
||||||
|
intermediateHasIode = true; |
||||||
|
intermediateIode = gpsEphemerisProto.iodc & IODE_TO_IODC_MASK; |
||||||
|
} |
||||||
|
if (partiallyDecodedIntermediateEphemeris.hasDecodedSubframe(SUBFRAME_2) |
||||||
|
|| partiallyDecodedIntermediateEphemeris.hasDecodedSubframe(SUBFRAME_3)) { |
||||||
|
intermediateHasIode = true; |
||||||
|
intermediateIode = gpsEphemerisProto.iode; |
||||||
|
} |
||||||
|
|
||||||
|
boolean canContinueDecoding; |
||||||
|
int iode; |
||||||
|
switch (subframe) { |
||||||
|
case SUBFRAME_1: |
||||||
|
iode = issueOfData & IODE_TO_IODC_MASK; |
||||||
|
canContinueDecoding = !intermediateHasIode || (intermediateIode == iode); |
||||||
|
break; |
||||||
|
case SUBFRAME_2: |
||||||
|
// fall through
|
||||||
|
case SUBFRAME_3: |
||||||
|
iode = issueOfData; |
||||||
|
canContinueDecoding = !intermediateHasIode || (intermediateIode == iode); |
||||||
|
break; |
||||||
|
case SUBFRAME_4: |
||||||
|
// fall through
|
||||||
|
case SUBFRAME_5: |
||||||
|
// always continue decoding for subframes 4-5
|
||||||
|
canContinueDecoding = true; |
||||||
|
break; |
||||||
|
default: |
||||||
|
throw new IllegalStateException("invalid subframe requested: " + subframe); |
||||||
|
} |
||||||
|
|
||||||
|
if (canContinueDecoding) { |
||||||
|
return partiallyDecodedIntermediateEphemeris; |
||||||
|
} |
||||||
|
return new IntermediateEphemeris(prn); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* A representation of an intermediate ephemeris that can be fully decoded or partially decoded. |
||||||
|
*/ |
||||||
|
private static class IntermediateEphemeris { |
||||||
|
|
||||||
|
private final GpsEphemerisProto gpsEphemerisProtoObj = new GpsEphemerisProto(); |
||||||
|
|
||||||
|
private int subframesDecoded; |
||||||
|
|
||||||
|
public IntermediateEphemeris(byte prn) { |
||||||
|
gpsEphemerisProtoObj.prn = prn; |
||||||
|
} |
||||||
|
|
||||||
|
public void reportDecodedSubframe(int subframe) { |
||||||
|
subframesDecoded |= subframe; |
||||||
|
} |
||||||
|
|
||||||
|
public boolean hasDecodedSubframe(int subframe) { |
||||||
|
return (subframesDecoded & subframe) == subframe; |
||||||
|
} |
||||||
|
|
||||||
|
public boolean isFullyDecoded() { |
||||||
|
return hasDecodedSubframe(SUBFRAME_1) && hasDecodedSubframe(SUBFRAME_2) |
||||||
|
&& hasDecodedSubframe(SUBFRAME_3); |
||||||
|
} |
||||||
|
|
||||||
|
public GpsEphemerisProto getEphemerisObj() { |
||||||
|
return gpsEphemerisProtoObj; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Verifies that the received subframe info (IODE and IODC) matches the existing info (IODE and |
||||||
|
* IODC). For each subframe there is a given issueOfData that must match, this method abstracts |
||||||
|
* the logic to perform such check. |
||||||
|
* |
||||||
|
* @param prn The expected prn. |
||||||
|
* @param subframe The expected subframe. |
||||||
|
* @param issueOfData The issueOfData for the given subframe. |
||||||
|
* |
||||||
|
* @return {@link SubframeCheckResult} representing the state found. |
||||||
|
*/ |
||||||
|
public SubframeCheckResult findSubframeInfo(byte prn, int subframe, int issueOfData) { |
||||||
|
if (gpsEphemerisProtoObj.prn != prn) { |
||||||
|
return new SubframeCheckResult(false /* hasSubframe */, false /* issueOfDataMatches */); |
||||||
|
} |
||||||
|
boolean issueOfDataMatches; |
||||||
|
switch (subframe) { |
||||||
|
case SUBFRAME_1: |
||||||
|
issueOfDataMatches = gpsEphemerisProtoObj.iodc == issueOfData; |
||||||
|
break; |
||||||
|
case SUBFRAME_2: |
||||||
|
// fall through
|
||||||
|
case SUBFRAME_3: |
||||||
|
issueOfDataMatches = gpsEphemerisProtoObj.iode == issueOfData; |
||||||
|
break; |
||||||
|
case SUBFRAME_4: |
||||||
|
// fall through
|
||||||
|
case SUBFRAME_5: |
||||||
|
// subframes 4-5 do not have IOD to match, so we assume they always match
|
||||||
|
issueOfDataMatches = true; |
||||||
|
break; |
||||||
|
default: |
||||||
|
throw new IllegalArgumentException("Invalid subframe provided: " + subframe); |
||||||
|
} |
||||||
|
boolean hasDecodedSubframe = hasDecodedSubframe(subframe); |
||||||
|
return new SubframeCheckResult(hasDecodedSubframe, issueOfDataMatches); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Represents a result while finding a subframe in an intermediate {@link IntermediateEphemeris}. |
||||||
|
*/ |
||||||
|
private static class SubframeCheckResult { |
||||||
|
|
||||||
|
/** |
||||||
|
* The intermediate {@link IntermediateEphemeris} has the requested subframe. |
||||||
|
*/ |
||||||
|
public final boolean hasSubframe; |
||||||
|
|
||||||
|
/** |
||||||
|
* The issue of data, associated with the requested subframe, matches the subframe found in the |
||||||
|
* intermediate state. |
||||||
|
*/ |
||||||
|
public final boolean issueOfDataMatches; |
||||||
|
|
||||||
|
public SubframeCheckResult(boolean hasSubframe, boolean issueOfDataMatches) { |
||||||
|
this.hasSubframe = hasSubframe; |
||||||
|
this.issueOfDataMatches = issueOfDataMatches; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* @return {@code true} if the requested subframe has been decoded in the intermediate state, |
||||||
|
* {@code false} otherwise. |
||||||
|
*/ |
||||||
|
public boolean isSubframeDecoded() { |
||||||
|
return hasSubframe && issueOfDataMatches; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,315 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
import android.util.Pair; |
||||||
|
import com.google.common.base.Preconditions; |
||||||
|
import com.google.common.primitives.Longs; |
||||||
|
import java.util.Calendar; |
||||||
|
import java.util.concurrent.TimeUnit; |
||||||
|
import java.time.ZoneId; |
||||||
|
import java.time.ZonedDateTime; |
||||||
|
import java.time.Instant; |
||||||
|
import java.util.GregorianCalendar; |
||||||
|
|
||||||
|
/** |
||||||
|
* A simple class to represent time unit used by GPS. |
||||||
|
*/ |
||||||
|
public class GpsTime implements Comparable<GpsTime> { |
||||||
|
public static final int MILLIS_IN_SECOND = 1000; |
||||||
|
public static final int SECONDS_IN_MINUTE = 60; |
||||||
|
public static final int MINUTES_IN_HOUR = 60; |
||||||
|
public static final int HOURS_IN_DAY = 24; |
||||||
|
public static final int SECONDS_IN_DAY = |
||||||
|
HOURS_IN_DAY * MINUTES_IN_HOUR * SECONDS_IN_MINUTE; |
||||||
|
public static final int DAYS_IN_WEEK = 7; |
||||||
|
public static final long MILLIS_IN_DAY = TimeUnit.DAYS.toMillis(1); |
||||||
|
public static final long MILLIS_IN_WEEK = TimeUnit.DAYS.toMillis(7); |
||||||
|
public static final long NANOS_IN_WEEK = TimeUnit.DAYS.toNanos(7); |
||||||
|
// GPS epoch is 1980/01/06
|
||||||
|
public static final long GPS_DAYS_SINCE_JAVA_EPOCH = 3657; |
||||||
|
public static final long GPS_UTC_EPOCH_OFFSET_SECONDS = |
||||||
|
TimeUnit.DAYS.toSeconds(GPS_DAYS_SINCE_JAVA_EPOCH); |
||||||
|
public static final long GPS_UTC_EPOCH_OFFSET_NANOS = |
||||||
|
TimeUnit.SECONDS.toNanos(GPS_UTC_EPOCH_OFFSET_SECONDS); |
||||||
|
private static final ZonedDateTime LEAP_SECOND_DATE_1981 = getZonedDateTimeUTC(1981, 7, 1); |
||||||
|
private static final ZonedDateTime LEAP_SECOND_DATE_2012 = getZonedDateTimeUTC(2012, 7, 1); |
||||||
|
private static final ZonedDateTime LEAP_SECOND_DATE_2015 = getZonedDateTimeUTC(2015, 7, 1); |
||||||
|
private static final ZonedDateTime LEAP_SECOND_DATE_2017 = getZonedDateTimeUTC(2017, 7, 1); |
||||||
|
private static final long nanoSecPerSec = TimeUnit.SECONDS.toNanos(7); |
||||||
|
// nanoseconds since GPS epoch (1980/1/6).
|
||||||
|
private long gpsNanos; |
||||||
|
private static ZonedDateTime getZonedDateTimeUTC(int year, int month, int day) { |
||||||
|
return getZonedDateTimeUTC(year, month, day, 0, 0, 0, 0); |
||||||
|
} |
||||||
|
|
||||||
|
private static ZonedDateTime getZonedDateTimeUTC(int year, int month, int day, |
||||||
|
int hour, int minute, int sec, int nanoSec){ |
||||||
|
ZoneId zone = ZoneId.of("UTC"); |
||||||
|
ZonedDateTime zdt = ZonedDateTime.of(year, month, day, hour, minute, sec, nanoSec, zone); |
||||||
|
return zdt; |
||||||
|
} |
||||||
|
|
||||||
|
private static long getMillisFromZonedDateTime(ZonedDateTime zdt) { |
||||||
|
return zdt.toInstant().toEpochMilli(); |
||||||
|
} |
||||||
|
/** |
||||||
|
* Constructor for GpsTime. Input values are all in GPS time. |
||||||
|
* @param year Year |
||||||
|
* @param month Month from 1 to 12 |
||||||
|
* @param day Day from 1 to 31 |
||||||
|
* @param hour Hour from 0 to 23 |
||||||
|
* @param minute Minute from 0 to 59 |
||||||
|
* @param second Second from 0 to 59 |
||||||
|
*/ |
||||||
|
public GpsTime(int year, int month, int day, int hour, int minute, double second) { |
||||||
|
ZonedDateTime utcDateTime = getZonedDateTimeUTC(year, month, day, hour, minute, |
||||||
|
(int) second, (int) ((second * nanoSecPerSec) % nanoSecPerSec)); |
||||||
|
|
||||||
|
|
||||||
|
// Since input time is already specify in GPS time, no need to count leap second here.
|
||||||
|
initGpsNanos(utcDateTime); |
||||||
|
|
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Constructor |
||||||
|
* @param zDateTime is created using GPS time values. |
||||||
|
*/ |
||||||
|
public GpsTime(ZonedDateTime zDateTime) { |
||||||
|
initGpsNanos(zDateTime); |
||||||
|
} |
||||||
|
|
||||||
|
public void initGpsNanos(ZonedDateTime zDateTime){ |
||||||
|
this.gpsNanos = TimeUnit.MILLISECONDS.toNanos(getMillisFromZonedDateTime(zDateTime)) |
||||||
|
- GPS_UTC_EPOCH_OFFSET_NANOS; |
||||||
|
} |
||||||
|
/** |
||||||
|
* Constructor |
||||||
|
* @param gpsNanos nanoseconds since GPS epoch. |
||||||
|
*/ |
||||||
|
public GpsTime(long gpsNanos) { |
||||||
|
this.gpsNanos = gpsNanos; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Creates a GPS time using a UTC based date and time. |
||||||
|
* @param zDateTime represents the current time in UTC time, must be after 2009 |
||||||
|
*/ |
||||||
|
public static GpsTime fromUtc(ZonedDateTime zDateTime) { |
||||||
|
return new GpsTime(TimeUnit.MILLISECONDS.toNanos(getMillisFromZonedDateTime(zDateTime)) |
||||||
|
+ TimeUnit.SECONDS.toNanos( |
||||||
|
GpsTime.getLeapSecond(zDateTime) - GPS_UTC_EPOCH_OFFSET_SECONDS)); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Creates a GPS time based upon the current time. |
||||||
|
*/ |
||||||
|
public static GpsTime now() { |
||||||
|
ZoneId zone = ZoneId.of("UTC"); |
||||||
|
ZonedDateTime current = ZonedDateTime.now(zone); |
||||||
|
return fromUtc(current); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Creates a GPS time using absolute GPS week number, and the time of week. |
||||||
|
* @param gpsWeek |
||||||
|
* @param towSec GPS time of week in second |
||||||
|
* @return actual time in GpsTime. |
||||||
|
*/ |
||||||
|
public static GpsTime fromWeekTow(int gpsWeek, int towSec) { |
||||||
|
long nanos = gpsWeek * NANOS_IN_WEEK + TimeUnit.SECONDS.toNanos(towSec); |
||||||
|
return new GpsTime(nanos); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Creates a GPS time using YUMA GPS week number (0..1023), and the time of week. |
||||||
|
* @param yumaWeek (0..1023) |
||||||
|
* @param towSec GPS time of week in second |
||||||
|
* @return actual time in GpsTime. |
||||||
|
*/ |
||||||
|
public static GpsTime fromYumaWeekTow(int yumaWeek, int towSec) { |
||||||
|
Preconditions.checkArgument(yumaWeek >= 0); |
||||||
|
Preconditions.checkArgument(yumaWeek < 1024); |
||||||
|
|
||||||
|
// Estimate the multiplier of current week.
|
||||||
|
ZoneId zone = ZoneId.of("UTC"); |
||||||
|
ZonedDateTime current = ZonedDateTime.now(zone); |
||||||
|
GpsTime refTime = new GpsTime(current); |
||||||
|
Pair<Integer, Integer> refWeekSec = refTime.getGpsWeekSecond(); |
||||||
|
int weekMultiplier = refWeekSec.first / 1024; |
||||||
|
|
||||||
|
int gpsWeek = weekMultiplier * 1024 + yumaWeek; |
||||||
|
return fromWeekTow(gpsWeek, towSec); |
||||||
|
} |
||||||
|
|
||||||
|
public static GpsTime fromTimeSinceGpsEpoch(long gpsSec) { |
||||||
|
return new GpsTime(TimeUnit.SECONDS.toNanos(gpsSec)); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Computes leap seconds. Only accurate after 2009. |
||||||
|
* @param time |
||||||
|
* @return number of leap seconds since GPS epoch. |
||||||
|
*/ |
||||||
|
public static int getLeapSecond(ZonedDateTime time) { |
||||||
|
if (LEAP_SECOND_DATE_2017.compareTo(time) <= 0) { |
||||||
|
return 18; |
||||||
|
} else if (LEAP_SECOND_DATE_2015.compareTo(time) <= 0) { |
||||||
|
return 17; |
||||||
|
} else if (LEAP_SECOND_DATE_2012.compareTo(time) <= 0) { |
||||||
|
return 16; |
||||||
|
} else if (LEAP_SECOND_DATE_1981.compareTo(time) <= 0) { |
||||||
|
// Only correct between 2012/7/1 to 2008/12/31
|
||||||
|
return 15; |
||||||
|
} else { |
||||||
|
return 0; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Computes GPS weekly epoch of the reference time. |
||||||
|
* <p>GPS weekly epoch are defined as of every Sunday 00:00:000 (mor |
||||||
|
* @param refTime reference time |
||||||
|
* @return nanoseconds since GPS epoch, for the week epoch. |
||||||
|
*/ |
||||||
|
public static Long getGpsWeekEpochNano(GpsTime refTime) { |
||||||
|
Pair<Integer, Integer> weekSecond = refTime.getGpsWeekSecond(); |
||||||
|
return weekSecond.first * NANOS_IN_WEEK; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* @return week count since GPS epoch, and second count since the beginning of |
||||||
|
* that week. |
||||||
|
*/ |
||||||
|
public Pair<Integer, Integer> getGpsWeekSecond() { |
||||||
|
// JAVA/UNIX epoch: January 1, 1970 in msec
|
||||||
|
// GPS epoch: January 6, 1980 in second
|
||||||
|
int week = (int) (gpsNanos / NANOS_IN_WEEK); |
||||||
|
int second = (int) TimeUnit.NANOSECONDS.toSeconds(gpsNanos % NANOS_IN_WEEK); |
||||||
|
return Pair.create(week, second); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* @return week count since GPS epoch, and second count in 0.08 sec |
||||||
|
* resolution, 23-bit presentation (required by RRLP.)" |
||||||
|
*/ |
||||||
|
public Pair<Integer, Integer> getGpsWeekTow23b() { |
||||||
|
// UNIX epoch: January 1, 1970 in msec
|
||||||
|
// GPS epoch: January 6, 1980 in second
|
||||||
|
int week = (int) (gpsNanos / NANOS_IN_WEEK); |
||||||
|
// 80 millis is 0.08 second.
|
||||||
|
int tow23b = (int) TimeUnit.NANOSECONDS.toMillis(gpsNanos % NANOS_IN_WEEK) / 80; |
||||||
|
return Pair.create(week, tow23b); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* @return Day of year in GPS time (GMT time) |
||||||
|
*/ |
||||||
|
public static int getCurrentDayOfYear() { |
||||||
|
ZoneId zone = ZoneId.of("UTC"); |
||||||
|
ZonedDateTime current = ZonedDateTime.now(zone); |
||||||
|
// Since current is derived from UTC time, we need to add leap second here.
|
||||||
|
long gpsTimeMillis = getMillisFromZonedDateTime(current) |
||||||
|
+ TimeUnit.SECONDS.toMillis(getLeapSecond(current)); |
||||||
|
ZonedDateTime gpsCurrent = ZonedDateTime.ofInstant(Instant.ofEpochMilli(gpsTimeMillis), ZoneId.of("UTC")); |
||||||
|
return gpsCurrent.getDayOfYear(); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* @return milliseconds since JAVA/UNIX epoch. |
||||||
|
*/ |
||||||
|
public final long getMillisSinceJavaEpoch() { |
||||||
|
return TimeUnit.NANOSECONDS.toMillis(gpsNanos + GPS_UTC_EPOCH_OFFSET_NANOS); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* @return milliseconds since GPS epoch. |
||||||
|
*/ |
||||||
|
public final long getMillisSinceGpsEpoch() { |
||||||
|
return TimeUnit.NANOSECONDS.toMillis(gpsNanos); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* @return microseconds since GPS epoch. |
||||||
|
*/ |
||||||
|
public final long getMicrosSinceGpsEpoch() { |
||||||
|
return TimeUnit.NANOSECONDS.toMicros(gpsNanos); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* @return nanoseconds since GPS epoch. |
||||||
|
*/ |
||||||
|
public final long getNanosSinceGpsEpoch() { |
||||||
|
return gpsNanos; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* @return the GPS time in Calendar. |
||||||
|
*/ |
||||||
|
public Calendar getTimeInCalendar() { |
||||||
|
return GregorianCalendar.from(getGpsDateTime()); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* @return a ZonedDateTime with leap seconds considered. |
||||||
|
*/ |
||||||
|
public ZonedDateTime getUtcDateTime() { |
||||||
|
ZonedDateTime gpsDateTime = getGpsDateTime(); |
||||||
|
long gpsMillis = getMillisFromZonedDateTime(gpsDateTime) |
||||||
|
- TimeUnit.SECONDS.toMillis(getLeapSecond(gpsDateTime)); |
||||||
|
return ZonedDateTime.ofInstant(Instant.ofEpochMilli(gpsMillis), ZoneId.of("UTC")); |
||||||
|
|
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* @return a ZonedDateTime based on the pure GPS time (without considering leap second). |
||||||
|
*/ |
||||||
|
public ZonedDateTime getGpsDateTime() { |
||||||
|
long gpsMillis = TimeUnit.NANOSECONDS.toMillis(gpsNanos + GPS_UTC_EPOCH_OFFSET_NANOS); |
||||||
|
return ZonedDateTime.ofInstant(Instant.ofEpochMilli(gpsMillis), ZoneId.of("UTC")); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Compares two {@code GpsTime} objects temporally. |
||||||
|
* |
||||||
|
* @param other the {@code GpsTime} to be compared. |
||||||
|
* @return the value {@code 0} if this {@code GpsTime} is simultaneous with |
||||||
|
* the argument {@code GpsTime}; a value less than {@code 0} if this |
||||||
|
* {@code GpsTime} occurs before the argument {@code GpsTime}; and |
||||||
|
* a value greater than {@code 0} if this {@code GpsTime} occurs |
||||||
|
* after the argument {@code GpsTime} (signed comparison). |
||||||
|
*/ |
||||||
|
@Override |
||||||
|
public int compareTo(GpsTime other) { |
||||||
|
return Long.compare(this.getNanosSinceGpsEpoch(), other.getNanosSinceGpsEpoch()); |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public boolean equals(Object other) { |
||||||
|
if (!(other instanceof GpsTime)) { |
||||||
|
return false; |
||||||
|
} |
||||||
|
GpsTime time = (GpsTime) other; |
||||||
|
return getNanosSinceGpsEpoch() == time.getNanosSinceGpsEpoch(); |
||||||
|
} |
||||||
|
|
||||||
|
@Override |
||||||
|
public int hashCode() { |
||||||
|
return Longs.hashCode(getNanosSinceGpsEpoch()); |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,139 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.Ecef2LlaConverter.GeodeticLlaValues; |
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.EcefToTopocentricConverter.TopocentricAEDValues; |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculates the Ionospheric correction of the pseudorange given the {@code userPosition}, |
||||||
|
* {@code satellitePosition}, {@code gpsTimeSeconds} and the ionospheric parameters sent by the |
||||||
|
* satellite {@code alpha} and {@code beta} |
||||||
|
* |
||||||
|
* <p>Source: http://www.navipedia.net/index.php/Klobuchar_Ionospheric_Model and
|
||||||
|
* http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4104345 and
|
||||||
|
* http://www.ion.org/museum/files/ACF2A4.pdf
|
||||||
|
*/ |
||||||
|
public class IonosphericModel { |
||||||
|
/** Center frequency of the L1 band in Hz. */ |
||||||
|
public static final double L1_FREQ_HZ = 10.23 * 1e6 * 154; |
||||||
|
/** Center frequency of the L2 band in Hz. */ |
||||||
|
public static final double L2_FREQ_HZ = 10.23 * 1e6 * 120; |
||||||
|
/** Center frequency of the L5 band in Hz. */ |
||||||
|
public static final double L5_FREQ_HZ = 10.23 * 1e6 * 115; |
||||||
|
|
||||||
|
private static final double SECONDS_PER_DAY = 86400.0; |
||||||
|
private static final double PERIOD_OF_DELAY_TRHESHOLD_SECONDS = 72000.0; |
||||||
|
private static final double IPP_LATITUDE_THRESHOLD_SEMI_CIRCLE = 0.416; |
||||||
|
private static final double DC_TERM = 5.0e-9; |
||||||
|
private static final double NORTH_GEOMAGNETIC_POLE_LONGITUDE_RADIANS = 5.08; |
||||||
|
private static final double GEOMETRIC_LATITUDE_CONSTANT = 0.064; |
||||||
|
private static final int DELAY_PHASE_TIME_CONSTANT_SECONDS = 50400; |
||||||
|
private static final int IONO_0_IDX = 0; |
||||||
|
private static final int IONO_1_IDX = 1; |
||||||
|
private static final int IONO_2_IDX = 2; |
||||||
|
private static final int IONO_3_IDX = 3; |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculates the Ionospheric correction of the pseudorane in seconds using the Klobuchar |
||||||
|
* Ionospheric model. |
||||||
|
*/ |
||||||
|
public static double ionoKloboucharCorrectionSeconds( |
||||||
|
double[] userPositionECEFMeters, |
||||||
|
double[] satellitePositionECEFMeters, |
||||||
|
double gpsTOWSeconds, |
||||||
|
double[] alpha, |
||||||
|
double[] beta, |
||||||
|
double frequencyHz) { |
||||||
|
|
||||||
|
TopocentricAEDValues elevationAndAzimuthRadians = EcefToTopocentricConverter |
||||||
|
.calculateElAzDistBetween2Points(userPositionECEFMeters, satellitePositionECEFMeters); |
||||||
|
double elevationSemiCircle = elevationAndAzimuthRadians.elevationRadians / Math.PI; |
||||||
|
double azimuthSemiCircle = elevationAndAzimuthRadians.azimuthRadians / Math.PI; |
||||||
|
GeodeticLlaValues latLngAlt = Ecef2LlaConverter.convertECEFToLLACloseForm( |
||||||
|
userPositionECEFMeters[0], userPositionECEFMeters[1], userPositionECEFMeters[2]); |
||||||
|
double latitudeUSemiCircle = latLngAlt.latitudeRadians / Math.PI; |
||||||
|
double longitudeUSemiCircle = latLngAlt.longitudeRadians / Math.PI; |
||||||
|
|
||||||
|
// earth's centered angle (semi-circles)
|
||||||
|
double earthCentredAngleSemiCirle = 0.0137 / (elevationSemiCircle + 0.11) - 0.022; |
||||||
|
|
||||||
|
// latitude of the Ionospheric Pierce Point (IPP) (semi-circles)
|
||||||
|
double latitudeISemiCircle = |
||||||
|
latitudeUSemiCircle + earthCentredAngleSemiCirle * Math.cos(azimuthSemiCircle * Math.PI); |
||||||
|
|
||||||
|
if (latitudeISemiCircle > IPP_LATITUDE_THRESHOLD_SEMI_CIRCLE) { |
||||||
|
latitudeISemiCircle = IPP_LATITUDE_THRESHOLD_SEMI_CIRCLE; |
||||||
|
} else if (latitudeISemiCircle < -IPP_LATITUDE_THRESHOLD_SEMI_CIRCLE) { |
||||||
|
latitudeISemiCircle = -IPP_LATITUDE_THRESHOLD_SEMI_CIRCLE; |
||||||
|
} |
||||||
|
|
||||||
|
// geodetic longitude of the Ionospheric Pierce Point (IPP) (semi-circles)
|
||||||
|
double longitudeISemiCircle = longitudeUSemiCircle + earthCentredAngleSemiCirle |
||||||
|
* Math.sin(azimuthSemiCircle * Math.PI) / Math.cos(latitudeISemiCircle * Math.PI); |
||||||
|
|
||||||
|
// geomagnetic latitude of the Ionospheric Pierce Point (IPP) (semi-circles)
|
||||||
|
double geomLatIPPSemiCircle = latitudeISemiCircle + GEOMETRIC_LATITUDE_CONSTANT |
||||||
|
* Math.cos(longitudeISemiCircle * Math.PI - NORTH_GEOMAGNETIC_POLE_LONGITUDE_RADIANS); |
||||||
|
|
||||||
|
// local time (sec) at the Ionospheric Pierce Point (IPP)
|
||||||
|
double localTimeSeconds = SECONDS_PER_DAY / 2.0 * longitudeISemiCircle + gpsTOWSeconds; |
||||||
|
localTimeSeconds %= SECONDS_PER_DAY; |
||||||
|
if (localTimeSeconds < 0) { |
||||||
|
localTimeSeconds += SECONDS_PER_DAY; |
||||||
|
} |
||||||
|
|
||||||
|
// amplitude of the ionospheric delay (seconds)
|
||||||
|
double amplitudeOfDelaySeconds = alpha[IONO_0_IDX] + alpha[IONO_1_IDX] * geomLatIPPSemiCircle |
||||||
|
+ alpha[IONO_2_IDX] * geomLatIPPSemiCircle * geomLatIPPSemiCircle + alpha[IONO_3_IDX] |
||||||
|
* geomLatIPPSemiCircle * geomLatIPPSemiCircle * geomLatIPPSemiCircle; |
||||||
|
if (amplitudeOfDelaySeconds < 0) { |
||||||
|
amplitudeOfDelaySeconds = 0; |
||||||
|
} |
||||||
|
|
||||||
|
// period of ionospheric delay
|
||||||
|
double periodOfDelaySeconds = beta[IONO_0_IDX] + beta[IONO_1_IDX] * geomLatIPPSemiCircle |
||||||
|
+ beta[IONO_2_IDX] * geomLatIPPSemiCircle * geomLatIPPSemiCircle + beta[IONO_3_IDX] |
||||||
|
* geomLatIPPSemiCircle * geomLatIPPSemiCircle * geomLatIPPSemiCircle; |
||||||
|
if (periodOfDelaySeconds < PERIOD_OF_DELAY_TRHESHOLD_SECONDS) { |
||||||
|
periodOfDelaySeconds = PERIOD_OF_DELAY_TRHESHOLD_SECONDS; |
||||||
|
} |
||||||
|
|
||||||
|
// phase of ionospheric delay
|
||||||
|
double phaseOfDelayRadians = |
||||||
|
2 * Math.PI * (localTimeSeconds - DELAY_PHASE_TIME_CONSTANT_SECONDS) / periodOfDelaySeconds; |
||||||
|
|
||||||
|
// slant factor
|
||||||
|
double slantFactor = 1.0 + 16.0 * Math.pow(0.53 - elevationSemiCircle, 3); |
||||||
|
|
||||||
|
// ionospheric time delay (seconds)
|
||||||
|
double ionoDelaySeconds; |
||||||
|
|
||||||
|
if (Math.abs(phaseOfDelayRadians) >= Math.PI / 2.0) { |
||||||
|
ionoDelaySeconds = DC_TERM * slantFactor; |
||||||
|
} else { |
||||||
|
ionoDelaySeconds = (DC_TERM |
||||||
|
+ (1 - Math.pow(phaseOfDelayRadians, 2) / 2.0 + Math.pow(phaseOfDelayRadians, 4) / 24.0) |
||||||
|
* amplitudeOfDelaySeconds) * slantFactor; |
||||||
|
} |
||||||
|
|
||||||
|
// apply factor for frequency bands other than L1
|
||||||
|
ionoDelaySeconds *= (L1_FREQ_HZ * L1_FREQ_HZ) / (frequencyHz * frequencyHz); |
||||||
|
|
||||||
|
return ionoDelaySeconds; |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,57 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.Ecef2LlaConverter.GeodeticLlaValues; |
||||||
|
|
||||||
|
/** |
||||||
|
* A tool to convert geodetic latitude, longitude and altitude above planetary ellipsoid to |
||||||
|
* Earth-centered Earth-fixed (ECEF) Cartesian coordinates |
||||||
|
* |
||||||
|
* <p>Source: https://www.mathworks.com/help/aeroblks/llatoecefposition.html
|
||||||
|
*/ |
||||||
|
public class Lla2EcefConverter { |
||||||
|
private static final double ECCENTRICITY = 8.1819190842622e-2; |
||||||
|
private static final double EARTH_SEMI_MAJOR_AXIS_METERS = 6378137.0; |
||||||
|
|
||||||
|
/** |
||||||
|
* Converts LLA (latitude,longitude, and altitude) coordinates to ECEF |
||||||
|
* (Earth-Centered Earth-Fixed) Cartesian coordinates |
||||||
|
* |
||||||
|
* <p>Inputs is GeodeticLlaValues class {@link GeodeticLlaValues} containing geodetic latitude |
||||||
|
* (radians), geodetic longitude (radians), height above WGS84 ellipsoid (m) |
||||||
|
* |
||||||
|
* <p>Output is cartesian coordinates x,y,z in meters |
||||||
|
*/ |
||||||
|
public static double[] convertFromLlaToEcefMeters(GeodeticLlaValues llaValues) { |
||||||
|
double cosLatitude = Math.cos(llaValues.latitudeRadians); |
||||||
|
double cosLongitude = Math.cos(llaValues.longitudeRadians); |
||||||
|
double sinLatitude = Math.sin(llaValues.latitudeRadians); |
||||||
|
double sinLongitude = Math.sin(llaValues.longitudeRadians); |
||||||
|
|
||||||
|
double r0 = |
||||||
|
EARTH_SEMI_MAJOR_AXIS_METERS |
||||||
|
/ Math.sqrt(1.0 - Math.pow(ECCENTRICITY, 2) * sinLatitude * sinLatitude); |
||||||
|
|
||||||
|
double[] positionEcefMeters = new double[3]; |
||||||
|
positionEcefMeters[0] = (llaValues.altitudeMeters + r0) * cosLatitude * cosLongitude; |
||||||
|
positionEcefMeters[1] = (llaValues.altitudeMeters + r0) * cosLatitude * sinLongitude; |
||||||
|
positionEcefMeters[2] = |
||||||
|
(llaValues.altitudeMeters + r0 * (1.0 - Math.pow(ECCENTRICITY, 2))) * sinLatitude; |
||||||
|
return positionEcefMeters; |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,36 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
import java.util.Collections; |
||||||
|
import java.util.List; |
||||||
|
|
||||||
|
/** |
||||||
|
* An implementation of {@link PseudorangeSmoother} that performs no smoothing. |
||||||
|
* |
||||||
|
* <p> A new list of {@link GpsMeasurementWithRangeAndUncertainty} instances is filled with a copy |
||||||
|
* of the input list. |
||||||
|
*/ |
||||||
|
class PseudorangeNoSmoothingSmoother implements PseudorangeSmoother { |
||||||
|
|
||||||
|
@Override |
||||||
|
public List<GpsMeasurementWithRangeAndUncertainty> updatePseudorangeSmoothingResult( |
||||||
|
List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToGPSReceiverMeasurements) { |
||||||
|
return Collections.unmodifiableList(usefulSatellitesToGPSReceiverMeasurements); |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
@ -0,0 +1,500 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
import android.location.GnssClock; |
||||||
|
import android.location.GnssMeasurement; |
||||||
|
import android.location.GnssMeasurementsEvent; |
||||||
|
import android.location.GnssNavigationMessage; |
||||||
|
import android.location.GnssStatus; |
||||||
|
import android.util.Log; |
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.Ecef2EnuConverter.EnuValues; |
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.Ecef2LlaConverter.GeodeticLlaValues; |
||||||
|
import android.location.cts.nano.Ephemeris.GpsEphemerisProto; |
||||||
|
import android.location.cts.nano.Ephemeris.GpsNavMessageProto; |
||||||
|
import android.location.cts.suplClient.SuplRrlpController; |
||||||
|
import java.io.BufferedReader; |
||||||
|
import java.io.IOException; |
||||||
|
import java.net.UnknownHostException; |
||||||
|
import java.util.ArrayList; |
||||||
|
import java.util.Arrays; |
||||||
|
import java.util.Calendar; |
||||||
|
import java.util.List; |
||||||
|
import java.util.concurrent.TimeUnit; |
||||||
|
|
||||||
|
/** |
||||||
|
* Helper class for calculating Gps position and velocity solution using weighted least squares |
||||||
|
* where the raw Gps measurements are parsed as a {@link BufferedReader} with the option to apply |
||||||
|
* doppler smoothing, carrier phase smoothing or no smoothing. |
||||||
|
* |
||||||
|
*/ |
||||||
|
public class PseudorangePositionVelocityFromRealTimeEvents { |
||||||
|
|
||||||
|
private static final String TAG = "PseudorangePositionVelocityFromRealTimeEvents"; |
||||||
|
private static final double SECONDS_PER_NANO = 1.0e-9; |
||||||
|
private static final int TOW_DECODED_MEASUREMENT_STATE_BIT = 3; |
||||||
|
/** Average signal travel time from GPS satellite and earth */ |
||||||
|
private static final int VALID_ACCUMULATED_DELTA_RANGE_STATE = 1; |
||||||
|
private static final int MINIMUM_NUMBER_OF_USEFUL_SATELLITES = 4; |
||||||
|
private static final int C_TO_N0_THRESHOLD_DB_HZ = 18; |
||||||
|
|
||||||
|
private static final String SUPL_SERVER_NAME = "supl.google.com"; |
||||||
|
private static final int SUPL_SERVER_PORT = 7276; |
||||||
|
|
||||||
|
private GpsNavMessageProto mHardwareGpsNavMessageProto = null; |
||||||
|
|
||||||
|
// navigation message parser
|
||||||
|
private GpsNavigationMessageStore mGpsNavigationMessageStore = new GpsNavigationMessageStore(); |
||||||
|
private double[] mPositionSolutionLatLngDeg = GpsMathOperations.createAndFillArray(3, Double.NaN); |
||||||
|
private double[] mVelocitySolutionEnuMps = GpsMathOperations.createAndFillArray(3, Double.NaN); |
||||||
|
private final double[] mPositionVelocityUncertaintyEnu |
||||||
|
= GpsMathOperations.createAndFillArray(6, Double.NaN); |
||||||
|
private double[] mPseudorangeResidualsMeters = |
||||||
|
GpsMathOperations.createAndFillArray( |
||||||
|
GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES, Double.NaN |
||||||
|
); |
||||||
|
private boolean mFirstUsefulMeasurementSet = true; |
||||||
|
private int[] mReferenceLocation = null; |
||||||
|
private long mLastReceivedSuplMessageTimeMillis = 0; |
||||||
|
private long mDeltaTimeMillisToMakeSuplRequest = TimeUnit.MINUTES.toMillis(30); |
||||||
|
private boolean mFirstSuplRequestNeeded = true; |
||||||
|
private GpsNavMessageProto mGpsNavMessageProtoUsed = null; |
||||||
|
|
||||||
|
// Only the interface of pseudorange smoother is provided. Please implement customized smoother.
|
||||||
|
PseudorangeSmoother mPseudorangeSmoother = new PseudorangeNoSmoothingSmoother(); |
||||||
|
private final UserPositionVelocityWeightedLeastSquare mUserPositionVelocityLeastSquareCalculator = |
||||||
|
new UserPositionVelocityWeightedLeastSquare(mPseudorangeSmoother); |
||||||
|
private GpsMeasurement[] mUsefulSatellitesToReceiverMeasurements = |
||||||
|
new GpsMeasurement[GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES]; |
||||||
|
private Long[] mUsefulSatellitesToTowNs = |
||||||
|
new Long[GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES]; |
||||||
|
private long mLargestTowNs = Long.MIN_VALUE; |
||||||
|
private double mArrivalTimeSinceGPSWeekNs = 0.0; |
||||||
|
private int mDayOfYear1To366 = 0; |
||||||
|
private int mGpsWeekNumber = 0; |
||||||
|
private long mArrivalTimeSinceGpsEpochNs = 0; |
||||||
|
|
||||||
|
/** |
||||||
|
* Computes Weighted least square position and velocity solutions from a received {@link |
||||||
|
* GnssMeasurementsEvent} and store the result in {@link |
||||||
|
* PseudorangePositionVelocityFromRealTimeEvents#mPositionSolutionLatLngDeg} and {@link |
||||||
|
* PseudorangePositionVelocityFromRealTimeEvents#mVelocitySolutionEnuMps} |
||||||
|
*/ |
||||||
|
public void computePositionVelocitySolutionsFromRawMeas(GnssMeasurementsEvent event) |
||||||
|
throws Exception { |
||||||
|
if (mReferenceLocation == null) { |
||||||
|
// If no reference location is received, we can not get navigation message from SUPL and hence
|
||||||
|
// we will not try to compute location.
|
||||||
|
Log.d(TAG, " No reference Location ..... no position is calculated"); |
||||||
|
return; |
||||||
|
} |
||||||
|
for (int i = 0; i < GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES; i++) { |
||||||
|
mUsefulSatellitesToReceiverMeasurements[i] = null; |
||||||
|
mUsefulSatellitesToTowNs[i] = null; |
||||||
|
} |
||||||
|
|
||||||
|
GnssClock gnssClock = event.getClock(); |
||||||
|
mArrivalTimeSinceGpsEpochNs = gnssClock.getTimeNanos() - gnssClock.getFullBiasNanos(); |
||||||
|
|
||||||
|
for (GnssMeasurement measurement : event.getMeasurements()) { |
||||||
|
// ignore any measurement if it is not from GPS constellation
|
||||||
|
if (measurement.getConstellationType() != GnssStatus.CONSTELLATION_GPS) { |
||||||
|
continue; |
||||||
|
} |
||||||
|
// ignore raw data if time is zero, if signal to noise ratio is below threshold or if
|
||||||
|
// TOW is not yet decoded
|
||||||
|
if (measurement.getCn0DbHz() >= C_TO_N0_THRESHOLD_DB_HZ |
||||||
|
&& (measurement.getState() & (1L << TOW_DECODED_MEASUREMENT_STATE_BIT)) != 0) { |
||||||
|
|
||||||
|
// calculate day of year and Gps week number needed for the least square
|
||||||
|
GpsTime gpsTime = new GpsTime(mArrivalTimeSinceGpsEpochNs); |
||||||
|
// Gps weekly epoch in Nanoseconds: defined as of every Sunday night at 00:00:000
|
||||||
|
long gpsWeekEpochNs = GpsTime.getGpsWeekEpochNano(gpsTime); |
||||||
|
mArrivalTimeSinceGPSWeekNs = mArrivalTimeSinceGpsEpochNs - gpsWeekEpochNs; |
||||||
|
mGpsWeekNumber = gpsTime.getGpsWeekSecond().first; |
||||||
|
// calculate day of the year between 1 and 366
|
||||||
|
Calendar cal = gpsTime.getTimeInCalendar(); |
||||||
|
mDayOfYear1To366 = cal.get(Calendar.DAY_OF_YEAR); |
||||||
|
|
||||||
|
long receivedGPSTowNs = measurement.getReceivedSvTimeNanos(); |
||||||
|
if (receivedGPSTowNs > mLargestTowNs) { |
||||||
|
mLargestTowNs = receivedGPSTowNs; |
||||||
|
} |
||||||
|
mUsefulSatellitesToTowNs[measurement.getSvid() - 1] = receivedGPSTowNs; |
||||||
|
GpsMeasurement gpsReceiverMeasurement = |
||||||
|
new GpsMeasurement( |
||||||
|
(long) mArrivalTimeSinceGPSWeekNs, |
||||||
|
measurement.getAccumulatedDeltaRangeMeters(), |
||||||
|
measurement.getAccumulatedDeltaRangeState() == VALID_ACCUMULATED_DELTA_RANGE_STATE, |
||||||
|
measurement.getPseudorangeRateMetersPerSecond(), |
||||||
|
measurement.getCn0DbHz(), |
||||||
|
measurement.getAccumulatedDeltaRangeUncertaintyMeters(), |
||||||
|
measurement.getPseudorangeRateUncertaintyMetersPerSecond()); |
||||||
|
mUsefulSatellitesToReceiverMeasurements[measurement.getSvid() - 1] = gpsReceiverMeasurement; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
// check if we should continue using the navigation message from the SUPL server, or use the
|
||||||
|
// navigation message from the device if we fully received it
|
||||||
|
boolean useNavMessageFromSupl = |
||||||
|
continueUsingNavMessageFromSupl( |
||||||
|
mUsefulSatellitesToReceiverMeasurements, mHardwareGpsNavMessageProto); |
||||||
|
if (useNavMessageFromSupl) { |
||||||
|
Log.d(TAG, "Using navigation message from SUPL server"); |
||||||
|
|
||||||
|
if (mFirstSuplRequestNeeded |
||||||
|
|| (System.currentTimeMillis() - mLastReceivedSuplMessageTimeMillis) |
||||||
|
> mDeltaTimeMillisToMakeSuplRequest) { |
||||||
|
// The following line is blocking call for SUPL connection and back. But it is fast enough
|
||||||
|
mGpsNavMessageProtoUsed = getSuplNavMessage(mReferenceLocation[0], mReferenceLocation[1]); |
||||||
|
if (!isEmptyNavMessage(mGpsNavMessageProtoUsed)) { |
||||||
|
mFirstSuplRequestNeeded = false; |
||||||
|
mLastReceivedSuplMessageTimeMillis = System.currentTimeMillis(); |
||||||
|
} else { |
||||||
|
return; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
} else { |
||||||
|
Log.d(TAG, "Using navigation message from the GPS receiver"); |
||||||
|
mGpsNavMessageProtoUsed = mHardwareGpsNavMessageProto; |
||||||
|
} |
||||||
|
|
||||||
|
// some times the SUPL server returns less satellites than the visible ones, so remove those
|
||||||
|
// visible satellites that are not returned by SUPL
|
||||||
|
for (int i = 0; i < GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES; i++) { |
||||||
|
if (mUsefulSatellitesToReceiverMeasurements[i] != null |
||||||
|
&& !navMessageProtoContainsSvid(mGpsNavMessageProtoUsed, i + 1)) { |
||||||
|
mUsefulSatellitesToReceiverMeasurements[i] = null; |
||||||
|
mUsefulSatellitesToTowNs[i] = null; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
// calculate the number of useful satellites
|
||||||
|
int numberOfUsefulSatellites = 0; |
||||||
|
for (GpsMeasurement element : mUsefulSatellitesToReceiverMeasurements) { |
||||||
|
if (element != null) { |
||||||
|
numberOfUsefulSatellites++; |
||||||
|
} |
||||||
|
} |
||||||
|
if (numberOfUsefulSatellites >= MINIMUM_NUMBER_OF_USEFUL_SATELLITES) { |
||||||
|
// ignore first set of > 4 satellites as they often result in erroneous position
|
||||||
|
if (!mFirstUsefulMeasurementSet) { |
||||||
|
// start with last known position and velocity of zero. Following the structure:
|
||||||
|
// [X position, Y position, Z position, clock bias,
|
||||||
|
// X Velocity, Y Velocity, Z Velocity, clock bias rate]
|
||||||
|
double[] positionVeloctySolutionEcef = GpsMathOperations.createAndFillArray(8, 0); |
||||||
|
double[] positionVelocityUncertaintyEnu = GpsMathOperations.createAndFillArray(6, 0); |
||||||
|
double[] pseudorangeResidualMeters |
||||||
|
= GpsMathOperations.createAndFillArray( |
||||||
|
GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES, Double.NaN |
||||||
|
); |
||||||
|
performPositionVelocityComputationEcef( |
||||||
|
mUserPositionVelocityLeastSquareCalculator, |
||||||
|
mUsefulSatellitesToReceiverMeasurements, |
||||||
|
mUsefulSatellitesToTowNs, |
||||||
|
mLargestTowNs, |
||||||
|
mArrivalTimeSinceGPSWeekNs, |
||||||
|
mDayOfYear1To366, |
||||||
|
mGpsWeekNumber, |
||||||
|
positionVeloctySolutionEcef, |
||||||
|
positionVelocityUncertaintyEnu, |
||||||
|
pseudorangeResidualMeters); |
||||||
|
// convert the position solution from ECEF to latitude, longitude and altitude
|
||||||
|
GeodeticLlaValues latLngAlt = |
||||||
|
Ecef2LlaConverter.convertECEFToLLACloseForm( |
||||||
|
positionVeloctySolutionEcef[0], |
||||||
|
positionVeloctySolutionEcef[1], |
||||||
|
positionVeloctySolutionEcef[2]); |
||||||
|
mPositionSolutionLatLngDeg[0] = Math.toDegrees(latLngAlt.latitudeRadians); |
||||||
|
mPositionSolutionLatLngDeg[1] = Math.toDegrees(latLngAlt.longitudeRadians); |
||||||
|
mPositionSolutionLatLngDeg[2] = latLngAlt.altitudeMeters; |
||||||
|
mPositionVelocityUncertaintyEnu[0] = positionVelocityUncertaintyEnu[0]; |
||||||
|
mPositionVelocityUncertaintyEnu[1] = positionVelocityUncertaintyEnu[1]; |
||||||
|
mPositionVelocityUncertaintyEnu[2] = positionVelocityUncertaintyEnu[2]; |
||||||
|
System.arraycopy( |
||||||
|
pseudorangeResidualMeters, |
||||||
|
0 /*source starting pos*/, |
||||||
|
mPseudorangeResidualsMeters, |
||||||
|
0 /*destination starting pos*/, |
||||||
|
GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES /*length of elements*/ |
||||||
|
); |
||||||
|
Log.d(TAG, |
||||||
|
"Position Uncertainty ENU Meters :" |
||||||
|
+ mPositionVelocityUncertaintyEnu[0] |
||||||
|
+ " " |
||||||
|
+ mPositionVelocityUncertaintyEnu[1] |
||||||
|
+ " " |
||||||
|
+ mPositionVelocityUncertaintyEnu[2]); |
||||||
|
Log.d( |
||||||
|
TAG, |
||||||
|
"Latitude, Longitude, Altitude: " |
||||||
|
+ mPositionSolutionLatLngDeg[0] |
||||||
|
+ " " |
||||||
|
+ mPositionSolutionLatLngDeg[1] |
||||||
|
+ " " |
||||||
|
+ mPositionSolutionLatLngDeg[2]); |
||||||
|
EnuValues velocityEnu = Ecef2EnuConverter.convertEcefToEnu( |
||||||
|
positionVeloctySolutionEcef[4], |
||||||
|
positionVeloctySolutionEcef[5], |
||||||
|
positionVeloctySolutionEcef[6], |
||||||
|
latLngAlt.latitudeRadians, |
||||||
|
latLngAlt.longitudeRadians |
||||||
|
); |
||||||
|
|
||||||
|
mVelocitySolutionEnuMps[0] = velocityEnu.enuEast; |
||||||
|
mVelocitySolutionEnuMps[1] = velocityEnu.enuNorth; |
||||||
|
mVelocitySolutionEnuMps[2] = velocityEnu.enuUP; |
||||||
|
Log.d( |
||||||
|
TAG, |
||||||
|
"Velocity ENU Mps: " |
||||||
|
+ mVelocitySolutionEnuMps[0] |
||||||
|
+ " " |
||||||
|
+ mVelocitySolutionEnuMps[1] |
||||||
|
+ " " |
||||||
|
+ mVelocitySolutionEnuMps[2]); |
||||||
|
mPositionVelocityUncertaintyEnu[3] = positionVelocityUncertaintyEnu[3]; |
||||||
|
mPositionVelocityUncertaintyEnu[4] = positionVelocityUncertaintyEnu[4]; |
||||||
|
mPositionVelocityUncertaintyEnu[5] = positionVelocityUncertaintyEnu[5]; |
||||||
|
Log.d(TAG, |
||||||
|
"Velocity Uncertainty ENU Mps :" |
||||||
|
+ mPositionVelocityUncertaintyEnu[3] |
||||||
|
+ " " |
||||||
|
+ mPositionVelocityUncertaintyEnu[4] |
||||||
|
+ " " |
||||||
|
+ mPositionVelocityUncertaintyEnu[5]); |
||||||
|
} |
||||||
|
mFirstUsefulMeasurementSet = false; |
||||||
|
} else { |
||||||
|
Log.d( |
||||||
|
TAG, |
||||||
|
"Less than four satellites with SNR above threshold visible ... " |
||||||
|
+ "no position is calculated!"); |
||||||
|
|
||||||
|
mPositionSolutionLatLngDeg = GpsMathOperations.createAndFillArray(3, Double.NaN); |
||||||
|
mVelocitySolutionEnuMps = GpsMathOperations.createAndFillArray(3, Double.NaN); |
||||||
|
mPseudorangeResidualsMeters = |
||||||
|
GpsMathOperations.createAndFillArray( |
||||||
|
GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES, Double.NaN |
||||||
|
); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
private boolean isEmptyNavMessage(GpsNavMessageProto navMessageProto) { |
||||||
|
if(navMessageProto.iono == null)return true; |
||||||
|
if(navMessageProto.ephemerids.length ==0)return true; |
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
private boolean navMessageProtoContainsSvid(GpsNavMessageProto navMessageProto, int svid) { |
||||||
|
List<GpsEphemerisProto> ephemeridesList = |
||||||
|
new ArrayList<GpsEphemerisProto>(Arrays.asList(navMessageProto.ephemerids)); |
||||||
|
for (GpsEphemerisProto ephProtoFromList : ephemeridesList) { |
||||||
|
if (ephProtoFromList.prn == svid) { |
||||||
|
return true; |
||||||
|
} |
||||||
|
} |
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculates ECEF least square position and velocity solutions from an array of {@link |
||||||
|
* GpsMeasurement} in meters and meters per second and store the result in {@code |
||||||
|
* positionVelocitySolutionEcef} |
||||||
|
*/ |
||||||
|
private void performPositionVelocityComputationEcef( |
||||||
|
UserPositionVelocityWeightedLeastSquare userPositionVelocityLeastSquare, |
||||||
|
GpsMeasurement[] usefulSatellitesToReceiverMeasurements, |
||||||
|
Long[] usefulSatellitesToTOWNs, |
||||||
|
long largestTowNs, |
||||||
|
double arrivalTimeSinceGPSWeekNs, |
||||||
|
int dayOfYear1To366, |
||||||
|
int gpsWeekNumber, |
||||||
|
double[] positionVelocitySolutionEcef, |
||||||
|
double[] positionVelocityUncertaintyEnu, |
||||||
|
double[] pseudorangeResidualMeters) |
||||||
|
throws Exception { |
||||||
|
|
||||||
|
List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToPseudorangeMeasurements = |
||||||
|
UserPositionVelocityWeightedLeastSquare.computePseudorangeAndUncertainties( |
||||||
|
Arrays.asList(usefulSatellitesToReceiverMeasurements), |
||||||
|
usefulSatellitesToTOWNs, |
||||||
|
largestTowNs); |
||||||
|
|
||||||
|
// calculate iterative least square position solution and velocity solutions
|
||||||
|
userPositionVelocityLeastSquare.calculateUserPositionVelocityLeastSquare( |
||||||
|
mGpsNavMessageProtoUsed, |
||||||
|
usefulSatellitesToPseudorangeMeasurements, |
||||||
|
arrivalTimeSinceGPSWeekNs * SECONDS_PER_NANO, |
||||||
|
gpsWeekNumber, |
||||||
|
dayOfYear1To366, |
||||||
|
positionVelocitySolutionEcef, |
||||||
|
positionVelocityUncertaintyEnu, |
||||||
|
pseudorangeResidualMeters); |
||||||
|
|
||||||
|
Log.d( |
||||||
|
TAG, |
||||||
|
"Least Square Position Solution in ECEF meters: " |
||||||
|
+ positionVelocitySolutionEcef[0] |
||||||
|
+ " " |
||||||
|
+ positionVelocitySolutionEcef[1] |
||||||
|
+ " " |
||||||
|
+ positionVelocitySolutionEcef[2]); |
||||||
|
Log.d(TAG, "Estimated Receiver clock offset in meters: " + positionVelocitySolutionEcef[3]); |
||||||
|
|
||||||
|
Log.d( |
||||||
|
TAG, |
||||||
|
"Velocity Solution in ECEF Mps: " |
||||||
|
+ positionVelocitySolutionEcef[4] |
||||||
|
+ " " |
||||||
|
+ positionVelocitySolutionEcef[5] |
||||||
|
+ " " |
||||||
|
+ positionVelocitySolutionEcef[6]); |
||||||
|
Log.d(TAG, "Estimated Reciever clock offset rate in mps: " + positionVelocitySolutionEcef[7]); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Reads the navigation message from the SUPL server by creating a Stubby client to Stubby server |
||||||
|
* that wraps the SUPL server. The input is the time in nanoseconds since the GPS epoch at which |
||||||
|
* the navigation message is required and the output is a {@link GpsNavMessageProto} |
||||||
|
* |
||||||
|
* @throws IOException |
||||||
|
* @throws UnknownHostException |
||||||
|
*/ |
||||||
|
private GpsNavMessageProto getSuplNavMessage(long latE7, long lngE7) |
||||||
|
throws UnknownHostException, IOException { |
||||||
|
SuplRrlpController suplRrlpController = |
||||||
|
new SuplRrlpController(SUPL_SERVER_NAME, SUPL_SERVER_PORT); |
||||||
|
GpsNavMessageProto navMessageProto = suplRrlpController.generateNavMessage(latE7, lngE7); |
||||||
|
|
||||||
|
return navMessageProto; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Checks if we should continue using the navigation message from the SUPL server, or use the |
||||||
|
* navigation message from the device if we fully received it. If the navigation message read from |
||||||
|
* the receiver has all the visible satellite ephemerides, return false, otherwise, return true. |
||||||
|
*/ |
||||||
|
private static boolean continueUsingNavMessageFromSupl( |
||||||
|
GpsMeasurement[] usefulSatellitesToReceiverMeasurements, |
||||||
|
GpsNavMessageProto hardwareGpsNavMessageProto) { |
||||||
|
boolean useNavMessageFromSupl = true; |
||||||
|
if (hardwareGpsNavMessageProto != null) { |
||||||
|
ArrayList<GpsEphemerisProto> hardwareEphemeridesList= |
||||||
|
new ArrayList<GpsEphemerisProto>(Arrays.asList(hardwareGpsNavMessageProto.ephemerids)); |
||||||
|
if (hardwareGpsNavMessageProto.iono != null) { |
||||||
|
for (int i = 0; i < GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES; i++) { |
||||||
|
if (usefulSatellitesToReceiverMeasurements[i] != null) { |
||||||
|
int prn = i + 1; |
||||||
|
for (GpsEphemerisProto hardwareEphProtoFromList : hardwareEphemeridesList) { |
||||||
|
if (hardwareEphProtoFromList.prn == prn) { |
||||||
|
useNavMessageFromSupl = false; |
||||||
|
break; |
||||||
|
} |
||||||
|
useNavMessageFromSupl = true; |
||||||
|
} |
||||||
|
if (useNavMessageFromSupl == true) { |
||||||
|
break; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
return useNavMessageFromSupl; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Parses a string array containing an updates to the navigation message and return the most |
||||||
|
* recent {@link GpsNavMessageProto}. |
||||||
|
*/ |
||||||
|
public void parseHwNavigationMessageUpdates(GnssNavigationMessage navigationMessage) { |
||||||
|
byte messagePrn = (byte) navigationMessage.getSvid(); |
||||||
|
byte messageType = (byte) (navigationMessage.getType() >> 8); |
||||||
|
int subMessageId = navigationMessage.getSubmessageId(); |
||||||
|
|
||||||
|
byte[] messageRawData = navigationMessage.getData(); |
||||||
|
// parse only GPS navigation messages for now
|
||||||
|
if (messageType == 1) { |
||||||
|
mGpsNavigationMessageStore.onNavMessageReported( |
||||||
|
messagePrn, messageType, (short) subMessageId, messageRawData); |
||||||
|
mHardwareGpsNavMessageProto = mGpsNavigationMessageStore.createDecodedNavMessage(); |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
|
|
||||||
|
/** Sets a rough location of the receiver that can be used to request SUPL assistance data */ |
||||||
|
public void setReferencePosition(int latE7, int lngE7, int altE7) { |
||||||
|
if (mReferenceLocation == null) { |
||||||
|
mReferenceLocation = new int[3]; |
||||||
|
} |
||||||
|
mReferenceLocation[0] = latE7; |
||||||
|
mReferenceLocation[1] = lngE7; |
||||||
|
mReferenceLocation[2] = altE7; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Converts the input from LLA coordinates to ECEF and set up the reference position of |
||||||
|
* {@code mUserPositionVelocityLeastSquareCalculator} to calculate a corrected residual. |
||||||
|
* |
||||||
|
* <p> Based on this input ground truth, true residuals can be computed. This is done by using |
||||||
|
* the high elevation satellites to compute the true user clock error and with the knowledge of |
||||||
|
* the satellite positions. |
||||||
|
* |
||||||
|
* <p> If no ground truth is set, no residual analysis will be performed. |
||||||
|
*/ |
||||||
|
public void setCorrectedResidualComputationTruthLocationLla |
||||||
|
(double[] groundTruthLocationLla) { |
||||||
|
if (groundTruthLocationLla == null) { |
||||||
|
mUserPositionVelocityLeastSquareCalculator |
||||||
|
.setTruthLocationForCorrectedResidualComputationEcef(null); |
||||||
|
return; |
||||||
|
} |
||||||
|
GeodeticLlaValues llaValues = |
||||||
|
new GeodeticLlaValues( |
||||||
|
Math.toRadians(groundTruthLocationLla[0]), |
||||||
|
Math.toRadians(groundTruthLocationLla[1]), |
||||||
|
Math.toRadians(groundTruthLocationLla[2])); |
||||||
|
mUserPositionVelocityLeastSquareCalculator.setTruthLocationForCorrectedResidualComputationEcef( |
||||||
|
Lla2EcefConverter.convertFromLlaToEcefMeters(llaValues)); |
||||||
|
} |
||||||
|
|
||||||
|
/** Returns the last computed weighted least square position solution */ |
||||||
|
public double[] getPositionSolutionLatLngDeg() { |
||||||
|
return mPositionSolutionLatLngDeg; |
||||||
|
} |
||||||
|
|
||||||
|
/** Returns the last computed Velocity solution */ |
||||||
|
public double[] getVelocitySolutionEnuMps() { |
||||||
|
return mVelocitySolutionEnuMps; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Returns the last computed position and velocity uncertainties in meters and meter per seconds, |
||||||
|
* respectively. |
||||||
|
*/ |
||||||
|
public double[] getPositionVelocityUncertaintyEnu() { |
||||||
|
return mPositionVelocityUncertaintyEnu; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Returns the pseudorange residuals corrected by using clock bias computed from highest |
||||||
|
* elevationDegree satellites. |
||||||
|
*/ |
||||||
|
public double[] getPseudorangeResidualsMeters() { |
||||||
|
return mPseudorangeResidualsMeters; |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,38 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
import java.util.List; |
||||||
|
|
||||||
|
/** |
||||||
|
* Interface for smoothing a list of {@link GpsMeasurementWithRangeAndUncertainty} instances |
||||||
|
* received at a point of time. |
||||||
|
*/ |
||||||
|
interface PseudorangeSmoother { |
||||||
|
|
||||||
|
/** |
||||||
|
* Takes an input list of {@link GpsMeasurementWithRangeAndUncertainty} instances and returns a |
||||||
|
* new list that contains smoothed pseudorange measurements. |
||||||
|
* |
||||||
|
* <p>The input list is of size {@link GpsNavigationMessageStore#MAX_NUMBER_OF_SATELLITES} with |
||||||
|
* not visible GPS satellites having null entries, and the returned new list is of the same size. |
||||||
|
* |
||||||
|
* <p>The method does not modify the input list. |
||||||
|
*/ |
||||||
|
List<GpsMeasurementWithRangeAndUncertainty> updatePseudorangeSmoothingResult( |
||||||
|
List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToGPSReceiverMeasurements); |
||||||
|
} |
||||||
@ -0,0 +1,202 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
import com.google.common.base.Preconditions; |
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.EcefToTopocentricConverter.TopocentricAEDValues; |
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.UserPositionVelocityWeightedLeastSquare. |
||||||
|
SatellitesPositionPseudorangesResidualAndCovarianceMatrix; |
||||||
|
import java.util.Arrays; |
||||||
|
import java.util.Comparator; |
||||||
|
|
||||||
|
/** |
||||||
|
* A tool with the methods to perform the pseudorange residual analysis. |
||||||
|
* |
||||||
|
* <p>The tool allows correcting the pseudorange residuals computed in WLS by removing the user |
||||||
|
* clock error. The user clock bias is computed using the highest elevation satellites as those are |
||||||
|
* assumed not to suffer from multipath. The reported residuals are provided at the input ground |
||||||
|
* truth position by applying an adjustment using the distance of WLS to satellites vs ground-truth |
||||||
|
* to satellites. |
||||||
|
*/ |
||||||
|
|
||||||
|
public class ResidualCorrectionCalculator { |
||||||
|
|
||||||
|
/** |
||||||
|
* The threshold for the residual of user clock bias per satellite with respect to the best user |
||||||
|
* clock bias. |
||||||
|
*/ |
||||||
|
private static final double BEST_USER_CLOCK_BIAS_RESIDUAL_THRESHOLD_METERS = 10; |
||||||
|
|
||||||
|
/* The number of satellites we pick for calculating the best user clock bias */ |
||||||
|
private static final int MIN_SATS_FOR_BIAS_COMPUTATION = 4; |
||||||
|
|
||||||
|
/** |
||||||
|
* Corrects the pseudorange residual by the best user clock bias estimation computed from the top |
||||||
|
* elevation satellites. |
||||||
|
* |
||||||
|
* @param satellitesPositionPseudorangesResidual satellite position and pseudorange residual info |
||||||
|
* passed in from WLS |
||||||
|
* @param positionVelocitySolutionECEF position velocity solution passed in from WLS |
||||||
|
* @param groundTruthInputECEFMeters the reference position in ECEF meters |
||||||
|
* @return an array contains the corrected pseusorange residual in meters for each satellite |
||||||
|
*/ |
||||||
|
public static double[] calculateCorrectedResiduals( |
||||||
|
SatellitesPositionPseudorangesResidualAndCovarianceMatrix |
||||||
|
satellitesPositionPseudorangesResidual, |
||||||
|
double[] positionVelocitySolutionECEF, |
||||||
|
double[] groundTruthInputECEFMeters) { |
||||||
|
|
||||||
|
|
||||||
|
double[] residuals = satellitesPositionPseudorangesResidual.pseudorangeResidualsMeters.clone(); |
||||||
|
int[] satellitePrn = satellitesPositionPseudorangesResidual.satellitePRNs.clone(); |
||||||
|
double[] satelliteElevationDegree = new double[residuals.length]; |
||||||
|
SatelliteElevationAndResiduals[] satelliteResidualsListAndElevation = |
||||||
|
new SatelliteElevationAndResiduals[residuals.length]; |
||||||
|
|
||||||
|
// Check the alignment between inputs
|
||||||
|
Preconditions.checkArgument(residuals.length == satellitePrn.length); |
||||||
|
|
||||||
|
// Apply residual corrections per satellite
|
||||||
|
for (int i = 0; i < residuals.length; i++) { |
||||||
|
// Calculate the delta of user-satellite distance between ground truth and WLS solution
|
||||||
|
// and use the delta to adjust the residuals computed from the WLS. With this adjustments all
|
||||||
|
// residuals will be as if they are computed with respect to the ground truth rather than
|
||||||
|
// the WLS.
|
||||||
|
double[] satellitePos = satellitesPositionPseudorangesResidual.satellitesPositionsMeters[i]; |
||||||
|
double wlsUserSatelliteDistance = |
||||||
|
GpsMathOperations.vectorNorm( |
||||||
|
GpsMathOperations.subtractTwoVectors( |
||||||
|
Arrays.copyOf(positionVelocitySolutionECEF, 3), |
||||||
|
satellitePos)); |
||||||
|
double groundTruthSatelliteDistance = |
||||||
|
GpsMathOperations.vectorNorm( |
||||||
|
GpsMathOperations.subtractTwoVectors(groundTruthInputECEFMeters, satellitePos)); |
||||||
|
|
||||||
|
// Compute the adjustment for satellite i
|
||||||
|
double groundTruthAdjustment = wlsUserSatelliteDistance - groundTruthSatelliteDistance; |
||||||
|
|
||||||
|
// Correct the input residual with the adjustment to ground truth
|
||||||
|
residuals[i] = residuals[i] - groundTruthAdjustment; |
||||||
|
|
||||||
|
// Calculate the elevation in degrees of satellites
|
||||||
|
TopocentricAEDValues topocentricAedValues = |
||||||
|
EcefToTopocentricConverter.calculateElAzDistBetween2Points( |
||||||
|
groundTruthInputECEFMeters, satellitesPositionPseudorangesResidual. |
||||||
|
satellitesPositionsMeters[i] |
||||||
|
); |
||||||
|
|
||||||
|
satelliteElevationDegree[i] = Math.toDegrees(topocentricAedValues.elevationRadians); |
||||||
|
|
||||||
|
// Store the computed satellite elevations and residuals into a SatelliteElevationAndResiduals
|
||||||
|
// list with clock correction removed.
|
||||||
|
satelliteResidualsListAndElevation[i] = |
||||||
|
new SatelliteElevationAndResiduals( |
||||||
|
satelliteElevationDegree[i], residuals[i] |
||||||
|
+ positionVelocitySolutionECEF[3], satellitePrn[i]); |
||||||
|
} |
||||||
|
|
||||||
|
double bestUserClockBiasMeters = calculateBestUserClockBias(satelliteResidualsListAndElevation); |
||||||
|
|
||||||
|
// Use the best clock bias to correct the residuals to ensure that the receiver clock errors are
|
||||||
|
// removed from the reported residuals in the analysis
|
||||||
|
double[] correctedResidualsMeters = |
||||||
|
GpsMathOperations.createAndFillArray( |
||||||
|
GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES, Double.NaN |
||||||
|
); |
||||||
|
|
||||||
|
for (SatelliteElevationAndResiduals element : |
||||||
|
satelliteResidualsListAndElevation) { |
||||||
|
correctedResidualsMeters[element.svID - 1] = element.residual - bestUserClockBiasMeters; |
||||||
|
} |
||||||
|
|
||||||
|
return correctedResidualsMeters; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Computes the user clock bias by iteratively averaging the clock bias of top elevation |
||||||
|
* satellites. |
||||||
|
* |
||||||
|
* @param satelliteResidualsAndElevationList a list of satellite elevation and |
||||||
|
* pseudorange residuals |
||||||
|
* @return the corrected best user clock bias |
||||||
|
*/ |
||||||
|
private static double calculateBestUserClockBias( |
||||||
|
SatelliteElevationAndResiduals[] satelliteResidualsAndElevationList) { |
||||||
|
|
||||||
|
// Sort the satellites by descending order of their elevations
|
||||||
|
Arrays.sort( |
||||||
|
satelliteResidualsAndElevationList, |
||||||
|
new Comparator<SatelliteElevationAndResiduals>() { |
||||||
|
@Override |
||||||
|
public int compare( |
||||||
|
SatelliteElevationAndResiduals o1, SatelliteElevationAndResiduals o2) { |
||||||
|
return Double.compare(o2.elevationDegree, o1.elevationDegree); |
||||||
|
} |
||||||
|
}); |
||||||
|
|
||||||
|
// Pick up the top elevation satellites
|
||||||
|
double[] topElevationSatsResiduals = GpsMathOperations.createAndFillArray( |
||||||
|
MIN_SATS_FOR_BIAS_COMPUTATION, Double.NaN |
||||||
|
); |
||||||
|
int numOfUsefulSatsToComputeBias = 0; |
||||||
|
for (int i = 0; i < satelliteResidualsAndElevationList.length |
||||||
|
&& i < topElevationSatsResiduals.length; i++) { |
||||||
|
topElevationSatsResiduals[i] = satelliteResidualsAndElevationList[i].residual; |
||||||
|
numOfUsefulSatsToComputeBias++; |
||||||
|
} |
||||||
|
|
||||||
|
double meanResidual; |
||||||
|
double[] deltaResidualFromMean; |
||||||
|
int maxDeltaIndex = -1; |
||||||
|
|
||||||
|
// Iteratively remove the satellites with highest residuals with respect to the mean of the
|
||||||
|
// residuals until the highest residual in the list is below threshold.
|
||||||
|
do { |
||||||
|
if (maxDeltaIndex >= 0) { |
||||||
|
topElevationSatsResiduals[maxDeltaIndex] = Double.NaN; |
||||||
|
numOfUsefulSatsToComputeBias--; |
||||||
|
} |
||||||
|
meanResidual = GpsMathOperations.meanOfVector(topElevationSatsResiduals); |
||||||
|
deltaResidualFromMean |
||||||
|
= GpsMathOperations.subtractByScalar(topElevationSatsResiduals, meanResidual); |
||||||
|
maxDeltaIndex = GpsMathOperations.maxIndexOfVector(deltaResidualFromMean); |
||||||
|
} while (deltaResidualFromMean[maxDeltaIndex] > BEST_USER_CLOCK_BIAS_RESIDUAL_THRESHOLD_METERS |
||||||
|
&& numOfUsefulSatsToComputeBias > 2); |
||||||
|
|
||||||
|
return meanResidual; |
||||||
|
} |
||||||
|
|
||||||
|
/** A container for satellite residual and elevationDegree information */ |
||||||
|
private static class SatelliteElevationAndResiduals { |
||||||
|
/** Satellite pseudorange or pseudorange rate residual with clock correction removed */ |
||||||
|
final double residual; |
||||||
|
|
||||||
|
/** Satellite elevation in degrees with respect to the user */ |
||||||
|
final double elevationDegree; |
||||||
|
|
||||||
|
/** Satellite ID */ |
||||||
|
final int svID; |
||||||
|
|
||||||
|
SatelliteElevationAndResiduals( |
||||||
|
double elevationDegree, double residual, int svID) { |
||||||
|
this.residual = residual; |
||||||
|
this.svID = svID; |
||||||
|
this.elevationDegree = elevationDegree; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
@ -0,0 +1,193 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
import android.location.cts.nano.Ephemeris.GpsEphemerisProto; |
||||||
|
/** |
||||||
|
* Calculates the GPS satellite clock correction based on parameters observed from the navigation |
||||||
|
* message |
||||||
|
* <p>Source: Page 88 - 90 of the ICD-GPS 200 |
||||||
|
*/ |
||||||
|
public class SatelliteClockCorrectionCalculator { |
||||||
|
private static final double SPEED_OF_LIGHT_MPS = 299792458.0; |
||||||
|
private static final double EARTH_UNIVERSAL_GRAVITATIONAL_CONSTANT_M3_SM2 = 3.986005e14; |
||||||
|
private static final double RELATIVISTIC_CONSTANT_F = -4.442807633e-10; |
||||||
|
private static final int SECONDS_IN_WEEK = 604800; |
||||||
|
private static final double ACCURACY_TOLERANCE = 1.0e-11; |
||||||
|
private static final int MAX_ITERATIONS = 100; |
||||||
|
|
||||||
|
/** |
||||||
|
* Computes the GPS satellite clock correction term in meters iteratively following page 88 - 90 |
||||||
|
* and 98 - 100 of the ICD GPS 200. The method returns a pair of satellite clock correction in |
||||||
|
* meters and Kepler Eccentric Anomaly in Radians. |
||||||
|
* |
||||||
|
* @param ephemerisProto parameters of the navigation message |
||||||
|
* @param receiverGpsTowAtTimeOfTransmission Reciever estimate of GPS time of week when signal was |
||||||
|
* transmitted (seconds) |
||||||
|
* @param receiverGpsWeekAtTimeOfTrasnmission Receiver estimate of GPS week when signal was |
||||||
|
* transmitted (0-1024+) |
||||||
|
* @throws Exception |
||||||
|
*/ |
||||||
|
|
||||||
|
public static SatClockCorrection calculateSatClockCorrAndEccAnomAndTkIteratively( |
||||||
|
GpsEphemerisProto ephemerisProto, double receiverGpsTowAtTimeOfTransmission, |
||||||
|
double receiverGpsWeekAtTimeOfTrasnmission) throws Exception { |
||||||
|
// Units are not added in the variable names to have the same name as the ICD-GPS200
|
||||||
|
// Mean anomaly (radians)
|
||||||
|
double meanAnomalyRad; |
||||||
|
// Kepler's Equation for Eccentric Anomaly iteratively (Radians)
|
||||||
|
double eccentricAnomalyRad; |
||||||
|
// Semi-major axis of orbit (meters)
|
||||||
|
double a = ephemerisProto.rootOfA * ephemerisProto.rootOfA; |
||||||
|
// Computed mean motion (radians/seconds)
|
||||||
|
double n0 = Math.sqrt(EARTH_UNIVERSAL_GRAVITATIONAL_CONSTANT_M3_SM2 / (a * a * a)); |
||||||
|
// Corrected mean motion (radians/seconds)
|
||||||
|
double n = n0 + ephemerisProto.deltaN; |
||||||
|
// In the following, Receiver GPS week and ephemeris GPS week are used to correct for week
|
||||||
|
// rollover when calculating the time from clock reference epoch (tcSec)
|
||||||
|
double timeOfTransmissionIncludingRxWeekSec = |
||||||
|
receiverGpsWeekAtTimeOfTrasnmission * SECONDS_IN_WEEK + receiverGpsTowAtTimeOfTransmission; |
||||||
|
// time from clock reference epoch (seconds) page 88 ICD-GPS200
|
||||||
|
double tcSec = timeOfTransmissionIncludingRxWeekSec |
||||||
|
- (ephemerisProto.week * SECONDS_IN_WEEK + ephemerisProto.toc); |
||||||
|
// Correction for week rollover
|
||||||
|
tcSec = fixWeekRollover(tcSec); |
||||||
|
double oldEcentricAnomalyRad = 0.0; |
||||||
|
double newSatClockCorrectionSeconds = 0.0; |
||||||
|
double relativisticCorrection = 0.0; |
||||||
|
double changeInSatClockCorrection = 0.0; |
||||||
|
// Initial satellite clock correction (unknown relativistic correction). Iterate to correct
|
||||||
|
// with the relativistic effect and obtain a stable
|
||||||
|
final double initSatClockCorrectionSeconds = ephemerisProto.af0 |
||||||
|
+ ephemerisProto.af1 * tcSec |
||||||
|
+ ephemerisProto.af2 * tcSec * tcSec - ephemerisProto.tgd; |
||||||
|
double satClockCorrectionSeconds = initSatClockCorrectionSeconds; |
||||||
|
double tkSec; |
||||||
|
int satClockCorrectionsCounter = 0; |
||||||
|
do { |
||||||
|
int eccentricAnomalyCounter = 0; |
||||||
|
// time from ephemeris reference epoch (seconds) page 98 ICD-GPS200
|
||||||
|
tkSec = timeOfTransmissionIncludingRxWeekSec - ( |
||||||
|
ephemerisProto.week * SECONDS_IN_WEEK + ephemerisProto.toe |
||||||
|
+ satClockCorrectionSeconds); |
||||||
|
// Correction for week rollover
|
||||||
|
tkSec = fixWeekRollover(tkSec); |
||||||
|
// Mean anomaly (radians)
|
||||||
|
meanAnomalyRad = ephemerisProto.m0 + n * tkSec; |
||||||
|
// eccentric anomaly (radians)
|
||||||
|
eccentricAnomalyRad = meanAnomalyRad; |
||||||
|
// Iteratively solve for Kepler's eccentric anomaly according to ICD-GPS200 page 99
|
||||||
|
do { |
||||||
|
oldEcentricAnomalyRad = eccentricAnomalyRad; |
||||||
|
eccentricAnomalyRad = |
||||||
|
meanAnomalyRad + ephemerisProto.e * Math.sin(eccentricAnomalyRad); |
||||||
|
eccentricAnomalyCounter++; |
||||||
|
if (eccentricAnomalyCounter > MAX_ITERATIONS) { |
||||||
|
throw new Exception("Kepler Eccentric Anomaly calculation did not converge in " |
||||||
|
+ MAX_ITERATIONS + " iterations"); |
||||||
|
} |
||||||
|
} while (Math.abs(oldEcentricAnomalyRad - eccentricAnomalyRad) > ACCURACY_TOLERANCE); |
||||||
|
// relativistic correction term (seconds)
|
||||||
|
relativisticCorrection = RELATIVISTIC_CONSTANT_F * ephemerisProto.e |
||||||
|
* ephemerisProto.rootOfA * Math.sin(eccentricAnomalyRad); |
||||||
|
// satellite clock correction including relativistic effect
|
||||||
|
newSatClockCorrectionSeconds = initSatClockCorrectionSeconds + relativisticCorrection; |
||||||
|
changeInSatClockCorrection = |
||||||
|
Math.abs(satClockCorrectionSeconds - newSatClockCorrectionSeconds); |
||||||
|
satClockCorrectionSeconds = newSatClockCorrectionSeconds; |
||||||
|
satClockCorrectionsCounter++; |
||||||
|
if (satClockCorrectionsCounter > MAX_ITERATIONS) { |
||||||
|
throw new Exception("Satellite Clock Correction calculation did not converge in " |
||||||
|
+ MAX_ITERATIONS + " iterations"); |
||||||
|
} |
||||||
|
} while (changeInSatClockCorrection > ACCURACY_TOLERANCE); |
||||||
|
tkSec = timeOfTransmissionIncludingRxWeekSec - ( |
||||||
|
ephemerisProto.week * SECONDS_IN_WEEK + ephemerisProto.toe |
||||||
|
+ satClockCorrectionSeconds); |
||||||
|
// return satellite clock correction (meters) and Kepler Eccentric Anomaly in Radians
|
||||||
|
return new SatClockCorrection(satClockCorrectionSeconds * SPEED_OF_LIGHT_MPS, |
||||||
|
eccentricAnomalyRad, tkSec); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculates Satellite Clock Error Rate in (meters/second) by subtracting the Satellite |
||||||
|
* Clock Error Values at t+0.5s and t-0.5s. |
||||||
|
* |
||||||
|
* <p>This approximation is more accurate than differentiating because both the orbital |
||||||
|
* and relativity terms have non-linearities that are not easily differentiable. |
||||||
|
*/ |
||||||
|
public static double calculateSatClockCorrErrorRate( |
||||||
|
GpsEphemerisProto ephemerisProto, double receiverGpsTowAtTimeOfTransmissionSeconds, |
||||||
|
double receiverGpsWeekAtTimeOfTrasnmission) throws Exception { |
||||||
|
SatClockCorrection satClockCorrectionPlus = calculateSatClockCorrAndEccAnomAndTkIteratively( |
||||||
|
ephemerisProto, receiverGpsTowAtTimeOfTransmissionSeconds + 0.5, |
||||||
|
receiverGpsWeekAtTimeOfTrasnmission); |
||||||
|
SatClockCorrection satClockCorrectionMinus = calculateSatClockCorrAndEccAnomAndTkIteratively( |
||||||
|
ephemerisProto, receiverGpsTowAtTimeOfTransmissionSeconds - 0.5, |
||||||
|
receiverGpsWeekAtTimeOfTrasnmission); |
||||||
|
double satelliteClockErrorRate = satClockCorrectionPlus.satelliteClockCorrectionMeters |
||||||
|
- satClockCorrectionMinus.satelliteClockCorrectionMeters; |
||||||
|
return satelliteClockErrorRate; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Method to check for week rollover according to ICD-GPS 200 page 98. |
||||||
|
* |
||||||
|
* <p>Result should be between -302400 and 302400 if the ephemeris is within one week of |
||||||
|
* transmission, otherwise it is adjusted to the correct range |
||||||
|
*/ |
||||||
|
private static double fixWeekRollover(double time) { |
||||||
|
double correctedTime = time; |
||||||
|
if (time > SECONDS_IN_WEEK / 2.0) { |
||||||
|
correctedTime = time - SECONDS_IN_WEEK; |
||||||
|
} |
||||||
|
if (time < -SECONDS_IN_WEEK / 2.0) { |
||||||
|
correctedTime = time + SECONDS_IN_WEEK; |
||||||
|
} |
||||||
|
return correctedTime; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* |
||||||
|
* Class containing the satellite clock correction parameters: The satellite clock correction in |
||||||
|
* meters, Kepler Eccentric Anomaly in Radians and the time from the reference epoch in seconds. |
||||||
|
*/ |
||||||
|
public static class SatClockCorrection { |
||||||
|
/** |
||||||
|
* Satellite clock correction in meters |
||||||
|
*/ |
||||||
|
public final double satelliteClockCorrectionMeters; |
||||||
|
/** |
||||||
|
* Kepler Eccentric Anomaly in Radians |
||||||
|
*/ |
||||||
|
public final double eccentricAnomalyRadians; |
||||||
|
/** |
||||||
|
* Time from the reference epoch in Seconds |
||||||
|
*/ |
||||||
|
public final double timeFromRefEpochSec; |
||||||
|
|
||||||
|
/** |
||||||
|
* Constructor |
||||||
|
*/ |
||||||
|
public SatClockCorrection(double satelliteClockCorrectionMeters, double eccentricAnomalyRadians, |
||||||
|
double timeFromRefEpochSec) { |
||||||
|
this.satelliteClockCorrectionMeters = satelliteClockCorrectionMeters; |
||||||
|
this.eccentricAnomalyRadians = eccentricAnomalyRadians; |
||||||
|
this.timeFromRefEpochSec = timeFromRefEpochSec; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,323 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.SatelliteClockCorrectionCalculator.SatClockCorrection; |
||||||
|
import android.location.cts.nano.Ephemeris.GpsEphemerisProto; |
||||||
|
|
||||||
|
/** |
||||||
|
* Class to calculate GPS satellite positions from the ephemeris data |
||||||
|
*/ |
||||||
|
public class SatellitePositionCalculator { |
||||||
|
private static final double SPEED_OF_LIGHT_MPS = 299792458.0; |
||||||
|
private static final double UNIVERSAL_GRAVITATIONAL_PARAMETER_M3_SM2 = 3.986005e14; |
||||||
|
private static final int NUMBER_OF_ITERATIONS_FOR_SAT_POS_CALCULATION = 5; |
||||||
|
private static final double EARTH_ROTATION_RATE_RAD_PER_SEC = 7.2921151467e-5; |
||||||
|
|
||||||
|
/** |
||||||
|
* |
||||||
|
* Calculates GPS satellite position and velocity from ephemeris including the Sagnac effect |
||||||
|
* starting from unknown user to satellite distance and speed. So we start from an initial guess |
||||||
|
* of the user to satellite range and range rate and iterate to include the Sagnac effect. Few |
||||||
|
* iterations are enough to achieve a satellite position with millimeter accuracy. |
||||||
|
* A {@code PositionAndVelocity} class is returned containing satellite position in meters |
||||||
|
* (x, y and z) and velocity in meters per second (x, y, z) |
||||||
|
* |
||||||
|
* <p>Satelite position and velocity equations are obtained from: |
||||||
|
* http://www.gps.gov/technical/icwg/ICD-GPS-200C.pdf) pages 94 - 101 and
|
||||||
|
* http://fenrir.naruoka.org/download/autopilot/note/080205_gps/gps_velocity.pdf
|
||||||
|
* |
||||||
|
* @param ephemerisProto parameters of the navigation message |
||||||
|
* @param receiverGpsTowAtTimeOfTransmissionCorrectedSec Receiver estimate of GPS time of week |
||||||
|
* when signal was transmitted corrected with the satellite clock drift (seconds) |
||||||
|
* @param receiverGpsWeekAtTimeOfTransmission Receiver estimate of GPS week when signal was |
||||||
|
* transmitted (0-1024+) |
||||||
|
* @param userPosXMeters Last known user x-position (if known) [meters] |
||||||
|
* @param userPosYMeters Last known user y-position (if known) [meters] |
||||||
|
* @param userPosZMeters Last known user z-position (if known) [meters] |
||||||
|
* @throws Exception |
||||||
|
*/ |
||||||
|
public static PositionAndVelocity calculateSatellitePositionAndVelocityFromEphemeris |
||||||
|
(GpsEphemerisProto ephemerisProto, double receiverGpsTowAtTimeOfTransmissionCorrectedSec, |
||||||
|
int receiverGpsWeekAtTimeOfTransmission, |
||||||
|
double userPosXMeters, |
||||||
|
double userPosYMeters, |
||||||
|
double userPosZMeters) throws Exception { |
||||||
|
|
||||||
|
// lets start with a first user to sat distance guess of 70 ms and zero velocity
|
||||||
|
RangeAndRangeRate userSatRangeAndRate = new RangeAndRangeRate |
||||||
|
(0.070 * SPEED_OF_LIGHT_MPS, 0.0 /* range rate*/); |
||||||
|
|
||||||
|
// To apply sagnac effect correction, We are starting from an approximate guess of the user to
|
||||||
|
// satellite range, iterate 3 times and that should be enough to reach millimeter accuracy
|
||||||
|
PositionAndVelocity satPosAndVel = new PositionAndVelocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); |
||||||
|
PositionAndVelocity userPosAndVel = |
||||||
|
new PositionAndVelocity(userPosXMeters, userPosYMeters, userPosZMeters, |
||||||
|
0.0 /* user velocity x*/, 0.0 /* user velocity y*/, 0.0 /* user velocity z */); |
||||||
|
for (int i = 0; i < NUMBER_OF_ITERATIONS_FOR_SAT_POS_CALCULATION; i++) { |
||||||
|
calculateSatellitePositionAndVelocity(ephemerisProto, |
||||||
|
receiverGpsTowAtTimeOfTransmissionCorrectedSec, receiverGpsWeekAtTimeOfTransmission, |
||||||
|
userSatRangeAndRate, satPosAndVel); |
||||||
|
computeUserToSatelliteRangeAndRangeRate(userPosAndVel, satPosAndVel, userSatRangeAndRate); |
||||||
|
} |
||||||
|
return satPosAndVel; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculates GPS satellite position and velocity from ephemeris based on the ICD-GPS-200. |
||||||
|
* Satellite position in meters (x, y and z) and velocity in meters per second (x, y, z) are set |
||||||
|
* in the passed {@code PositionAndVelocity} instance. |
||||||
|
* |
||||||
|
* <p>Sources: http://www.gps.gov/technical/icwg/ICD-GPS-200C.pdf) pages 94 - 101 and
|
||||||
|
* http://fenrir.naruoka.org/download/autopilot/note/080205_gps/gps_velocity.pdf
|
||||||
|
* |
||||||
|
* @param ephemerisProto parameters of the navigation message |
||||||
|
* @param receiverGpsTowAtTimeOfTransmissionCorrected Receiver estimate of GPS time of week when |
||||||
|
* signal was transmitted corrected with the satellite clock drift (seconds) |
||||||
|
* @param receiverGpsWeekAtTimeOfTransmission Receiver estimate of GPS week when signal was |
||||||
|
* transmitted (0-1024+) |
||||||
|
* @param userSatRangeAndRate user to satellite range and range rate |
||||||
|
* @param satPosAndVel Satellite position and velocity instance in which the method results will |
||||||
|
* be set |
||||||
|
* @throws Exception |
||||||
|
*/ |
||||||
|
public static void calculateSatellitePositionAndVelocity(GpsEphemerisProto ephemerisProto, |
||||||
|
double receiverGpsTowAtTimeOfTransmissionCorrected, int receiverGpsWeekAtTimeOfTransmission, |
||||||
|
RangeAndRangeRate userSatRangeAndRate, PositionAndVelocity satPosAndVel) throws Exception { |
||||||
|
|
||||||
|
// Calculate satellite clock correction (meters), Kepler Eccentric anomaly (radians) and time
|
||||||
|
// from ephemeris refrence epoch (tkSec) iteratively
|
||||||
|
SatClockCorrection satClockCorrectionValues = |
||||||
|
SatelliteClockCorrectionCalculator.calculateSatClockCorrAndEccAnomAndTkIteratively( |
||||||
|
ephemerisProto, receiverGpsTowAtTimeOfTransmissionCorrected, |
||||||
|
receiverGpsWeekAtTimeOfTransmission); |
||||||
|
|
||||||
|
double eccentricAnomalyRadians = satClockCorrectionValues.eccentricAnomalyRadians; |
||||||
|
double tkSec = satClockCorrectionValues.timeFromRefEpochSec; |
||||||
|
|
||||||
|
// True_anomaly (angle from perigee)
|
||||||
|
double trueAnomalyRadians = Math.atan2( |
||||||
|
Math.sqrt(1.0 - ephemerisProto.e * ephemerisProto.e) |
||||||
|
* Math.sin(eccentricAnomalyRadians), |
||||||
|
Math.cos(eccentricAnomalyRadians) - ephemerisProto.e); |
||||||
|
|
||||||
|
// Argument of latitude of the satellite
|
||||||
|
double argumentOfLatitudeRadians = trueAnomalyRadians + ephemerisProto.omega; |
||||||
|
|
||||||
|
// Radius of satellite orbit
|
||||||
|
double radiusOfSatelliteOrbitMeters = ephemerisProto.rootOfA * ephemerisProto.rootOfA |
||||||
|
* (1.0 - ephemerisProto.e * Math.cos(eccentricAnomalyRadians)); |
||||||
|
|
||||||
|
// Radius correction due to second harmonic perturbations of the orbit
|
||||||
|
double radiusCorrectionMeters = ephemerisProto.crc |
||||||
|
* Math.cos(2.0 * argumentOfLatitudeRadians) + ephemerisProto.crs |
||||||
|
* Math.sin(2.0 * argumentOfLatitudeRadians); |
||||||
|
// Argument of latitude correction due to second harmonic perturbations of the orbit
|
||||||
|
double argumentOfLatitudeCorrectionRadians = ephemerisProto.cuc |
||||||
|
* Math.cos(2.0 * argumentOfLatitudeRadians) + ephemerisProto.cus |
||||||
|
* Math.sin(2.0 * argumentOfLatitudeRadians); |
||||||
|
// Correction to inclination due to second harmonic perturbations of the orbit
|
||||||
|
double inclinationCorrectionRadians = ephemerisProto.cic |
||||||
|
* Math.cos(2.0 * argumentOfLatitudeRadians) + ephemerisProto.cis |
||||||
|
* Math.sin(2.0 * argumentOfLatitudeRadians); |
||||||
|
|
||||||
|
// Corrected radius of satellite orbit
|
||||||
|
radiusOfSatelliteOrbitMeters += radiusCorrectionMeters; |
||||||
|
// Corrected argument of latitude
|
||||||
|
argumentOfLatitudeRadians += argumentOfLatitudeCorrectionRadians; |
||||||
|
// Corrected inclination
|
||||||
|
double inclinationRadians = |
||||||
|
ephemerisProto.i0 + inclinationCorrectionRadians + ephemerisProto.iDot * tkSec; |
||||||
|
|
||||||
|
// Position in orbital plane
|
||||||
|
double xPositionMeters = radiusOfSatelliteOrbitMeters * Math.cos(argumentOfLatitudeRadians); |
||||||
|
double yPositionMeters = radiusOfSatelliteOrbitMeters * Math.sin(argumentOfLatitudeRadians); |
||||||
|
|
||||||
|
// Corrected longitude of the ascending node (signal propagation time is included to compensate
|
||||||
|
// for the Sagnac effect)
|
||||||
|
double omegaKRadians = ephemerisProto.omega0 |
||||||
|
+ (ephemerisProto.omegaDot - EARTH_ROTATION_RATE_RAD_PER_SEC) * tkSec |
||||||
|
- EARTH_ROTATION_RATE_RAD_PER_SEC |
||||||
|
* (ephemerisProto.toe + userSatRangeAndRate.rangeMeters / SPEED_OF_LIGHT_MPS); |
||||||
|
|
||||||
|
// compute the resulting satellite position
|
||||||
|
double satPosXMeters = xPositionMeters * Math.cos(omegaKRadians) - yPositionMeters |
||||||
|
* Math.cos(inclinationRadians) * Math.sin(omegaKRadians); |
||||||
|
double satPosYMeters = xPositionMeters * Math.sin(omegaKRadians) + yPositionMeters |
||||||
|
* Math.cos(inclinationRadians) * Math.cos(omegaKRadians); |
||||||
|
double satPosZMeters = yPositionMeters * Math.sin(inclinationRadians); |
||||||
|
|
||||||
|
// Satellite Velocity Computation using the broadcast ephemeris
|
||||||
|
// http://fenrir.naruoka.org/download/autopilot/note/080205_gps/gps_velocity.pdf
|
||||||
|
// Units are not added in some of the variable names to have the same name as the ICD-GPS200
|
||||||
|
// Semi-major axis of orbit (meters)
|
||||||
|
double a = ephemerisProto.rootOfA * ephemerisProto.rootOfA; |
||||||
|
// Computed mean motion (radians/seconds)
|
||||||
|
double n0 = Math.sqrt(UNIVERSAL_GRAVITATIONAL_PARAMETER_M3_SM2 / (a * a * a)); |
||||||
|
// Corrected mean motion (radians/seconds)
|
||||||
|
double n = n0 + ephemerisProto.deltaN; |
||||||
|
// Derivative of mean anomaly (radians/seconds)
|
||||||
|
double meanAnomalyDotRadPerSec = n; |
||||||
|
// Derivative of eccentric anomaly (radians/seconds)
|
||||||
|
double eccentricAnomalyDotRadPerSec = |
||||||
|
meanAnomalyDotRadPerSec / (1.0 - ephemerisProto.e * Math.cos(eccentricAnomalyRadians)); |
||||||
|
// Derivative of true anomaly (radians/seconds)
|
||||||
|
double trueAnomalydotRadPerSec = Math.sin(eccentricAnomalyRadians) |
||||||
|
* eccentricAnomalyDotRadPerSec |
||||||
|
* (1.0 + ephemerisProto.e * Math.cos(trueAnomalyRadians)) / ( |
||||||
|
Math.sin(trueAnomalyRadians) |
||||||
|
* (1.0 - ephemerisProto.e * Math.cos(eccentricAnomalyRadians))); |
||||||
|
// Derivative of argument of latitude (radians/seconds)
|
||||||
|
double argumentOfLatitudeDotRadPerSec = trueAnomalydotRadPerSec + 2.0 * (ephemerisProto.cus |
||||||
|
* Math.cos(2.0 * argumentOfLatitudeRadians) - ephemerisProto.cuc |
||||||
|
* Math.sin(2.0 * argumentOfLatitudeRadians)) * trueAnomalydotRadPerSec; |
||||||
|
// Derivative of radius of satellite orbit (m/s)
|
||||||
|
double radiusOfSatelliteOrbitDotMPerSec = a * ephemerisProto.e |
||||||
|
* Math.sin(eccentricAnomalyRadians) * n |
||||||
|
/ (1.0 - ephemerisProto.e * Math.cos(eccentricAnomalyRadians)) + 2.0 * ( |
||||||
|
ephemerisProto.crs * Math.cos(2.0 * argumentOfLatitudeRadians) |
||||||
|
- ephemerisProto.crc * Math.sin(2.0 * argumentOfLatitudeRadians)) |
||||||
|
* trueAnomalydotRadPerSec; |
||||||
|
// Derivative of the inclination (radians/seconds)
|
||||||
|
double inclinationDotRadPerSec = ephemerisProto.iDot + (ephemerisProto.cis |
||||||
|
* Math.cos(2.0 * argumentOfLatitudeRadians) - ephemerisProto.cic |
||||||
|
* Math.sin(2.0 * argumentOfLatitudeRadians)) * 2.0 * trueAnomalydotRadPerSec; |
||||||
|
|
||||||
|
double xVelocityMPS = radiusOfSatelliteOrbitDotMPerSec * Math.cos(argumentOfLatitudeRadians) |
||||||
|
- yPositionMeters * argumentOfLatitudeDotRadPerSec; |
||||||
|
double yVelocityMPS = radiusOfSatelliteOrbitDotMPerSec * Math.sin(argumentOfLatitudeRadians) |
||||||
|
+ xPositionMeters * argumentOfLatitudeDotRadPerSec; |
||||||
|
|
||||||
|
// Corrected rate of right ascension including compensation for the Sagnac effect
|
||||||
|
double omegaDotRadPerSec = ephemerisProto.omegaDot - EARTH_ROTATION_RATE_RAD_PER_SEC |
||||||
|
* (1.0 + userSatRangeAndRate.rangeRateMetersPerSec / SPEED_OF_LIGHT_MPS); |
||||||
|
// compute the resulting satellite velocity
|
||||||
|
double satVelXMPS = |
||||||
|
(xVelocityMPS - yPositionMeters * Math.cos(inclinationRadians) * omegaDotRadPerSec) |
||||||
|
* Math.cos(omegaKRadians) - (xPositionMeters * omegaDotRadPerSec + yVelocityMPS |
||||||
|
* Math.cos(inclinationRadians) - yPositionMeters * Math.sin(inclinationRadians) |
||||||
|
* inclinationDotRadPerSec) * Math.sin(omegaKRadians); |
||||||
|
double satVelYMPS = |
||||||
|
(xVelocityMPS - yPositionMeters * Math.cos(inclinationRadians) * omegaDotRadPerSec) |
||||||
|
* Math.sin(omegaKRadians) + (xPositionMeters * omegaDotRadPerSec + yVelocityMPS |
||||||
|
* Math.cos(inclinationRadians) - yPositionMeters * Math.sin(inclinationRadians) |
||||||
|
* inclinationDotRadPerSec) * Math.cos(omegaKRadians); |
||||||
|
double satVelZMPS = yVelocityMPS * Math.sin(inclinationRadians) + yPositionMeters |
||||||
|
* Math.cos(inclinationRadians) * inclinationDotRadPerSec; |
||||||
|
|
||||||
|
satPosAndVel.positionXMeters = satPosXMeters; |
||||||
|
satPosAndVel.positionYMeters = satPosYMeters; |
||||||
|
satPosAndVel.positionZMeters = satPosZMeters; |
||||||
|
satPosAndVel.velocityXMetersPerSec = satVelXMPS; |
||||||
|
satPosAndVel.velocityYMetersPerSec = satVelYMPS; |
||||||
|
satPosAndVel.velocityZMetersPerSec = satVelZMPS; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Computes and sets the passed {@code RangeAndRangeRate} instance containing user to satellite |
||||||
|
* range (meters) and range rate (m/s) given the user position (ECEF meters), user velocity (m/s), |
||||||
|
* satellite position (ECEF meters) and satellite velocity (m/s). |
||||||
|
*/ |
||||||
|
private static void computeUserToSatelliteRangeAndRangeRate(PositionAndVelocity userPosAndVel, |
||||||
|
PositionAndVelocity satPosAndVel, RangeAndRangeRate rangeAndRangeRate) { |
||||||
|
double dXMeters = satPosAndVel.positionXMeters - userPosAndVel.positionXMeters; |
||||||
|
double dYMeters = satPosAndVel.positionYMeters - userPosAndVel.positionYMeters; |
||||||
|
double dZMeters = satPosAndVel.positionZMeters - userPosAndVel.positionZMeters; |
||||||
|
// range in meters
|
||||||
|
double rangeMeters = Math.sqrt(dXMeters * dXMeters + dYMeters * dYMeters + dZMeters * dZMeters); |
||||||
|
// range rate in meters / second
|
||||||
|
double rangeRateMetersPerSec = |
||||||
|
((userPosAndVel.velocityXMetersPerSec - satPosAndVel.velocityXMetersPerSec) * dXMeters |
||||||
|
+ (userPosAndVel.velocityYMetersPerSec - satPosAndVel.velocityYMetersPerSec) * dYMeters |
||||||
|
+ (userPosAndVel.velocityZMetersPerSec - satPosAndVel.velocityZMetersPerSec) * dZMeters) |
||||||
|
/ rangeMeters; |
||||||
|
rangeAndRangeRate.rangeMeters = rangeMeters; |
||||||
|
rangeAndRangeRate.rangeRateMetersPerSec = rangeRateMetersPerSec; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* |
||||||
|
* A class containing position values (x, y, z) in meters and velocity values (x, y, z) in meters |
||||||
|
* per seconds |
||||||
|
*/ |
||||||
|
public static class PositionAndVelocity { |
||||||
|
/** |
||||||
|
* x - position in meters |
||||||
|
*/ |
||||||
|
public double positionXMeters; |
||||||
|
/** |
||||||
|
* y - position in meters |
||||||
|
*/ |
||||||
|
public double positionYMeters; |
||||||
|
/** |
||||||
|
* z - position in meters |
||||||
|
*/ |
||||||
|
public double positionZMeters; |
||||||
|
/** |
||||||
|
* x - velocity in meters |
||||||
|
*/ |
||||||
|
public double velocityXMetersPerSec; |
||||||
|
/** |
||||||
|
* y - velocity in meters |
||||||
|
*/ |
||||||
|
public double velocityYMetersPerSec; |
||||||
|
/** |
||||||
|
* z - velocity in meters |
||||||
|
*/ |
||||||
|
public double velocityZMetersPerSec; |
||||||
|
|
||||||
|
/** |
||||||
|
* Constructor |
||||||
|
*/ |
||||||
|
public PositionAndVelocity(double positionXMeters, |
||||||
|
double positionYMeters, |
||||||
|
double positionZMeters, |
||||||
|
double velocityXMetersPerSec, |
||||||
|
double velocityYMetersPerSec, |
||||||
|
double velocityZMetersPerSec) { |
||||||
|
this.positionXMeters = positionXMeters; |
||||||
|
this.positionYMeters = positionYMeters; |
||||||
|
this.positionZMeters = positionZMeters; |
||||||
|
this.velocityXMetersPerSec = velocityXMetersPerSec; |
||||||
|
this.velocityYMetersPerSec = velocityYMetersPerSec; |
||||||
|
this.velocityZMetersPerSec = velocityZMetersPerSec; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* |
||||||
|
* A class containing range of satellite to user in meters and range rate in meters per seconds |
||||||
|
*/ |
||||||
|
public static class RangeAndRangeRate { |
||||||
|
/** |
||||||
|
* Range in meters |
||||||
|
*/ |
||||||
|
public double rangeMeters; |
||||||
|
/** |
||||||
|
* Range rate in meters per seconds |
||||||
|
*/ |
||||||
|
public double rangeRateMetersPerSec; |
||||||
|
|
||||||
|
/** |
||||||
|
* Constructor |
||||||
|
*/ |
||||||
|
public RangeAndRangeRate(double rangeMeters, double rangeRateMetersPerSec) { |
||||||
|
this.rangeMeters = rangeMeters; |
||||||
|
this.rangeRateMetersPerSec = rangeRateMetersPerSec; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,330 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculate the troposheric delay based on the ENGOS Tropospheric model. |
||||||
|
* |
||||||
|
* <p>The tropospheric delay is modeled as a combined effect of the delay experienced due to |
||||||
|
* hyrostatic (dry) and wet components of the troposphere. Both delays experienced at zenith are |
||||||
|
* scaled with a mapping function to get the delay at any specific elevation. |
||||||
|
* |
||||||
|
* <p>The tropospheric model algorithm of EGNOS model by Penna, N., A. Dodson and W. Chen (2001) |
||||||
|
* (http://espace.library.curtin.edu.au/cgi-bin/espace.pdf?file=/2008/11/13/file_1/18917) is used
|
||||||
|
* for calculating the zenith delays. In this model, the weather parameters are extracted using |
||||||
|
* interpolation from lookup table derived from the US Standard Atmospheric Supplements, 1966. |
||||||
|
* |
||||||
|
* <p>A close form mapping function is built using Guo and Langley, 2003 |
||||||
|
* (http://gauss2.gge.unb.ca/papers.pdf/iongpsgnss2003.guo.pdf) which is able to calculate accurate
|
||||||
|
* mapping down to 2 degree elevations. |
||||||
|
* |
||||||
|
* <p>Sources: |
||||||
|
* <p>http://espace.library.curtin.edu.au/cgi-bin/espace.pdf?file=/2008/11/13/file_1/18917
|
||||||
|
* <p>- http://www.academia.edu/3512180/Assessment_of_UNB3M_neutral
|
||||||
|
* _atmosphere_model_and_EGNOS_model_for_near-equatorial-tropospheric_delay_correction |
||||||
|
* <p>- http://gauss.gge.unb.ca/papers.pdf/ion52am.collins.pdf
|
||||||
|
* <p>- http://www.navipedia.net/index.php/Tropospheric_Delay#cite_ref-3
|
||||||
|
* <p>Hydrostatic and non-hydrostatic mapping functions are obtained from: |
||||||
|
* http://gauss2.gge.unb.ca/papers.pdf/iongpsgnss2003.guo.pdf
|
||||||
|
* |
||||||
|
*/ |
||||||
|
public class TroposphericModelEgnos { |
||||||
|
// parameters of the EGNOS models
|
||||||
|
private static final int INDEX_15_DEGREES = 0; |
||||||
|
private static final int INDEX_75_DEGREES = 4; |
||||||
|
private static final int LATITUDE_15_DEGREES = 15; |
||||||
|
private static final int LATITUDE_75_DEGREES = 75; |
||||||
|
// Lookup Average parameters
|
||||||
|
// Troposphere average presssure mbar
|
||||||
|
private static final double[] latDegreeToPressureMbarAvgMap = |
||||||
|
{1013.25, 1017.25, 1015.75, 1011.75, 1013.0}; |
||||||
|
// Troposphere average temperature Kelvin
|
||||||
|
private static final double[] latDegreeToTempKelvinAvgMap = |
||||||
|
{299.65, 294.15, 283.15, 272.15, 263.65}; |
||||||
|
// Troposphere average wator vapor pressure
|
||||||
|
private static final double[] latDegreeToWVPressureMbarAvgMap = {26.31, 21.79, 11.66, 6.78, 4.11}; |
||||||
|
// Troposphere average temperature lapse rate K/m
|
||||||
|
private static final double[] latDegreeToBetaAvgMapKPM = |
||||||
|
{6.30e-3, 6.05e-3, 5.58e-3, 5.39e-3, 4.53e-3}; |
||||||
|
// Troposphere average water vapor lapse rate (dimensionless)
|
||||||
|
private static final double[] latDegreeToLampdaAvgMap = {2.77, 3.15, 2.57, 1.81, 1.55}; |
||||||
|
|
||||||
|
// Lookup Amplitude parameters
|
||||||
|
// Troposphere amplitude presssure mbar
|
||||||
|
private static final double[] latDegreeToPressureMbarAmpMap = {0.0, -3.75, -2.25, -1.75, -0.5}; |
||||||
|
// Troposphere amplitude temperature Kelvin
|
||||||
|
private static final double[] latDegreeToTempKelvinAmpMap = {0.0, 7.0, 11.0, 15.0, 14.5}; |
||||||
|
// Troposphere amplitude wator vapor pressure
|
||||||
|
private static final double[] latDegreeToWVPressureMbarAmpMap = {0.0, 8.85, 7.24, 5.36, 3.39}; |
||||||
|
// Troposphere amplitude temperature lapse rate K/m
|
||||||
|
private static final double[] latDegreeToBetaAmpMapKPM = |
||||||
|
{0.0, 0.25e-3, 0.32e-3, 0.81e-3, 0.62e-3}; |
||||||
|
// Troposphere amplitude water vapor lapse rate (dimensionless)
|
||||||
|
private static final double[] latDegreeToLampdaAmpMap = {0.0, 0.33, 0.46, 0.74, 0.30}; |
||||||
|
// Zenith delay dry constant K/mbar
|
||||||
|
private static final double K1 = 77.604; |
||||||
|
// Zenith delay wet constant K^2/mbar
|
||||||
|
private static final double K2 = 382000.0; |
||||||
|
// gas constant for dry air J/kg/K
|
||||||
|
private static final double RD = 287.054; |
||||||
|
// Acceleration of gravity at the atmospheric column centroid m/s^-2
|
||||||
|
private static final double GM = 9.784; |
||||||
|
// Gravity m/s^2
|
||||||
|
private static final double GRAVITY_MPS2 = 9.80665; |
||||||
|
|
||||||
|
private static final double MINIMUM_INTERPOLATION_THRESHOLD = 1e-25; |
||||||
|
private static final double B_HYDROSTATIC = 0.0035716; |
||||||
|
private static final double C_HYDROSTATIC = 0.082456; |
||||||
|
private static final double B_NON_HYDROSTATIC = 0.0018576; |
||||||
|
private static final double C_NON_HYDROSTATIC = 0.062741; |
||||||
|
private static final double SOUTHERN_HEMISPHERE_DMIN = 211.0; |
||||||
|
private static final double NORTHERN_HEMISPHERE_DMIN = 28.0; |
||||||
|
// Days recalling that every fourth year is a leap year and has an extra day - February 29th
|
||||||
|
private static final double DAYS_PER_YEAR = 365.25; |
||||||
|
|
||||||
|
/** |
||||||
|
* Computes the tropospheric correction in meters given the satellite elevation in radians, the |
||||||
|
* user latitude in radians, the user Orthometric height above sea level in meters and the day of |
||||||
|
* the year. |
||||||
|
* |
||||||
|
* <p>Dry and wet delay zenith delay components are calculated and then scaled with the mapping |
||||||
|
* function at the given satellite elevation. |
||||||
|
* |
||||||
|
*/ |
||||||
|
public static double calculateTropoCorrectionMeters(double satElevationRadians, |
||||||
|
double userLatitudeRadian, double heightMetersAboveSeaLevel, int dayOfYear1To366) { |
||||||
|
DryAndWetMappingValues dryAndWetMappingValues = |
||||||
|
computeDryAndWetMappingValuesUsingUNBabcMappingFunction(satElevationRadians, |
||||||
|
userLatitudeRadian, heightMetersAboveSeaLevel); |
||||||
|
DryAndWetZenithDelays dryAndWetZenithDelays = calculateZenithDryAndWetDelaysSec |
||||||
|
(userLatitudeRadian, heightMetersAboveSeaLevel, dayOfYear1To366); |
||||||
|
|
||||||
|
double drydelaySeconds = |
||||||
|
dryAndWetZenithDelays.dryZenithDelaySec * dryAndWetMappingValues.dryMappingValue; |
||||||
|
double wetdelaySeconds = |
||||||
|
dryAndWetZenithDelays.wetZenithDelaySec * dryAndWetMappingValues.wetMappingValue; |
||||||
|
return drydelaySeconds + wetdelaySeconds; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Computes the dry and wet mapping values based on the University of Brunswick UNBabc model. The |
||||||
|
* mapping function inputs are satellite elevation in radians, user latitude in radians and user |
||||||
|
* orthometric height above sea level in meters. The function returns |
||||||
|
* {@code DryAndWetMappingValues} containing dry and wet mapping values. |
||||||
|
* |
||||||
|
* <p>From the many dry and wet mapping functions of components of the troposphere, the method |
||||||
|
* from the University of Brunswick in Canada was selected due to its reasonable computation time |
||||||
|
* and accuracy with satellites as low as 2 degrees elevation. |
||||||
|
* <p>Source: http://gauss2.gge.unb.ca/papers.pdf/iongpsgnss2003.guo.pdf
|
||||||
|
*/ |
||||||
|
private static DryAndWetMappingValues computeDryAndWetMappingValuesUsingUNBabcMappingFunction( |
||||||
|
double satElevationRadians, double userLatitudeRadians, double heightMetersAboveSeaLevel) { |
||||||
|
|
||||||
|
if (satElevationRadians > Math.PI / 2.0) { |
||||||
|
satElevationRadians = Math.PI / 2.0; |
||||||
|
} else if (satElevationRadians < 2.0 * Math.PI / 180.0) { |
||||||
|
satElevationRadians = Math.toRadians(2.0); |
||||||
|
} |
||||||
|
|
||||||
|
// dry components mapping parameters
|
||||||
|
double aHidrostatic = (1.18972 - 0.026855 * heightMetersAboveSeaLevel / 1000.0 + 0.10664 |
||||||
|
* Math.cos(userLatitudeRadians)) / 1000.0; |
||||||
|
|
||||||
|
|
||||||
|
double numeratorDry = 1.0 + (aHidrostatic / (1.0 + (B_HYDROSTATIC / (1.0 + C_HYDROSTATIC)))); |
||||||
|
double denominatorDry = Math.sin(satElevationRadians) + (aHidrostatic / ( |
||||||
|
Math.sin(satElevationRadians) |
||||||
|
+ (B_HYDROSTATIC / (Math.sin(satElevationRadians) + C_HYDROSTATIC)))); |
||||||
|
|
||||||
|
double drymap = numeratorDry / denominatorDry; |
||||||
|
|
||||||
|
// wet components mapping parameters
|
||||||
|
double aNonHydrostatic = (0.61120 - 0.035348 * heightMetersAboveSeaLevel / 1000.0 - 0.01526 |
||||||
|
* Math.cos(userLatitudeRadians)) / 1000.0; |
||||||
|
|
||||||
|
|
||||||
|
double numeratorWet = |
||||||
|
1.0 + (aNonHydrostatic / (1.0 + (B_NON_HYDROSTATIC / (1.0 + C_NON_HYDROSTATIC)))); |
||||||
|
double denominatorWet = Math.sin(satElevationRadians) + (aNonHydrostatic / ( |
||||||
|
Math.sin(satElevationRadians) |
||||||
|
+ (B_NON_HYDROSTATIC / (Math.sin(satElevationRadians) + C_NON_HYDROSTATIC)))); |
||||||
|
|
||||||
|
double wetmap = numeratorWet / denominatorWet; |
||||||
|
return new DryAndWetMappingValues(drymap, wetmap); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Computes the combined effect of the delay at zenith experienced due to hyrostatic (dry) and wet |
||||||
|
* components of the troposphere. The function inputs are the user latitude in radians, user |
||||||
|
* orthometric height above sea level in meters and the day of the year (1-366). The function |
||||||
|
* returns a {@code DryAndWetZenithDelays} containing dry and wet delays at zenith. |
||||||
|
* |
||||||
|
* <p>EGNOS Tropospheric model by Penna et al. (2001) is used in this case. |
||||||
|
* (http://espace.library.curtin.edu.au/cgi-bin/espace.pdf?file=/2008/11/13/file_1/18917)
|
||||||
|
* |
||||||
|
*/ |
||||||
|
private static DryAndWetZenithDelays calculateZenithDryAndWetDelaysSec(double userLatitudeRadians, |
||||||
|
double heightMetersAboveSeaLevel, int dayOfyear1To366) { |
||||||
|
// interpolated meteorological values
|
||||||
|
double pressureMbar; |
||||||
|
double tempKelvin; |
||||||
|
double waterVaporPressureMbar; |
||||||
|
// temperature lapse rate, [K/m]
|
||||||
|
double beta; |
||||||
|
// water vapor lapse rate, dimensionless
|
||||||
|
double lambda; |
||||||
|
|
||||||
|
double absLatitudeDeg = Math.toDegrees(Math.abs(userLatitudeRadians)); |
||||||
|
// day of year min constant
|
||||||
|
double dmin; |
||||||
|
if (userLatitudeRadians < 0) { |
||||||
|
dmin = SOUTHERN_HEMISPHERE_DMIN; |
||||||
|
} else { |
||||||
|
dmin = NORTHERN_HEMISPHERE_DMIN; |
||||||
|
|
||||||
|
} |
||||||
|
double amplitudeScalefactor = Math.cos((2 * Math.PI * (dayOfyear1To366 - dmin)) |
||||||
|
/ DAYS_PER_YEAR); |
||||||
|
|
||||||
|
if (absLatitudeDeg <= LATITUDE_15_DEGREES) { |
||||||
|
pressureMbar = latDegreeToPressureMbarAvgMap[INDEX_15_DEGREES] |
||||||
|
- latDegreeToPressureMbarAmpMap[INDEX_15_DEGREES] * amplitudeScalefactor; |
||||||
|
tempKelvin = latDegreeToTempKelvinAvgMap[INDEX_15_DEGREES] |
||||||
|
- latDegreeToTempKelvinAmpMap[INDEX_15_DEGREES] * amplitudeScalefactor; |
||||||
|
waterVaporPressureMbar = latDegreeToWVPressureMbarAvgMap[INDEX_15_DEGREES] |
||||||
|
- latDegreeToWVPressureMbarAmpMap[INDEX_15_DEGREES] * amplitudeScalefactor; |
||||||
|
beta = latDegreeToBetaAvgMapKPM[INDEX_15_DEGREES] - latDegreeToBetaAmpMapKPM[INDEX_15_DEGREES] |
||||||
|
* amplitudeScalefactor; |
||||||
|
lambda = latDegreeToLampdaAmpMap[INDEX_15_DEGREES] - latDegreeToLampdaAmpMap[INDEX_15_DEGREES] |
||||||
|
* amplitudeScalefactor; |
||||||
|
} else if (absLatitudeDeg > LATITUDE_15_DEGREES && absLatitudeDeg < LATITUDE_75_DEGREES) { |
||||||
|
int key = (int) (absLatitudeDeg / LATITUDE_15_DEGREES); |
||||||
|
|
||||||
|
double averagePressureMbar = interpolate(key * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToPressureMbarAvgMap[key - 1], (key + 1) * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToPressureMbarAvgMap[key], absLatitudeDeg); |
||||||
|
double amplitudePressureMbar = interpolate(key * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToPressureMbarAmpMap[key - 1], (key + 1) * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToPressureMbarAmpMap[key], absLatitudeDeg); |
||||||
|
pressureMbar = averagePressureMbar - amplitudePressureMbar * amplitudeScalefactor; |
||||||
|
|
||||||
|
double averageTempKelvin = interpolate(key * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToTempKelvinAvgMap[key - 1], (key + 1) * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToTempKelvinAvgMap[key], absLatitudeDeg); |
||||||
|
double amplitudeTempKelvin = interpolate(key * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToTempKelvinAmpMap[key - 1], (key + 1) * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToTempKelvinAmpMap[key], absLatitudeDeg); |
||||||
|
tempKelvin = averageTempKelvin - amplitudeTempKelvin * amplitudeScalefactor; |
||||||
|
|
||||||
|
double averageWaterVaporPressureMbar = interpolate(key * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToWVPressureMbarAvgMap[key - 1], (key + 1) * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToWVPressureMbarAvgMap[key], absLatitudeDeg); |
||||||
|
double amplitudeWaterVaporPressureMbar = interpolate(key * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToWVPressureMbarAmpMap[key - 1], (key + 1) * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToWVPressureMbarAmpMap[key], absLatitudeDeg); |
||||||
|
waterVaporPressureMbar = |
||||||
|
averageWaterVaporPressureMbar - amplitudeWaterVaporPressureMbar * amplitudeScalefactor; |
||||||
|
|
||||||
|
double averageBeta = interpolate(key * LATITUDE_15_DEGREES, latDegreeToBetaAvgMapKPM[key - 1], |
||||||
|
(key + 1) * LATITUDE_15_DEGREES, latDegreeToBetaAvgMapKPM[key], absLatitudeDeg); |
||||||
|
double amplitudeBeta = interpolate(key * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToBetaAmpMapKPM[key - 1], (key + 1) * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToBetaAmpMapKPM[key], absLatitudeDeg); |
||||||
|
beta = averageBeta - amplitudeBeta * amplitudeScalefactor; |
||||||
|
|
||||||
|
double averageLambda = interpolate(key * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToLampdaAvgMap[key - 1], (key + 1) * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToLampdaAvgMap[key], absLatitudeDeg); |
||||||
|
double amplitudeLambda = interpolate(key * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToLampdaAmpMap[key - 1], (key + 1) * LATITUDE_15_DEGREES, |
||||||
|
latDegreeToLampdaAmpMap[key], absLatitudeDeg); |
||||||
|
lambda = averageLambda - amplitudeLambda * amplitudeScalefactor; |
||||||
|
} else { |
||||||
|
pressureMbar = latDegreeToPressureMbarAvgMap[INDEX_75_DEGREES] |
||||||
|
- latDegreeToPressureMbarAmpMap[INDEX_75_DEGREES] * amplitudeScalefactor; |
||||||
|
tempKelvin = latDegreeToTempKelvinAvgMap[INDEX_75_DEGREES] |
||||||
|
- latDegreeToTempKelvinAmpMap[INDEX_75_DEGREES] * amplitudeScalefactor; |
||||||
|
waterVaporPressureMbar = latDegreeToWVPressureMbarAvgMap[INDEX_75_DEGREES] |
||||||
|
- latDegreeToWVPressureMbarAmpMap[INDEX_75_DEGREES] * amplitudeScalefactor; |
||||||
|
beta = latDegreeToBetaAvgMapKPM[INDEX_75_DEGREES] - latDegreeToBetaAmpMapKPM[INDEX_75_DEGREES] |
||||||
|
* amplitudeScalefactor; |
||||||
|
lambda = latDegreeToLampdaAmpMap[INDEX_75_DEGREES] - latDegreeToLampdaAmpMap[INDEX_75_DEGREES] |
||||||
|
* amplitudeScalefactor; |
||||||
|
} |
||||||
|
|
||||||
|
double zenithDryDelayAtSeaLevelSeconds = (1.0e-6 * K1 * RD * pressureMbar) / GM; |
||||||
|
double zenithWetDelayAtSeaLevelSeconds = (((1.0e-6 * K2 * RD) |
||||||
|
/ (GM * (lambda + 1.0) - beta * RD)) * (waterVaporPressureMbar / tempKelvin)); |
||||||
|
double commonBase = 1.0 - ((beta * heightMetersAboveSeaLevel) / tempKelvin); |
||||||
|
|
||||||
|
double powerDry = (GRAVITY_MPS2 / (RD * beta)); |
||||||
|
double powerWet = (((lambda + 1.0) * GRAVITY_MPS2) / (RD * beta)) - 1.0; |
||||||
|
double zenithDryDelaySeconds = zenithDryDelayAtSeaLevelSeconds * Math.pow(commonBase, powerDry); |
||||||
|
double zenithWetDelaySeconds = zenithWetDelayAtSeaLevelSeconds * Math.pow(commonBase, powerWet); |
||||||
|
return new DryAndWetZenithDelays(zenithDryDelaySeconds, zenithWetDelaySeconds); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Interpolates linearly given two points (point1X, point1Y) and (point2X, point2Y). Given the |
||||||
|
* desired value of x (xInterpolated), an interpolated value of y shall be computed and returned. |
||||||
|
*/ |
||||||
|
private static double interpolate(double point1X, double point1Y, double point2X, double point2Y, |
||||||
|
double xOutput) { |
||||||
|
// Check that xOutput is between the two interpolation points.
|
||||||
|
if ((point1X < point2X && (xOutput < point1X || xOutput > point2X)) |
||||||
|
|| (point2X < point1X && (xOutput < point2X || xOutput > point1X))) { |
||||||
|
throw new IllegalArgumentException("Interpolated value is outside the interpolated region"); |
||||||
|
} |
||||||
|
double deltaX = point2X - point1X; |
||||||
|
double yOutput; |
||||||
|
|
||||||
|
if (Math.abs(deltaX) > MINIMUM_INTERPOLATION_THRESHOLD) { |
||||||
|
yOutput = point1Y + (xOutput - point1X) / deltaX * (point2Y - point1Y); |
||||||
|
} else { |
||||||
|
yOutput = point1Y; |
||||||
|
} |
||||||
|
return yOutput; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* |
||||||
|
* A class containing dry and wet mapping values |
||||||
|
*/ |
||||||
|
private static class DryAndWetMappingValues { |
||||||
|
public double dryMappingValue; |
||||||
|
public double wetMappingValue; |
||||||
|
|
||||||
|
public DryAndWetMappingValues(double dryMappingValue, double wetMappingValue) { |
||||||
|
this.dryMappingValue = dryMappingValue; |
||||||
|
this.wetMappingValue = wetMappingValue; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* |
||||||
|
* A class containing dry and wet delays in seconds experienced at zenith |
||||||
|
*/ |
||||||
|
private static class DryAndWetZenithDelays { |
||||||
|
public double dryZenithDelaySec; |
||||||
|
public double wetZenithDelaySec; |
||||||
|
|
||||||
|
public DryAndWetZenithDelays(double dryZenithDelay, double wetZenithDelay) { |
||||||
|
this.dryZenithDelaySec = dryZenithDelay; |
||||||
|
this.wetZenithDelaySec = wetZenithDelay; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,989 @@ |
|||||||
|
/* |
||||||
|
* Copyright (C) 2017 The Android Open Source Project |
||||||
|
* |
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||||
|
* you may not use this file except in compliance with the License. |
||||||
|
* You may obtain a copy of the License at |
||||||
|
* |
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
* |
||||||
|
* Unless required by applicable law or agreed to in writing, software |
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, |
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||||
|
* See the License for the specific language governing permissions and |
||||||
|
* limitations under the License. |
||||||
|
*/ |
||||||
|
|
||||||
|
package com.google.location.lbs.gnss.gps.pseudorange; |
||||||
|
|
||||||
|
import com.google.common.annotations.VisibleForTesting; |
||||||
|
import com.google.common.base.Preconditions; |
||||||
|
import com.google.common.collect.Lists; |
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.Ecef2LlaConverter.GeodeticLlaValues; |
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.EcefToTopocentricConverter.TopocentricAEDValues; |
||||||
|
import com.google.location.lbs.gnss.gps.pseudorange.SatellitePositionCalculator.PositionAndVelocity; |
||||||
|
import android.location.cts.nano.Ephemeris.GpsEphemerisProto; |
||||||
|
import android.location.cts.nano.Ephemeris.GpsNavMessageProto; |
||||||
|
|
||||||
|
import java.util.ArrayList; |
||||||
|
import java.util.Arrays; |
||||||
|
import java.util.Collections; |
||||||
|
import java.util.List; |
||||||
|
import org.apache.commons.math3.linear.Array2DRowRealMatrix; |
||||||
|
import org.apache.commons.math3.linear.LUDecomposition; |
||||||
|
import org.apache.commons.math3.linear.QRDecomposition; |
||||||
|
import org.apache.commons.math3.linear.RealMatrix; |
||||||
|
|
||||||
|
/** |
||||||
|
* Computes an iterative least square receiver position solution given the pseudorange (meters) and |
||||||
|
* accumulated delta range (meters) measurements, receiver time of week, week number and the |
||||||
|
* navigation message. |
||||||
|
*/ |
||||||
|
class UserPositionVelocityWeightedLeastSquare { |
||||||
|
private static final double SPEED_OF_LIGHT_MPS = 299792458.0; |
||||||
|
private static final int SECONDS_IN_WEEK = 604800; |
||||||
|
private static final double LEAST_SQUARE_TOLERANCE_METERS = 4.0e-8; |
||||||
|
/** Position correction threshold below which atmospheric correction will be applied */ |
||||||
|
private static final double ATMPOSPHERIC_CORRECTIONS_THRESHOLD_METERS = 1000.0; |
||||||
|
private static final int MINIMUM_NUMER_OF_SATELLITES = 4; |
||||||
|
private static final double RESIDUAL_TO_REPEAT_LEAST_SQUARE_METERS = 20.0; |
||||||
|
private static final int MAXIMUM_NUMBER_OF_LEAST_SQUARE_ITERATIONS = 100; |
||||||
|
/** GPS C/A code chip width Tc = 1 microseconds */ |
||||||
|
private static final double GPS_CHIP_WIDTH_T_C_SEC = 1.0e-6; |
||||||
|
/** Narrow correlator with spacing d = 0.1 chip */ |
||||||
|
private static final double GPS_CORRELATOR_SPACING_IN_CHIPS = 0.1; |
||||||
|
/** Average time of DLL correlator T of 20 milliseconds */ |
||||||
|
private static final double GPS_DLL_AVERAGING_TIME_SEC = 20.0e-3; |
||||||
|
/** Average signal travel time from GPS satellite and earth */ |
||||||
|
private static final double AVERAGE_TRAVEL_TIME_SECONDS = 70.0e-3; |
||||||
|
private static final double SECONDS_PER_NANO = 1.0e-9; |
||||||
|
private static final double DOUBLE_ROUND_OFF_TOLERANCE = 0.0000000001; |
||||||
|
|
||||||
|
private final PseudorangeSmoother pseudorangeSmoother; |
||||||
|
private double geoidHeightMeters; |
||||||
|
private ElevationApiHelper elevationApiHelper; |
||||||
|
private boolean calculateGeoidMeters = true; |
||||||
|
private RealMatrix geometryMatrix; |
||||||
|
private double[] truthLocationForCorrectedResidualComputationEcef = null; |
||||||
|
|
||||||
|
/** Constructor */ |
||||||
|
public UserPositionVelocityWeightedLeastSquare(PseudorangeSmoother pseudorangeSmoother) { |
||||||
|
this.pseudorangeSmoother = pseudorangeSmoother; |
||||||
|
} |
||||||
|
|
||||||
|
/** Constructor with Google Elevation API Key */ |
||||||
|
public UserPositionVelocityWeightedLeastSquare(PseudorangeSmoother pseudorangeSmoother, |
||||||
|
String elevationApiKey){ |
||||||
|
this.pseudorangeSmoother = pseudorangeSmoother; |
||||||
|
this.elevationApiHelper = new ElevationApiHelper(elevationApiKey); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Sets the reference ground truth for pseudornage residual correction calculation. If no ground |
||||||
|
* truth is set, no corrected pesudorange residual will be calculated. |
||||||
|
*/ |
||||||
|
public void setTruthLocationForCorrectedResidualComputationEcef |
||||||
|
(double[] groundTruthForResidualCorrectionEcef) { |
||||||
|
this.truthLocationForCorrectedResidualComputationEcef = groundTruthForResidualCorrectionEcef; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Least square solution to calculate the user position given the navigation message, pseudorange |
||||||
|
* and accumulated delta range measurements. Also calculates user velocity non-iteratively from |
||||||
|
* Least square position solution. |
||||||
|
* |
||||||
|
* <p>The method fills the user position and velocity in ECEF coordinates and receiver clock |
||||||
|
* offset in meters and clock offset rate in meters per second. |
||||||
|
* |
||||||
|
* <p>One can choose between no smoothing, using the carrier phase measurements (accumulated delta |
||||||
|
* range) or the doppler measurements (pseudorange rate) for smoothing the pseudorange. The |
||||||
|
* smoothing is applied only if time has changed below a specific threshold since last invocation. |
||||||
|
* |
||||||
|
* <p>Source for least squares: |
||||||
|
* |
||||||
|
* <ul> |
||||||
|
* <li>http://www.u-blox.com/images/downloads/Product_Docs/GPS_Compendium%28GPS-X-02007%29.pdf
|
||||||
|
* page 81 - 85 |
||||||
|
* <li>Parkinson, B.W., Spilker Jr., J.J.: ‘Global positioning system: theory and applications’ |
||||||
|
* page 412 - 414 |
||||||
|
* </ul> |
||||||
|
* |
||||||
|
* <p>Sources for smoothing pseudorange with carrier phase measurements: |
||||||
|
* |
||||||
|
* <ul> |
||||||
|
* <li>Satellite Communications and Navigation Systems book, page 424, |
||||||
|
* <li>Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, page 388, |
||||||
|
* 389. |
||||||
|
* </ul> |
||||||
|
* |
||||||
|
* <p>The function does not modify the smoothed measurement list {@code |
||||||
|
* immutableSmoothedSatellitesToReceiverMeasurements} |
||||||
|
* |
||||||
|
* @param navMessageProto parameters of the navigation message |
||||||
|
* @param usefulSatellitesToReceiverMeasurements Map of useful satellite PRN to {@link |
||||||
|
* GpsMeasurementWithRangeAndUncertainty} containing receiver measurements for computing the |
||||||
|
* position solution. |
||||||
|
* @param receiverGPSTowAtReceptionSeconds Receiver estimate of GPS time of week (seconds) |
||||||
|
* @param receiverGPSWeek Receiver estimate of GPS week (0-1024+) |
||||||
|
* @param dayOfYear1To366 The day of the year between 1 and 366 |
||||||
|
* @param positionVelocitySolutionECEF Solution array of the following format: |
||||||
|
* [0-2] xyz solution of user. |
||||||
|
* [3] clock bias of user. |
||||||
|
* [4-6] velocity of user. |
||||||
|
* [7] clock bias rate of user. |
||||||
|
* @param positionVelocityUncertaintyEnu Uncertainty of calculated position and velocity solution |
||||||
|
* in meters and mps local ENU system. Array has the following format: |
||||||
|
* [0-2] Enu uncertainty of position solution in meters |
||||||
|
* [3-5] Enu uncertainty of velocity solution in meters per second. |
||||||
|
* @param pseudorangeResidualMeters The pseudorange residual corrected by subtracting expected |
||||||
|
* psudorange calculated with the use clock bias of the highest elevation satellites. |
||||||
|
*/ |
||||||
|
public void calculateUserPositionVelocityLeastSquare( |
||||||
|
GpsNavMessageProto navMessageProto, |
||||||
|
List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, |
||||||
|
double receiverGPSTowAtReceptionSeconds, |
||||||
|
int receiverGPSWeek, |
||||||
|
int dayOfYear1To366, |
||||||
|
double[] positionVelocitySolutionECEF, |
||||||
|
double[] positionVelocityUncertaintyEnu, |
||||||
|
double[] pseudorangeResidualMeters) |
||||||
|
throws Exception { |
||||||
|
|
||||||
|
// Use PseudorangeSmoother to smooth the pseudorange according to: Satellite Communications and
|
||||||
|
// Navigation Systems book, page 424 and Principles of GNSS, Inertial, and Multisensor
|
||||||
|
// Integrated Navigation Systems, page 388, 389.
|
||||||
|
double[] deltaPositionMeters; |
||||||
|
List<GpsMeasurementWithRangeAndUncertainty> immutableSmoothedSatellitesToReceiverMeasurements = |
||||||
|
pseudorangeSmoother.updatePseudorangeSmoothingResult( |
||||||
|
Collections.unmodifiableList(usefulSatellitesToReceiverMeasurements)); |
||||||
|
List<GpsMeasurementWithRangeAndUncertainty> mutableSmoothedSatellitesToReceiverMeasurements = |
||||||
|
Lists.newArrayList(immutableSmoothedSatellitesToReceiverMeasurements); |
||||||
|
int numberOfUsefulSatellites = |
||||||
|
getNumberOfUsefulSatellites(mutableSmoothedSatellitesToReceiverMeasurements); |
||||||
|
// Least square position solution is supported only if 4 or more satellites visible
|
||||||
|
Preconditions.checkArgument(numberOfUsefulSatellites >= MINIMUM_NUMER_OF_SATELLITES, |
||||||
|
"At least 4 satellites have to be visible... Only 3D mode is supported..."); |
||||||
|
boolean repeatLeastSquare = false; |
||||||
|
SatellitesPositionPseudorangesResidualAndCovarianceMatrix satPosPseudorangeResidualAndWeight; |
||||||
|
|
||||||
|
boolean isFirstWLS = true; |
||||||
|
|
||||||
|
do { |
||||||
|
// Calculate satellites' positions, measurement residuals per visible satellite and
|
||||||
|
// weight matrix for the iterative least square
|
||||||
|
boolean doAtmosphericCorrections = false; |
||||||
|
satPosPseudorangeResidualAndWeight = |
||||||
|
calculateSatPosAndPseudorangeResidual( |
||||||
|
navMessageProto, |
||||||
|
mutableSmoothedSatellitesToReceiverMeasurements, |
||||||
|
receiverGPSTowAtReceptionSeconds, |
||||||
|
receiverGPSWeek, |
||||||
|
dayOfYear1To366, |
||||||
|
positionVelocitySolutionECEF, |
||||||
|
doAtmosphericCorrections); |
||||||
|
|
||||||
|
// Calculate the geometry matrix according to "Global Positioning System: Theory and
|
||||||
|
// Applications", Parkinson and Spilker page 413
|
||||||
|
RealMatrix covarianceMatrixM2 = |
||||||
|
new Array2DRowRealMatrix(satPosPseudorangeResidualAndWeight.covarianceMatrixMetersSquare); |
||||||
|
geometryMatrix = new Array2DRowRealMatrix(calculateGeometryMatrix( |
||||||
|
satPosPseudorangeResidualAndWeight.satellitesPositionsMeters, |
||||||
|
positionVelocitySolutionECEF)); |
||||||
|
RealMatrix weightedGeometryMatrix; |
||||||
|
RealMatrix weightMatrixMetersMinus2 = null; |
||||||
|
// Apply weighted least square only if the covariance matrix is not singular (has a non-zero
|
||||||
|
// determinant), otherwise apply ordinary least square. The reason is to ignore reported
|
||||||
|
// signal to noise ratios by the receiver that can lead to such singularities
|
||||||
|
LUDecomposition ludCovMatrixM2 = new LUDecomposition(covarianceMatrixM2); |
||||||
|
double det = ludCovMatrixM2.getDeterminant(); |
||||||
|
|
||||||
|
if (det <= DOUBLE_ROUND_OFF_TOLERANCE) { |
||||||
|
// Do not weight the geometry matrix if covariance matrix is singular.
|
||||||
|
weightedGeometryMatrix = geometryMatrix; |
||||||
|
} else { |
||||||
|
weightMatrixMetersMinus2 = ludCovMatrixM2.getSolver().getInverse(); |
||||||
|
RealMatrix hMatrix = |
||||||
|
calculateHMatrix(weightMatrixMetersMinus2, geometryMatrix); |
||||||
|
weightedGeometryMatrix = hMatrix.multiply(geometryMatrix.transpose()) |
||||||
|
.multiply(weightMatrixMetersMinus2); |
||||||
|
} |
||||||
|
|
||||||
|
// Equation 9 page 413 from "Global Positioning System: Theory and Applicaitons", Parkinson
|
||||||
|
// and Spilker
|
||||||
|
deltaPositionMeters = |
||||||
|
GpsMathOperations.matrixByColVectMultiplication(weightedGeometryMatrix.getData(), |
||||||
|
satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters); |
||||||
|
|
||||||
|
// Apply corrections to the position estimate
|
||||||
|
positionVelocitySolutionECEF[0] += deltaPositionMeters[0]; |
||||||
|
positionVelocitySolutionECEF[1] += deltaPositionMeters[1]; |
||||||
|
positionVelocitySolutionECEF[2] += deltaPositionMeters[2]; |
||||||
|
positionVelocitySolutionECEF[3] += deltaPositionMeters[3]; |
||||||
|
// Iterate applying corrections to the position solution until correction is below threshold
|
||||||
|
satPosPseudorangeResidualAndWeight = |
||||||
|
applyWeightedLeastSquare( |
||||||
|
navMessageProto, |
||||||
|
mutableSmoothedSatellitesToReceiverMeasurements, |
||||||
|
receiverGPSTowAtReceptionSeconds, |
||||||
|
receiverGPSWeek, |
||||||
|
dayOfYear1To366, |
||||||
|
positionVelocitySolutionECEF, |
||||||
|
deltaPositionMeters, |
||||||
|
doAtmosphericCorrections, |
||||||
|
satPosPseudorangeResidualAndWeight, |
||||||
|
weightMatrixMetersMinus2); |
||||||
|
|
||||||
|
// We use the first WLS iteration results and correct them based on the ground truth position
|
||||||
|
// and using a clock error computed from high elevation satellites. The first iteration is
|
||||||
|
// used before satellite with high residuals being removed.
|
||||||
|
if (isFirstWLS && truthLocationForCorrectedResidualComputationEcef != null) { |
||||||
|
// Snapshot the information needed before high residual satellites are removed
|
||||||
|
System.arraycopy( |
||||||
|
ResidualCorrectionCalculator.calculateCorrectedResiduals( |
||||||
|
satPosPseudorangeResidualAndWeight, |
||||||
|
positionVelocitySolutionECEF.clone(), |
||||||
|
truthLocationForCorrectedResidualComputationEcef), |
||||||
|
0 /*source starting pos*/, |
||||||
|
pseudorangeResidualMeters, |
||||||
|
0 /*destination starting pos*/, |
||||||
|
GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES /*length of elements*/); |
||||||
|
isFirstWLS = false; |
||||||
|
} |
||||||
|
repeatLeastSquare = false; |
||||||
|
int satsWithResidualBelowThreshold = |
||||||
|
satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters.length; |
||||||
|
// remove satellites that have residuals above RESIDUAL_TO_REPEAT_LEAST_SQUARE_METERS as they
|
||||||
|
// worsen the position solution accuracy. If any satellite is removed, repeat the least square
|
||||||
|
repeatLeastSquare = |
||||||
|
removeHighResidualSats( |
||||||
|
mutableSmoothedSatellitesToReceiverMeasurements, |
||||||
|
repeatLeastSquare, |
||||||
|
satPosPseudorangeResidualAndWeight, |
||||||
|
satsWithResidualBelowThreshold); |
||||||
|
|
||||||
|
} while (repeatLeastSquare); |
||||||
|
calculateGeoidMeters = false; |
||||||
|
|
||||||
|
// The computed ECEF position will be used next to compute the user velocity.
|
||||||
|
// we calculate and fill in the user velocity solutions based on following equation:
|
||||||
|
// Weight Matrix * GeometryMatrix * User Velocity Vector
|
||||||
|
// = Weight Matrix * deltaPseudoRangeRateWeightedMps
|
||||||
|
// Reference: Pratap Misra and Per Enge
|
||||||
|
// "Global Positioning System: Signals, Measurements, and Performance" Page 218.
|
||||||
|
|
||||||
|
// Get the number of satellite used in Geometry Matrix
|
||||||
|
numberOfUsefulSatellites = geometryMatrix.getRowDimension(); |
||||||
|
|
||||||
|
RealMatrix rangeRateMps = new Array2DRowRealMatrix(numberOfUsefulSatellites, 1); |
||||||
|
RealMatrix deltaPseudoRangeRateMps = |
||||||
|
new Array2DRowRealMatrix(numberOfUsefulSatellites, 1); |
||||||
|
RealMatrix pseudorangeRateWeight |
||||||
|
= new Array2DRowRealMatrix(numberOfUsefulSatellites, numberOfUsefulSatellites); |
||||||
|
|
||||||
|
// Correct the receiver time of week with the estimated receiver clock bias
|
||||||
|
receiverGPSTowAtReceptionSeconds = |
||||||
|
receiverGPSTowAtReceptionSeconds - positionVelocitySolutionECEF[3] / SPEED_OF_LIGHT_MPS; |
||||||
|
|
||||||
|
int measurementCount = 0; |
||||||
|
|
||||||
|
// Calculate range rates
|
||||||
|
for (int i = 0; i < GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES; i++) { |
||||||
|
if (mutableSmoothedSatellitesToReceiverMeasurements.get(i) != null) { |
||||||
|
GpsEphemerisProto ephemeridesProto = getEphemerisForSatellite(navMessageProto, i + 1); |
||||||
|
|
||||||
|
double pseudorangeMeasurementMeters = |
||||||
|
mutableSmoothedSatellitesToReceiverMeasurements.get(i).pseudorangeMeters; |
||||||
|
GpsTimeOfWeekAndWeekNumber correctedTowAndWeek = |
||||||
|
calculateCorrectedTransmitTowAndWeek(ephemeridesProto, receiverGPSTowAtReceptionSeconds, |
||||||
|
receiverGPSWeek, pseudorangeMeasurementMeters); |
||||||
|
|
||||||
|
// Calculate satellite velocity
|
||||||
|
PositionAndVelocity satPosECEFMetersVelocityMPS = SatellitePositionCalculator |
||||||
|
.calculateSatellitePositionAndVelocityFromEphemeris( |
||||||
|
ephemeridesProto, |
||||||
|
correctedTowAndWeek.gpsTimeOfWeekSeconds, |
||||||
|
correctedTowAndWeek.weekNumber, |
||||||
|
positionVelocitySolutionECEF[0], |
||||||
|
positionVelocitySolutionECEF[1], |
||||||
|
positionVelocitySolutionECEF[2]); |
||||||
|
|
||||||
|
// Calculate satellite clock error rate
|
||||||
|
double satelliteClockErrorRateMps = SatelliteClockCorrectionCalculator. |
||||||
|
calculateSatClockCorrErrorRate( |
||||||
|
ephemeridesProto, |
||||||
|
correctedTowAndWeek.gpsTimeOfWeekSeconds, |
||||||
|
correctedTowAndWeek.weekNumber); |
||||||
|
|
||||||
|
// Fill in range rates. range rate = satellite velocity (dot product) line-of-sight vector
|
||||||
|
rangeRateMps.setEntry(measurementCount, 0, -1 * ( |
||||||
|
satPosECEFMetersVelocityMPS.velocityXMetersPerSec |
||||||
|
* geometryMatrix.getEntry(measurementCount, 0) |
||||||
|
+ satPosECEFMetersVelocityMPS.velocityYMetersPerSec |
||||||
|
* geometryMatrix.getEntry(measurementCount, 1) |
||||||
|
+ satPosECEFMetersVelocityMPS.velocityZMetersPerSec |
||||||
|
* geometryMatrix.getEntry(measurementCount, 2))); |
||||||
|
|
||||||
|
deltaPseudoRangeRateMps.setEntry(measurementCount, 0, |
||||||
|
mutableSmoothedSatellitesToReceiverMeasurements.get(i).pseudorangeRateMps |
||||||
|
- rangeRateMps.getEntry(measurementCount, 0) + satelliteClockErrorRateMps |
||||||
|
- positionVelocitySolutionECEF[7]); |
||||||
|
|
||||||
|
// Calculate the velocity weight matrix by using 1 / square(Pseudorangerate Uncertainty)
|
||||||
|
// along the diagonal
|
||||||
|
pseudorangeRateWeight.setEntry(measurementCount, measurementCount, |
||||||
|
1 / (mutableSmoothedSatellitesToReceiverMeasurements |
||||||
|
.get(i).pseudorangeRateUncertaintyMps |
||||||
|
* mutableSmoothedSatellitesToReceiverMeasurements |
||||||
|
.get(i).pseudorangeRateUncertaintyMps)); |
||||||
|
measurementCount++; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
RealMatrix weightedGeoMatrix = pseudorangeRateWeight.multiply(geometryMatrix); |
||||||
|
RealMatrix deltaPseudoRangeRateWeightedMps = |
||||||
|
pseudorangeRateWeight.multiply(deltaPseudoRangeRateMps); |
||||||
|
QRDecomposition qrdWeightedGeoMatrix = new QRDecomposition(weightedGeoMatrix); |
||||||
|
RealMatrix velocityMps |
||||||
|
= qrdWeightedGeoMatrix.getSolver().solve(deltaPseudoRangeRateWeightedMps); |
||||||
|
positionVelocitySolutionECEF[4] = velocityMps.getEntry(0, 0); |
||||||
|
positionVelocitySolutionECEF[5] = velocityMps.getEntry(1, 0); |
||||||
|
positionVelocitySolutionECEF[6] = velocityMps.getEntry(2, 0); |
||||||
|
positionVelocitySolutionECEF[7] = velocityMps.getEntry(3, 0); |
||||||
|
|
||||||
|
RealMatrix pseudorangeWeight |
||||||
|
= new LUDecomposition( |
||||||
|
new Array2DRowRealMatrix(satPosPseudorangeResidualAndWeight.covarianceMatrixMetersSquare |
||||||
|
) |
||||||
|
).getSolver().getInverse(); |
||||||
|
|
||||||
|
// Calculate and store the uncertainties of position and velocity in local ENU system in meters
|
||||||
|
// and meters per second.
|
||||||
|
double[] pvUncertainty = |
||||||
|
calculatePositionVelocityUncertaintyEnu(pseudorangeRateWeight, pseudorangeWeight, |
||||||
|
positionVelocitySolutionECEF); |
||||||
|
System.arraycopy(pvUncertainty, |
||||||
|
0 /*source starting pos*/, |
||||||
|
positionVelocityUncertaintyEnu, |
||||||
|
0 /*destination starting pos*/, |
||||||
|
6 /*length of elements*/); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculates the position uncertainty in meters and the velocity uncertainty |
||||||
|
* in meters per second solution in local ENU system. |
||||||
|
* |
||||||
|
* <p> Reference: Global Positioning System: Signals, Measurements, and Performance |
||||||
|
* by Pratap Misra, Per Enge, Page 206 - 209. |
||||||
|
* |
||||||
|
* @param velocityWeightMatrix the velocity weight matrix |
||||||
|
* @param positionWeightMatrix the position weight matrix |
||||||
|
* @param positionVelocitySolution the position and velocity solution in ECEF |
||||||
|
* @return an array containing the position and velocity uncertainties in ENU coordinate system. |
||||||
|
* [0-2] Enu uncertainty of position solution in meters. |
||||||
|
* [3-5] Enu uncertainty of velocity solution in meters per second. |
||||||
|
*/ |
||||||
|
public double[] calculatePositionVelocityUncertaintyEnu( |
||||||
|
RealMatrix velocityWeightMatrix, RealMatrix positionWeightMatrix, |
||||||
|
double[] positionVelocitySolution){ |
||||||
|
|
||||||
|
if (geometryMatrix == null){ |
||||||
|
return null; |
||||||
|
} |
||||||
|
|
||||||
|
RealMatrix velocityH = calculateHMatrix(velocityWeightMatrix, geometryMatrix); |
||||||
|
RealMatrix positionH = calculateHMatrix(positionWeightMatrix, geometryMatrix); |
||||||
|
|
||||||
|
// Calculate the rotation Matrix to convert to local ENU system.
|
||||||
|
RealMatrix rotationMatrix = new Array2DRowRealMatrix(4, 4); |
||||||
|
GeodeticLlaValues llaValues = Ecef2LlaConverter.convertECEFToLLACloseForm |
||||||
|
(positionVelocitySolution[0], positionVelocitySolution[1], positionVelocitySolution[2]); |
||||||
|
rotationMatrix.setSubMatrix( |
||||||
|
Ecef2EnuConverter.getRotationMatrix(llaValues.longitudeRadians, |
||||||
|
llaValues.latitudeRadians).getData(), 0, 0); |
||||||
|
rotationMatrix.setEntry(3, 3, 1); |
||||||
|
|
||||||
|
// Convert to local ENU by pre-multiply rotation matrix and multiply rotation matrix transposed
|
||||||
|
velocityH = rotationMatrix.multiply(velocityH).multiply(rotationMatrix.transpose()); |
||||||
|
positionH = rotationMatrix.multiply(positionH).multiply(rotationMatrix.transpose()); |
||||||
|
|
||||||
|
// Return the square root of diagonal entries
|
||||||
|
return new double[] { |
||||||
|
Math.sqrt(positionH.getEntry(0, 0)), Math.sqrt(positionH.getEntry(1, 1)), |
||||||
|
Math.sqrt(positionH.getEntry(2, 2)), Math.sqrt(velocityH.getEntry(0, 0)), |
||||||
|
Math.sqrt(velocityH.getEntry(1, 1)), Math.sqrt(velocityH.getEntry(2, 2))}; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculates the measurement connection matrix H as a function of weightMatrix and |
||||||
|
* geometryMatrix. |
||||||
|
* |
||||||
|
* <p> H = (geometryMatrixTransposed * Weight * geometryMatrix) ^ -1 |
||||||
|
* |
||||||
|
* <p> Reference: Global Positioning System: Signals, Measurements, and Performance, P207 |
||||||
|
* @param weightMatrix Weights for computing H Matrix |
||||||
|
* @return H Matrix |
||||||
|
*/ |
||||||
|
private RealMatrix calculateHMatrix |
||||||
|
(RealMatrix weightMatrix, RealMatrix geometryMatrix){ |
||||||
|
|
||||||
|
RealMatrix tempH = geometryMatrix.transpose().multiply(weightMatrix).multiply(geometryMatrix); |
||||||
|
return new LUDecomposition(tempH).getSolver().getInverse(); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Applies weighted least square iterations and corrects to the position solution until correction |
||||||
|
* is below threshold. An exception is thrown if the maximum number of iterations: |
||||||
|
* {@value #MAXIMUM_NUMBER_OF_LEAST_SQUARE_ITERATIONS} is reached without convergence. |
||||||
|
*/ |
||||||
|
private SatellitesPositionPseudorangesResidualAndCovarianceMatrix applyWeightedLeastSquare( |
||||||
|
GpsNavMessageProto navMessageProto, |
||||||
|
List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, |
||||||
|
double receiverGPSTowAtReceptionSeconds, |
||||||
|
int receiverGPSWeek, |
||||||
|
int dayOfYear1To366, |
||||||
|
double[] positionSolutionECEF, |
||||||
|
double[] deltaPositionMeters, |
||||||
|
boolean doAtmosphericCorrections, |
||||||
|
SatellitesPositionPseudorangesResidualAndCovarianceMatrix satPosPseudorangeResidualAndWeight, |
||||||
|
RealMatrix weightMatrixMetersMinus2) |
||||||
|
throws Exception { |
||||||
|
RealMatrix weightedGeometryMatrix; |
||||||
|
int numberOfIterations = 0; |
||||||
|
|
||||||
|
while ((Math.abs(deltaPositionMeters[0]) + Math.abs(deltaPositionMeters[1]) |
||||||
|
+ Math.abs(deltaPositionMeters[2])) >= LEAST_SQUARE_TOLERANCE_METERS) { |
||||||
|
// Apply ionospheric and tropospheric corrections only if the applied correction to
|
||||||
|
// position is below a specific threshold
|
||||||
|
if ((Math.abs(deltaPositionMeters[0]) + Math.abs(deltaPositionMeters[1]) |
||||||
|
+ Math.abs(deltaPositionMeters[2])) < ATMPOSPHERIC_CORRECTIONS_THRESHOLD_METERS) { |
||||||
|
doAtmosphericCorrections = true; |
||||||
|
} |
||||||
|
// Calculate satellites' positions, measurement residual per visible satellite and
|
||||||
|
// weight matrix for the iterative least square
|
||||||
|
satPosPseudorangeResidualAndWeight = |
||||||
|
calculateSatPosAndPseudorangeResidual( |
||||||
|
navMessageProto, |
||||||
|
usefulSatellitesToReceiverMeasurements, |
||||||
|
receiverGPSTowAtReceptionSeconds, |
||||||
|
receiverGPSWeek, |
||||||
|
dayOfYear1To366, |
||||||
|
positionSolutionECEF, |
||||||
|
doAtmosphericCorrections); |
||||||
|
|
||||||
|
// Calculate the geometry matrix according to "Global Positioning System: Theory and
|
||||||
|
// Applications", Parkinson and Spilker page 413
|
||||||
|
geometryMatrix = new Array2DRowRealMatrix(calculateGeometryMatrix( |
||||||
|
satPosPseudorangeResidualAndWeight.satellitesPositionsMeters, positionSolutionECEF)); |
||||||
|
// Apply weighted least square only if the covariance matrix is
|
||||||
|
// not singular (has a non-zero determinant), otherwise apply ordinary least square.
|
||||||
|
// The reason is to ignore reported signal to noise ratios by the receiver that can
|
||||||
|
// lead to such singularities
|
||||||
|
if (weightMatrixMetersMinus2 == null) { |
||||||
|
weightedGeometryMatrix = geometryMatrix; |
||||||
|
} else { |
||||||
|
RealMatrix hMatrix = |
||||||
|
calculateHMatrix(weightMatrixMetersMinus2, geometryMatrix); |
||||||
|
weightedGeometryMatrix = hMatrix.multiply(geometryMatrix.transpose()) |
||||||
|
.multiply(weightMatrixMetersMinus2); |
||||||
|
} |
||||||
|
|
||||||
|
// Equation 9 page 413 from "Global Positioning System: Theory and Applicaitons",
|
||||||
|
// Parkinson and Spilker
|
||||||
|
deltaPositionMeters = |
||||||
|
GpsMathOperations.matrixByColVectMultiplication( |
||||||
|
weightedGeometryMatrix.getData(), |
||||||
|
satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters); |
||||||
|
|
||||||
|
// Apply corrections to the position estimate
|
||||||
|
positionSolutionECEF[0] += deltaPositionMeters[0]; |
||||||
|
positionSolutionECEF[1] += deltaPositionMeters[1]; |
||||||
|
positionSolutionECEF[2] += deltaPositionMeters[2]; |
||||||
|
positionSolutionECEF[3] += deltaPositionMeters[3]; |
||||||
|
numberOfIterations++; |
||||||
|
Preconditions.checkArgument(numberOfIterations <= MAXIMUM_NUMBER_OF_LEAST_SQUARE_ITERATIONS, |
||||||
|
"Maximum number of least square iterations reached without convergance..."); |
||||||
|
} |
||||||
|
return satPosPseudorangeResidualAndWeight; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Removes satellites that have residuals above {@value #RESIDUAL_TO_REPEAT_LEAST_SQUARE_METERS} |
||||||
|
* from the {@code usefulSatellitesToReceiverMeasurements} list. Returns true if any satellite is |
||||||
|
* removed. |
||||||
|
*/ |
||||||
|
private boolean removeHighResidualSats( |
||||||
|
List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, |
||||||
|
boolean repeatLeastSquare, |
||||||
|
SatellitesPositionPseudorangesResidualAndCovarianceMatrix satPosPseudorangeResidualAndWeight, |
||||||
|
int satsWithResidualBelowThreshold) { |
||||||
|
|
||||||
|
for (int i = 0; i < satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters.length; i++) { |
||||||
|
if (satsWithResidualBelowThreshold > MINIMUM_NUMER_OF_SATELLITES) { |
||||||
|
if (Math.abs(satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters[i]) |
||||||
|
> RESIDUAL_TO_REPEAT_LEAST_SQUARE_METERS) { |
||||||
|
int prn = satPosPseudorangeResidualAndWeight.satellitePRNs[i]; |
||||||
|
usefulSatellitesToReceiverMeasurements.set(prn - 1, null); |
||||||
|
satsWithResidualBelowThreshold--; |
||||||
|
repeatLeastSquare = true; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
return repeatLeastSquare; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculates position of all visible satellites and pseudorange measurement residual |
||||||
|
* (difference of measured to predicted pseudoranges) needed for the least square computation. The |
||||||
|
* result is stored in an instance of {@link |
||||||
|
* SatellitesPositionPseudorangesResidualAndCovarianceMatrix} |
||||||
|
* |
||||||
|
* @param navMeassageProto parameters of the navigation message |
||||||
|
* @param usefulSatellitesToReceiverMeasurements Map of useful satellite PRN to {@link |
||||||
|
* GpsMeasurementWithRangeAndUncertainty} containing receiver measurements for computing the |
||||||
|
* position solution |
||||||
|
* @param receiverGPSTowAtReceptionSeconds Receiver estimate of GPS time of week (seconds) |
||||||
|
* @param receiverGpsWeek Receiver estimate of GPS week (0-1024+) |
||||||
|
* @param dayOfYear1To366 The day of the year between 1 and 366 |
||||||
|
* @param userPositionECEFMeters receiver ECEF position in meters |
||||||
|
* @param doAtmosphericCorrections boolean indicating if atmospheric range corrections should be |
||||||
|
* applied |
||||||
|
* @return SatellitesPositionPseudorangesResidualAndCovarianceMatrix Object containing satellite |
||||||
|
* prns, satellite positions in ECEF, pseudorange residuals and covariance matrix. |
||||||
|
*/ |
||||||
|
public SatellitesPositionPseudorangesResidualAndCovarianceMatrix |
||||||
|
calculateSatPosAndPseudorangeResidual( |
||||||
|
GpsNavMessageProto navMeassageProto, |
||||||
|
List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, |
||||||
|
double receiverGPSTowAtReceptionSeconds, |
||||||
|
int receiverGpsWeek, |
||||||
|
int dayOfYear1To366, |
||||||
|
double[] userPositionECEFMeters, |
||||||
|
boolean doAtmosphericCorrections) |
||||||
|
throws Exception { |
||||||
|
int numberOfUsefulSatellites = |
||||||
|
getNumberOfUsefulSatellites(usefulSatellitesToReceiverMeasurements); |
||||||
|
// deltaPseudorange is the pseudorange measurement residual
|
||||||
|
double[] deltaPseudorangesMeters = new double[numberOfUsefulSatellites]; |
||||||
|
double[][] satellitesPositionsECEFMeters = new double[numberOfUsefulSatellites][3]; |
||||||
|
|
||||||
|
// satellite PRNs
|
||||||
|
int[] satellitePRNs = new int[numberOfUsefulSatellites]; |
||||||
|
|
||||||
|
// Ionospheric model parameters
|
||||||
|
double[] alpha = |
||||||
|
{navMeassageProto.iono.alpha[0], navMeassageProto.iono.alpha[1], |
||||||
|
navMeassageProto.iono.alpha[2], navMeassageProto.iono.alpha[3]}; |
||||||
|
double[] beta = {navMeassageProto.iono.beta[0], navMeassageProto.iono.beta[1], |
||||||
|
navMeassageProto.iono.beta[2], navMeassageProto.iono.beta[3]}; |
||||||
|
// Weight matrix for the weighted least square
|
||||||
|
RealMatrix covarianceMatrixMetersSquare = |
||||||
|
new Array2DRowRealMatrix(numberOfUsefulSatellites, numberOfUsefulSatellites); |
||||||
|
calculateSatPosAndResiduals( |
||||||
|
navMeassageProto, |
||||||
|
usefulSatellitesToReceiverMeasurements, |
||||||
|
receiverGPSTowAtReceptionSeconds, |
||||||
|
receiverGpsWeek, |
||||||
|
dayOfYear1To366, |
||||||
|
userPositionECEFMeters, |
||||||
|
doAtmosphericCorrections, |
||||||
|
deltaPseudorangesMeters, |
||||||
|
satellitesPositionsECEFMeters, |
||||||
|
satellitePRNs, |
||||||
|
alpha, |
||||||
|
beta, |
||||||
|
covarianceMatrixMetersSquare); |
||||||
|
|
||||||
|
return new SatellitesPositionPseudorangesResidualAndCovarianceMatrix(satellitePRNs, |
||||||
|
satellitesPositionsECEFMeters, deltaPseudorangesMeters, |
||||||
|
covarianceMatrixMetersSquare.getData()); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculates and fill the position of all visible satellites: |
||||||
|
* {@code satellitesPositionsECEFMeters}, pseudorange measurement residual (difference of |
||||||
|
* measured to predicted pseudoranges): {@code deltaPseudorangesMeters} and covariance matrix from |
||||||
|
* the weighted least square: {@code covarianceMatrixMetersSquare}. An array of the satellite PRNs |
||||||
|
* {@code satellitePRNs} is as well filled. |
||||||
|
*/ |
||||||
|
private void calculateSatPosAndResiduals( |
||||||
|
GpsNavMessageProto navMeassageProto, |
||||||
|
List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, |
||||||
|
double receiverGPSTowAtReceptionSeconds, |
||||||
|
int receiverGpsWeek, |
||||||
|
int dayOfYear1To366, |
||||||
|
double[] userPositionECEFMeters, |
||||||
|
boolean doAtmosphericCorrections, |
||||||
|
double[] deltaPseudorangesMeters, |
||||||
|
double[][] satellitesPositionsECEFMeters, |
||||||
|
int[] satellitePRNs, |
||||||
|
double[] alpha, |
||||||
|
double[] beta, |
||||||
|
RealMatrix covarianceMatrixMetersSquare) |
||||||
|
throws Exception { |
||||||
|
// user position without the clock estimate
|
||||||
|
double[] userPositionTempECEFMeters = |
||||||
|
{userPositionECEFMeters[0], userPositionECEFMeters[1], userPositionECEFMeters[2]}; |
||||||
|
int satsCounter = 0; |
||||||
|
for (int i = 0; i < GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES; i++) { |
||||||
|
if (usefulSatellitesToReceiverMeasurements.get(i) != null) { |
||||||
|
GpsEphemerisProto ephemeridesProto = getEphemerisForSatellite(navMeassageProto, i + 1); |
||||||
|
// Correct the receiver time of week with the estimated receiver clock bias
|
||||||
|
receiverGPSTowAtReceptionSeconds = |
||||||
|
receiverGPSTowAtReceptionSeconds - userPositionECEFMeters[3] / SPEED_OF_LIGHT_MPS; |
||||||
|
|
||||||
|
double pseudorangeMeasurementMeters = |
||||||
|
usefulSatellitesToReceiverMeasurements.get(i).pseudorangeMeters; |
||||||
|
double pseudorangeUncertaintyMeters = |
||||||
|
usefulSatellitesToReceiverMeasurements.get(i).pseudorangeUncertaintyMeters; |
||||||
|
|
||||||
|
// Assuming uncorrelated pseudorange measurements, the covariance matrix will be diagonal as
|
||||||
|
// follows
|
||||||
|
covarianceMatrixMetersSquare.setEntry(satsCounter, satsCounter, |
||||||
|
pseudorangeUncertaintyMeters * pseudorangeUncertaintyMeters); |
||||||
|
|
||||||
|
// Calculate time of week at transmission time corrected with the satellite clock drift
|
||||||
|
GpsTimeOfWeekAndWeekNumber correctedTowAndWeek = |
||||||
|
calculateCorrectedTransmitTowAndWeek(ephemeridesProto, receiverGPSTowAtReceptionSeconds, |
||||||
|
receiverGpsWeek, pseudorangeMeasurementMeters); |
||||||
|
|
||||||
|
// calculate satellite position and velocity
|
||||||
|
PositionAndVelocity satPosECEFMetersVelocityMPS = SatellitePositionCalculator |
||||||
|
.calculateSatellitePositionAndVelocityFromEphemeris(ephemeridesProto, |
||||||
|
correctedTowAndWeek.gpsTimeOfWeekSeconds, correctedTowAndWeek.weekNumber, |
||||||
|
userPositionECEFMeters[0], userPositionECEFMeters[1], userPositionECEFMeters[2]); |
||||||
|
|
||||||
|
satellitesPositionsECEFMeters[satsCounter][0] = satPosECEFMetersVelocityMPS.positionXMeters; |
||||||
|
satellitesPositionsECEFMeters[satsCounter][1] = satPosECEFMetersVelocityMPS.positionYMeters; |
||||||
|
satellitesPositionsECEFMeters[satsCounter][2] = satPosECEFMetersVelocityMPS.positionZMeters; |
||||||
|
|
||||||
|
// Calculate ionospheric and tropospheric corrections
|
||||||
|
double ionosphericCorrectionMeters; |
||||||
|
double troposphericCorrectionMeters; |
||||||
|
if (doAtmosphericCorrections) { |
||||||
|
ionosphericCorrectionMeters = |
||||||
|
IonosphericModel.ionoKloboucharCorrectionSeconds( |
||||||
|
userPositionTempECEFMeters, |
||||||
|
satellitesPositionsECEFMeters[satsCounter], |
||||||
|
correctedTowAndWeek.gpsTimeOfWeekSeconds, |
||||||
|
alpha, |
||||||
|
beta, |
||||||
|
IonosphericModel.L1_FREQ_HZ) |
||||||
|
* SPEED_OF_LIGHT_MPS; |
||||||
|
|
||||||
|
troposphericCorrectionMeters = |
||||||
|
calculateTroposphericCorrectionMeters( |
||||||
|
dayOfYear1To366, |
||||||
|
satellitesPositionsECEFMeters, |
||||||
|
userPositionTempECEFMeters, |
||||||
|
satsCounter); |
||||||
|
} else { |
||||||
|
troposphericCorrectionMeters = 0.0; |
||||||
|
ionosphericCorrectionMeters = 0.0; |
||||||
|
} |
||||||
|
double predictedPseudorangeMeters = |
||||||
|
calculatePredictedPseudorange(userPositionECEFMeters, satellitesPositionsECEFMeters, |
||||||
|
userPositionTempECEFMeters, satsCounter, ephemeridesProto, correctedTowAndWeek, |
||||||
|
ionosphericCorrectionMeters, troposphericCorrectionMeters); |
||||||
|
|
||||||
|
// Pseudorange residual (difference of measured to predicted pseudoranges)
|
||||||
|
deltaPseudorangesMeters[satsCounter] = |
||||||
|
pseudorangeMeasurementMeters - predictedPseudorangeMeters; |
||||||
|
|
||||||
|
// Satellite PRNs
|
||||||
|
satellitePRNs[satsCounter] = i + 1; |
||||||
|
satsCounter++; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
/** Searches ephemerides list for the ephemeris associated with current satellite in process */ |
||||||
|
private GpsEphemerisProto getEphemerisForSatellite(GpsNavMessageProto navMeassageProto, |
||||||
|
int satPrn) { |
||||||
|
List<GpsEphemerisProto> ephemeridesList |
||||||
|
= new ArrayList<GpsEphemerisProto>(Arrays.asList(navMeassageProto.ephemerids)); |
||||||
|
GpsEphemerisProto ephemeridesProto = null; |
||||||
|
int ephemerisPrn = 0; |
||||||
|
for (GpsEphemerisProto ephProtoFromList : ephemeridesList) { |
||||||
|
ephemerisPrn = ephProtoFromList.prn; |
||||||
|
if (ephemerisPrn == satPrn) { |
||||||
|
ephemeridesProto = ephProtoFromList; |
||||||
|
break; |
||||||
|
} |
||||||
|
} |
||||||
|
return ephemeridesProto; |
||||||
|
} |
||||||
|
|
||||||
|
/** Calculates predicted pseudorange in meters */ |
||||||
|
private double calculatePredictedPseudorange( |
||||||
|
double[] userPositionECEFMeters, |
||||||
|
double[][] satellitesPositionsECEFMeters, |
||||||
|
double[] userPositionNoClockECEFMeters, |
||||||
|
int satsCounter, |
||||||
|
GpsEphemerisProto ephemeridesProto, |
||||||
|
GpsTimeOfWeekAndWeekNumber correctedTowAndWeek, |
||||||
|
double ionosphericCorrectionMeters, |
||||||
|
double troposphericCorrectionMeters) |
||||||
|
throws Exception { |
||||||
|
// Calcualte the satellite clock drift
|
||||||
|
double satelliteClockCorrectionMeters = |
||||||
|
SatelliteClockCorrectionCalculator.calculateSatClockCorrAndEccAnomAndTkIteratively( |
||||||
|
ephemeridesProto, |
||||||
|
correctedTowAndWeek.gpsTimeOfWeekSeconds, |
||||||
|
correctedTowAndWeek.weekNumber) |
||||||
|
.satelliteClockCorrectionMeters; |
||||||
|
|
||||||
|
double satelliteToUserDistanceMeters = |
||||||
|
GpsMathOperations.vectorNorm(GpsMathOperations.subtractTwoVectors( |
||||||
|
satellitesPositionsECEFMeters[satsCounter], userPositionNoClockECEFMeters)); |
||||||
|
// Predicted pseudorange
|
||||||
|
double predictedPseudorangeMeters = |
||||||
|
satelliteToUserDistanceMeters - satelliteClockCorrectionMeters + ionosphericCorrectionMeters |
||||||
|
+ troposphericCorrectionMeters + userPositionECEFMeters[3]; |
||||||
|
return predictedPseudorangeMeters; |
||||||
|
} |
||||||
|
|
||||||
|
/** Calculates the Gps tropospheric correction in meters */ |
||||||
|
private double calculateTroposphericCorrectionMeters(int dayOfYear1To366, |
||||||
|
double[][] satellitesPositionsECEFMeters, double[] userPositionTempECEFMeters, |
||||||
|
int satsCounter) { |
||||||
|
double troposphericCorrectionMeters; |
||||||
|
TopocentricAEDValues elevationAzimuthDist = |
||||||
|
EcefToTopocentricConverter.convertCartesianToTopocentericRadMeters( |
||||||
|
userPositionTempECEFMeters, GpsMathOperations.subtractTwoVectors( |
||||||
|
satellitesPositionsECEFMeters[satsCounter], userPositionTempECEFMeters)); |
||||||
|
|
||||||
|
GeodeticLlaValues lla = |
||||||
|
Ecef2LlaConverter.convertECEFToLLACloseForm(userPositionTempECEFMeters[0], |
||||||
|
userPositionTempECEFMeters[1], userPositionTempECEFMeters[2]); |
||||||
|
|
||||||
|
// Geoid of the area where the receiver is located is calculated once and used for the
|
||||||
|
// rest of the dataset as it change very slowly over wide area. This to save the delay
|
||||||
|
// associated with accessing Google Elevation API. We assume this very first iteration of WLS
|
||||||
|
// will compute the correct altitude above the ellipsoid of the ground at the latitude and
|
||||||
|
// longitude
|
||||||
|
if (calculateGeoidMeters) { |
||||||
|
double elevationAboveSeaLevelMeters = 0; |
||||||
|
if (elevationApiHelper == null){ |
||||||
|
System.out.println("No Google API key is set. Elevation above sea level is set to " |
||||||
|
+ "default 0 meters. This may cause inaccuracy in tropospheric correction."); |
||||||
|
} else { |
||||||
|
try { |
||||||
|
elevationAboveSeaLevelMeters = elevationApiHelper |
||||||
|
.getElevationAboveSeaLevelMeters( |
||||||
|
Math.toDegrees(lla.latitudeRadians), Math.toDegrees(lla.longitudeRadians) |
||||||
|
); |
||||||
|
} catch (Exception e){ |
||||||
|
e.printStackTrace(); |
||||||
|
System.out.println("Error when getting elevation from Google Server. " |
||||||
|
+ "Could be wrong Api key or network error. Elevation above sea level is set to " |
||||||
|
+ "default 0 meters. This may cause inaccuracy in tropospheric correction."); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
geoidHeightMeters = ElevationApiHelper.calculateGeoidHeightMeters( |
||||||
|
lla.altitudeMeters, |
||||||
|
elevationAboveSeaLevelMeters |
||||||
|
); |
||||||
|
troposphericCorrectionMeters = TroposphericModelEgnos.calculateTropoCorrectionMeters( |
||||||
|
elevationAzimuthDist.elevationRadians, lla.latitudeRadians, elevationAboveSeaLevelMeters, |
||||||
|
dayOfYear1To366); |
||||||
|
} else { |
||||||
|
troposphericCorrectionMeters = TroposphericModelEgnos.calculateTropoCorrectionMeters( |
||||||
|
elevationAzimuthDist.elevationRadians, lla.latitudeRadians, |
||||||
|
lla.altitudeMeters - geoidHeightMeters, dayOfYear1To366); |
||||||
|
} |
||||||
|
return troposphericCorrectionMeters; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Gets the number of useful satellites from a list of |
||||||
|
* {@link GpsMeasurementWithRangeAndUncertainty}. |
||||||
|
*/ |
||||||
|
private int getNumberOfUsefulSatellites( |
||||||
|
List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements) { |
||||||
|
// calculate the number of useful satellites
|
||||||
|
int numberOfUsefulSatellites = 0; |
||||||
|
for (int i = 0; i < usefulSatellitesToReceiverMeasurements.size(); i++) { |
||||||
|
if (usefulSatellitesToReceiverMeasurements.get(i) != null) { |
||||||
|
numberOfUsefulSatellites++; |
||||||
|
} |
||||||
|
} |
||||||
|
return numberOfUsefulSatellites; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Computes the GPS time of week at the time of transmission and as well the corrected GPS week |
||||||
|
* taking into consideration week rollover. The returned GPS time of week is corrected by the |
||||||
|
* computed satellite clock drift. The result is stored in an instance of |
||||||
|
* {@link GpsTimeOfWeekAndWeekNumber} |
||||||
|
* |
||||||
|
* @param ephemerisProto parameters of the navigation message |
||||||
|
* @param receiverGpsTowAtReceptionSeconds Receiver estimate of GPS time of week when signal was |
||||||
|
* received (seconds) |
||||||
|
* @param receiverGpsWeek Receiver estimate of GPS week (0-1024+) |
||||||
|
* @param pseudorangeMeters Measured pseudorange in meters |
||||||
|
* @return GpsTimeOfWeekAndWeekNumber Object containing Gps time of week and week number. |
||||||
|
*/ |
||||||
|
private static GpsTimeOfWeekAndWeekNumber calculateCorrectedTransmitTowAndWeek( |
||||||
|
GpsEphemerisProto ephemerisProto, double receiverGpsTowAtReceptionSeconds, |
||||||
|
int receiverGpsWeek, double pseudorangeMeters) throws Exception { |
||||||
|
// GPS time of week at time of transmission: Gps time corrected for transit time (page 98 ICD
|
||||||
|
// GPS 200)
|
||||||
|
double receiverGpsTowAtTimeOfTransmission = |
||||||
|
receiverGpsTowAtReceptionSeconds - pseudorangeMeters / SPEED_OF_LIGHT_MPS; |
||||||
|
|
||||||
|
// Adjust for week rollover
|
||||||
|
if (receiverGpsTowAtTimeOfTransmission < 0) { |
||||||
|
receiverGpsTowAtTimeOfTransmission += SECONDS_IN_WEEK; |
||||||
|
receiverGpsWeek -= 1; |
||||||
|
} else if (receiverGpsTowAtTimeOfTransmission > SECONDS_IN_WEEK) { |
||||||
|
receiverGpsTowAtTimeOfTransmission -= SECONDS_IN_WEEK; |
||||||
|
receiverGpsWeek += 1; |
||||||
|
} |
||||||
|
|
||||||
|
// Compute the satellite clock correction term (Seconds)
|
||||||
|
double clockCorrectionSeconds = |
||||||
|
SatelliteClockCorrectionCalculator.calculateSatClockCorrAndEccAnomAndTkIteratively( |
||||||
|
ephemerisProto, receiverGpsTowAtTimeOfTransmission, |
||||||
|
receiverGpsWeek).satelliteClockCorrectionMeters / SPEED_OF_LIGHT_MPS; |
||||||
|
|
||||||
|
// Correct with the satellite clock correction term
|
||||||
|
double receiverGpsTowAtTimeOfTransmissionCorrectedSec = |
||||||
|
receiverGpsTowAtTimeOfTransmission + clockCorrectionSeconds; |
||||||
|
|
||||||
|
// Adjust for week rollover due to satellite clock correction
|
||||||
|
if (receiverGpsTowAtTimeOfTransmissionCorrectedSec < 0.0) { |
||||||
|
receiverGpsTowAtTimeOfTransmissionCorrectedSec += SECONDS_IN_WEEK; |
||||||
|
receiverGpsWeek -= 1; |
||||||
|
} |
||||||
|
if (receiverGpsTowAtTimeOfTransmissionCorrectedSec > SECONDS_IN_WEEK) { |
||||||
|
receiverGpsTowAtTimeOfTransmissionCorrectedSec -= SECONDS_IN_WEEK; |
||||||
|
receiverGpsWeek += 1; |
||||||
|
} |
||||||
|
return new GpsTimeOfWeekAndWeekNumber(receiverGpsTowAtTimeOfTransmissionCorrectedSec, |
||||||
|
receiverGpsWeek); |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Calculates the Geometry matrix (describing user to satellite geometry) given a list of |
||||||
|
* satellite positions in ECEF coordinates in meters and the user position in ECEF in meters. |
||||||
|
* |
||||||
|
* <p>The geometry matrix has four columns, and rows equal to the number of satellites. For each |
||||||
|
* of the rows (i.e. for each of the satellites used), the columns are filled with the normalized |
||||||
|
* line–of-sight vectors and 1 s for the fourth column. |
||||||
|
* |
||||||
|
* <p>Source: Parkinson, B.W., Spilker Jr., J.J.: ‘Global positioning system: theory and |
||||||
|
* applications’ page 413 |
||||||
|
*/ |
||||||
|
private static double[][] calculateGeometryMatrix(double[][] satellitePositionsECEFMeters, |
||||||
|
double[] userPositionECEFMeters) { |
||||||
|
|
||||||
|
double[][] geometeryMatrix = new double[satellitePositionsECEFMeters.length][4]; |
||||||
|
for (int i = 0; i < satellitePositionsECEFMeters.length; i++) { |
||||||
|
geometeryMatrix[i][3] = 1; |
||||||
|
} |
||||||
|
// iterate over all satellites
|
||||||
|
for (int i = 0; i < satellitePositionsECEFMeters.length; i++) { |
||||||
|
double[] r = {satellitePositionsECEFMeters[i][0] - userPositionECEFMeters[0], |
||||||
|
satellitePositionsECEFMeters[i][1] - userPositionECEFMeters[1], |
||||||
|
satellitePositionsECEFMeters[i][2] - userPositionECEFMeters[2]}; |
||||||
|
double norm = Math.sqrt(Math.pow(r[0], 2) + Math.pow(r[1], 2) + Math.pow(r[2], 2)); |
||||||
|
for (int j = 0; j < 3; j++) { |
||||||
|
geometeryMatrix[i][j] = |
||||||
|
(userPositionECEFMeters[j] - satellitePositionsECEFMeters[i][j]) / norm; |
||||||
|
} |
||||||
|
} |
||||||
|
return geometeryMatrix; |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Class containing satellites' PRNs, satellites' positions in ECEF meters, the pseudorange |
||||||
|
* residual per visible satellite in meters and the covariance matrix of the |
||||||
|
* pseudoranges in meters square |
||||||
|
*/ |
||||||
|
protected static class SatellitesPositionPseudorangesResidualAndCovarianceMatrix { |
||||||
|
|
||||||
|
/** Satellites' PRNs */ |
||||||
|
protected final int[] satellitePRNs; |
||||||
|
|
||||||
|
/** ECEF positions (meters) of useful satellites */ |
||||||
|
protected final double[][] satellitesPositionsMeters; |
||||||
|
|
||||||
|
/** Pseudorange measurement residuals (difference of measured to predicted pseudoranges) */ |
||||||
|
protected final double[] pseudorangeResidualsMeters; |
||||||
|
|
||||||
|
/** Pseudorange covariance Matrix for the weighted least squares (meters square) */ |
||||||
|
protected final double[][] covarianceMatrixMetersSquare; |
||||||
|
|
||||||
|
/** Constructor */ |
||||||
|
private SatellitesPositionPseudorangesResidualAndCovarianceMatrix(int[] satellitePRNs, |
||||||
|
double[][] satellitesPositionsMeters, double[] pseudorangeResidualsMeters, |
||||||
|
double[][] covarianceMatrixMetersSquare) { |
||||||
|
this.satellitePRNs = satellitePRNs; |
||||||
|
this.satellitesPositionsMeters = satellitesPositionsMeters; |
||||||
|
this.pseudorangeResidualsMeters = pseudorangeResidualsMeters; |
||||||
|
this.covarianceMatrixMetersSquare = covarianceMatrixMetersSquare; |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Class containing GPS time of week in seconds and GPS week number |
||||||
|
*/ |
||||||
|
private static class GpsTimeOfWeekAndWeekNumber { |
||||||
|
/** GPS time of week in seconds */ |
||||||
|
private final double gpsTimeOfWeekSeconds; |
||||||
|
|
||||||
|
/** GPS week number */ |
||||||
|
private final int weekNumber; |
||||||
|
|
||||||
|
/** Constructor */ |
||||||
|
private GpsTimeOfWeekAndWeekNumber(double gpsTimeOfWeekSeconds, int weekNumber) { |
||||||
|
this.gpsTimeOfWeekSeconds = gpsTimeOfWeekSeconds; |
||||||
|
this.weekNumber = weekNumber; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
/** |
||||||
|
* Uses the common reception time approach to calculate pseudoranges from the time of week |
||||||
|
* measurements reported by the receiver according to http://cdn.intechopen.com/pdfs-wm/27712.pdf.
|
||||||
|
* As well computes the pseudoranges uncertainties for each input satellite |
||||||
|
*/ |
||||||
|
@VisibleForTesting |
||||||
|
static List<GpsMeasurementWithRangeAndUncertainty> computePseudorangeAndUncertainties( |
||||||
|
List<GpsMeasurement> usefulSatellitesToReceiverMeasurements, |
||||||
|
Long[] usefulSatellitesToTOWNs, |
||||||
|
long largestTowNs) { |
||||||
|
|
||||||
|
List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToPseudorangeMeasurements = |
||||||
|
Arrays.asList( |
||||||
|
new GpsMeasurementWithRangeAndUncertainty |
||||||
|
[GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES]); |
||||||
|
for (int i = 0; i < GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES; i++) { |
||||||
|
if (usefulSatellitesToTOWNs[i] != null) { |
||||||
|
double deltai = largestTowNs - usefulSatellitesToTOWNs[i]; |
||||||
|
double pseudorangeMeters = |
||||||
|
(AVERAGE_TRAVEL_TIME_SECONDS + deltai * SECONDS_PER_NANO) * SPEED_OF_LIGHT_MPS; |
||||||
|
|
||||||
|
double signalToNoiseRatioLinear = |
||||||
|
Math.pow(10, usefulSatellitesToReceiverMeasurements.get(i).signalToNoiseRatioDb / 10.0); |
||||||
|
// From Global Positoning System book, Misra and Enge, page 416, the uncertainty of the
|
||||||
|
// pseudorange measurement is calculated next.
|
||||||
|
// For GPS C/A code chip width Tc = 1 microseconds. Narrow correlator with spacing d = 0.1
|
||||||
|
// chip and an average time of DLL correlator T of 20 milliseconds are used.
|
||||||
|
double sigmaMeters = |
||||||
|
SPEED_OF_LIGHT_MPS |
||||||
|
* GPS_CHIP_WIDTH_T_C_SEC |
||||||
|
* Math.sqrt( |
||||||
|
GPS_CORRELATOR_SPACING_IN_CHIPS |
||||||
|
/ (4 * GPS_DLL_AVERAGING_TIME_SEC * signalToNoiseRatioLinear)); |
||||||
|
usefulSatellitesToPseudorangeMeasurements.set( |
||||||
|
i, |
||||||
|
new GpsMeasurementWithRangeAndUncertainty( |
||||||
|
usefulSatellitesToReceiverMeasurements.get(i), pseudorangeMeters, sigmaMeters)); |
||||||
|
} |
||||||
|
} |
||||||
|
return usefulSatellitesToPseudorangeMeasurements; |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
@ -1 +1 @@ |
|||||||
include ':app' |
include ':app', ':pseudorange' |
||||||
|
|||||||
Loading…
Reference in new issue