Skip to content

Commit 4868023

Browse files
Lezh1kLezh1k
committed
Added ability to not use GeoHash filter in service. Formed it as usual class, it doesn't require to implement any interface
1 parent 86f3090 commit 4868023

File tree

8 files changed

+117
-115
lines changed

8 files changed

+117
-115
lines changed

‎C/src/SensorController.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -208,12 +208,12 @@ FilterInputFile(const QString &inputFile,
208208
if (!initialData)
209209
break;
210210

211-
static const int GPS_COUNT = 2;
211+
static const int GPS_COUNT = 1;
212212
int gps_count = GPS_COUNT;
213213

214214
bool usePredicted = false;
215-
bool noise = true;
216-
static const double accDev = 0.1;
215+
bool noise = false;
216+
static const double accDev = 0.5;
217217

218218
double writeDt = sd.timestamp;
219219
double xVel = sd.speed * cos(sd.course);

‎C/src/mainwindow.cpp‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -180,10 +180,9 @@ MainWindow::initMap(QWebEnginePage *page,
180180
std::vector<geopoint_t> lstJavaFilter = CoordGetFromFile(pathToCoordsFile, LMT_FILTERED_GPS_DATA);
181181
// std::vector<geopoint_t> lstJavaFilter = CoordGetFromFile(filteredCoordsFile2, LMT_FILTERED_GPS_DATA);
182182

183-
const int filterPrec = 7;
184-
const int minPoints = 3;
183+
const int filterPrec = 8;
184+
const int minPoints = 2;
185185

186-
lstJavaFilter.clear();
187186
// qDebug() << "RealTime Src distance: " << filterDistanceRealTime(lstCoords, GEOHASH_MAX_PRECISION, 1);
188187
// qDebug() << "RealTime Desk distance: " << filterDistanceRealTime(lstGeoFilter, GEOHASH_MAX_PRECISION, 1);
189188
// qDebug() << "RealTime Java distance: " << filterDistanceRealTime(lstJavaFilter, GEOHASH_MAX_PRECISION, 1);

‎app/src/main/java/com/example/lezh1k/sensordatacollector/MainActivity.java‎

Lines changed: 28 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -123,29 +123,27 @@ protected void onProgressUpdate(Object... values) {
123123
TextView tvStatus = (TextView) findViewById(R.id.tvStatus);
124124
TextView tvDistance = (TextView) findViewById(R.id.tvDistance);
125125
if (m_isLogging) {
126-
ServicesHelper.getLocationService(owner, value -> {
127-
KalmanLocationService kls = (KalmanLocationService) value;
128-
GeohashRTFilter kdl = kls.getGeohashRTFilter();
129-
tvDistance.setText(String.format(
130-
"Distance (geo): %fm\n" +
131-
"Distance (geo) HP: %fm\n" +
132-
"Distance as is : %fm\n" +
133-
"Distance as is HP: %fm",
134-
kdl.getDistanceGeoFiltered(),
135-
kdl.getDistanceGeoFilteredHP(),
136-
kdl.getDistanceAsIs(),
137-
kdl.getDistanceAsIsHP()));
138-
});
139-
} else {
140-
if (m_sensorCalibrator.isInProgress()) {
141-
tvStatus.setText(m_sensorCalibrator.getCalibrationStatus());
142-
if (m_sensorCalibrator.getDcAbsLinearAcceleration().isCalculated() &&
143-
m_sensorCalibrator.getDcLinearAcceleration().isCalculated()) {
144-
set_isCalibrating(false, false);
145-
tvDistance.setText(m_sensorCalibrator.getDcLinearAcceleration().deviationInfoString());
146-
}
126+
if (m_geoHashRTFilter == null)
127+
return;
147128

129+
tvDistance.setText(String.format(
130+
"Distance (geo): %fm\n" +
131+
"Distance (geo) HP: %fm\n" +
132+
"Distance as is : %fm\n" +
133+
"Distance as is HP: %fm",
134+
m_geoHashRTFilter.getDistanceGeoFiltered(),
135+
m_geoHashRTFilter.getDistanceGeoFilteredHP(),
136+
m_geoHashRTFilter.getDistanceAsIs(),
137+
m_geoHashRTFilter.getDistanceAsIsHP()));
138+
} else {
139+
if (!m_sensorCalibrator.isInProgress())
140+
return;
148141

142+
tvStatus.setText(m_sensorCalibrator.getCalibrationStatus());
143+
if (m_sensorCalibrator.getDcAbsLinearAcceleration().isCalculated() &&
144+
m_sensorCalibrator.getDcLinearAcceleration().isCalculated()) {
145+
set_isCalibrating(false, false);
146+
tvDistance.setText(m_sensorCalibrator.getDcLinearAcceleration().deviationInfoString());
149147
}
150148
}
151149
}
@@ -156,6 +154,7 @@ protected void onProgressUpdate(Object... values) {
156154
private MapboxMap m_map;
157155
private MapView m_mapView;
158156

157+
private GeohashRTFilter m_geoHashRTFilter;
159158
private SensorCalibrator m_sensorCalibrator = null;
160159
private boolean m_isLogging = false;
161160
private boolean m_isCalibrating = false;
@@ -179,16 +178,21 @@ private void set_isLogging(boolean isLogging) {
179178
if (value.IsRunning())
180179
return;
181180
value.stop();
181+
182+
if (m_geoHashRTFilter != null)
183+
m_geoHashRTFilter.stop();
184+
182185
initXlogPrintersFileName();
183186
KalmanLocationService.Settings settings =
184187
new KalmanLocationService.Settings(Utils.ACCELEROMETER_DEFAULT_DEVIATION,
185188
Utils.GPS_MIN_DISTANCE,
186189
Utils.GPS_MIN_TIME,
187-
Utils.SENSOR_DEFAULT_FREQ_HZ,
188190
Utils.GEOHASH_DEFAULT_PREC,
189191
Utils.GEOHASH_DEFAULT_MIN_POINT_COUNT,
192+
Utils.SENSOR_DEFAULT_FREQ_HZ,
190193
this);
191194
value.reset(settings); //warning!! here you can adjust your filter behavior
195+
m_geoHashRTFilter.reset(this);
192196
value.start();
193197
});
194198
btnStartStopText = "Stop tracking";
@@ -373,7 +377,7 @@ public void setupMap(@Nullable Bundle savedInstanceState) {
373377
m_mapView = (MapView) findViewById(R.id.mapView);
374378
m_mapView.onCreate(savedInstanceState);
375379

376-
m_presenter = new MapPresenter(this, this);
380+
m_presenter = new MapPresenter(this, this, m_geoHashRTFilter);
377381
m_mapView.getMapAsync(mapboxMap -> {
378382
m_map = mapboxMap;
379383
m_map.setStyleUrl(BuildConfig.lightMapStyle);
@@ -390,7 +394,6 @@ public void setupMap(@Nullable Bundle savedInstanceState) {
390394
ServicesHelper.addLocationServiceInterface(this);
391395
m_presenter.getRoute();
392396
});
393-
394397
}
395398

396399
@Override
@@ -400,6 +403,7 @@ protected void onCreate(Bundle savedInstanceState) {
400403
Thread.setDefaultUncaughtExceptionHandler(_unCaughtExceptionHandler);
401404
Mapbox.getInstance(this, BuildConfig.access_token);
402405
setContentView(R.layout.activity_main);
406+
m_geoHashRTFilter = new GeohashRTFilter(Utils.GEOHASH_DEFAULT_PREC, Utils.GEOHASH_DEFAULT_MIN_POINT_COUNT);
403407
setupMap(savedInstanceState);
404408

405409
CheckBox cbGps, cbFilteredKalman, cbFilteredKalmanGeo;

‎app/src/main/java/com/example/lezh1k/sensordatacollector/Presenters/MapPresenter.java‎

Lines changed: 12 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,10 @@
11
package com.example.lezh1k.sensordatacollector.Presenters;
22

33
import android.content.Context;
4-
import android.content.Intent;
54
import android.location.Location;
6-
import android.util.Log;
75

6+
import com.example.gpsacckalmanfusion.Lib.Commons.Utils;
7+
import com.example.gpsacckalmanfusion.Lib.Loggers.GeohashRTFilter;
88
import com.example.gpsacckalmanfusion.Lib.Services.KalmanLocationService;
99
import com.example.gpsacckalmanfusion.Lib.Services.ServicesHelper;
1010
import com.example.lezh1k.sensordatacollector.Interfaces.MapInterface;
@@ -22,34 +22,36 @@
2222
public class MapPresenter {
2323
private MapInterface mapInterface;
2424
private Context context;
25+
private GeohashRTFilter m_geoHashRTFilter;
2526

26-
public MapPresenter(Context context, MapInterface mapInterface) {
27+
public MapPresenter(Context context, MapInterface mapInterface, GeohashRTFilter geoHashRTFilter) {
2728
this.mapInterface = mapInterface;
2829
this.context = context;
30+
m_geoHashRTFilter = geoHashRTFilter;
2931
}
3032

3133
public void onLocationChanged(Location location, CameraPosition currentCameraPosition) {
3234
CameraPosition.Builder position =
3335
new CameraPosition.Builder(currentCameraPosition).target(new LatLng(location));
3436
mapInterface.moveCamera(position.build());
3537
getRoute();
38+
m_geoHashRTFilter.filter(location);
3639
}
3740

3841
public void getRoute() {
3942
ServicesHelper.getLocationService(context, value -> {
4043
KalmanLocationService kls = (KalmanLocationService) value;
41-
42-
List<LatLng> routeFilteredKalman = new ArrayList<>(value.getTrack().size());
44+
45+
List<LatLng> routGpsAsIs = new ArrayList<>(kls.getGpsTrack().size());
46+
List<LatLng> routeFilteredKalman = new ArrayList<>(value.getKalmanOnlyFilteredTrack().size());
4347
List<LatLng> routeFilteredWithGeoHash =
44-
new ArrayList<>(kls.getGeohashRTFilter().getGeoFilteredTrack().size());
45-
List<LatLng> routGpsAsIs =
46-
new ArrayList<>(kls.getGpsTrack().size());
48+
new ArrayList<>(m_geoHashRTFilter.getGeoFilteredTrack().size());
4749

48-
for (Location location : new ArrayList<>(value.getTrack())) {
50+
for (Location location : new ArrayList<>(value.getKalmanOnlyFilteredTrack())) {
4951
routeFilteredKalman.add(new LatLng(location.getLatitude(), location.getLongitude()));
5052
}
5153

52-
for (Location location : new ArrayList<>(kls.getGeohashRTFilter().getGeoFilteredTrack())) {
54+
for (Location location : new ArrayList<>(m_geoHashRTFilter.getGeoFilteredTrack())) {
5355
routeFilteredWithGeoHash.add(new LatLng(location.getLatitude(),
5456
location.getLongitude()));
5557
}

‎madlocationmanager/src/main/java/com/example/gpsacckalmanfusion/Lib/Commons/Coordinates.java‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313
public class Coordinates {
1414
private static final double EARTH_RADIUS = 6371.0 * 1000.0; // meters
1515

16-
public static double geoDistanceMeters(double lon1, double lat1, double lon2, double lat2) {
16+
public static double distanceBetween(double lon1, double lat1, double lon2, double lat2) {
1717
double deltaLon = Math.toRadians(lon2 - lon1);
1818
double deltaLat = Math.toRadians(lat2 - lat1);
1919
double a =
@@ -26,7 +26,7 @@ public static double geoDistanceMeters(double lon1, double lat1, double lon2, do
2626
}
2727

2828
public static double longitudeToMeters(double lon) {
29-
double distance = geoDistanceMeters(lon, 0.0, 0.0, 0.0);
29+
double distance = distanceBetween(lon, 0.0, 0.0, 0.0);
3030
return distance * (lon < 0.0 ? -1.0 : 1.0);
3131
}
3232

@@ -39,7 +39,7 @@ public static GeoPoint metersToGeoPoint(double lonMeters,
3939
}
4040

4141
public static double latitudeToMeters(double lat) {
42-
double distance = geoDistanceMeters(0.0, lat, 0.0, 0.0);
42+
double distance = distanceBetween(0.0, lat, 0.0, 0.0);
4343
return distance * (lat < 0.0 ? -1.0 : 1.0);
4444
}
4545

@@ -132,7 +132,7 @@ public static double calculateDistance(GeoPoint track[]) {
132132
lastLat = track[0].Latitude;
133133

134134
for (int i = 1; i < track.length; ++i) {
135-
distance += Coordinates.geoDistanceMeters(
135+
distance += Coordinates.distanceBetween(
136136
lastLat, lastLon,
137137
track[i].Latitude, track[i].Longitude);
138138
lastLat = track[i].Latitude;

‎madlocationmanager/src/main/java/com/example/gpsacckalmanfusion/Lib/Filters/GPSAccKalmanFilter.java‎

Lines changed: 19 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -12,13 +12,17 @@ public class GPSAccKalmanFilter {
1212
private int m_predictCount;
1313
private KalmanFilter m_kf;
1414
private double m_accSigma;
15+
private boolean m_useGpsSpeed;
1516

16-
public GPSAccKalmanFilter(double x, double y,
17+
public GPSAccKalmanFilter(boolean useGpsSpeed,
18+
double x, double y,
1719
double xVel, double yVel,
1820
double accDev, double posDev,
1921
double timeStampMs) {
22+
int mesDim = useGpsSpeed ? 4 : 2;
23+
m_useGpsSpeed = useGpsSpeed;
2024

21-
m_kf = new KalmanFilter(4, 4, 1);
25+
m_kf = new KalmanFilter(4, mesDim, 1);
2226
m_timeStampMsPredict = m_timeStampMsUpdate = timeStampMs;
2327
m_accSigma = accDev;
2428
m_predictCount = 0;
@@ -56,14 +60,18 @@ private void rebuildB(double dtPredict) {
5660
}
5761

5862
private void rebuildR(double posSigma, double velSigma) {
59-
//Warning!!! this matrix depends on measure dimension.
60-
double R[] = {
61-
posSigma, 0.0, 0.0, 0.0,
62-
0.0, posSigma, 0.0, 0.0,
63-
0.0, 0.0, velSigma, 0.0,
64-
0.0, 0.0, 0.0, velSigma
65-
};
66-
m_kf.R.setData(R);
63+
if (m_useGpsSpeed) {
64+
double R[] = {
65+
posSigma, 0.0, 0.0, 0.0,
66+
0.0, posSigma, 0.0, 0.0,
67+
0.0, 0.0, velSigma, 0.0,
68+
0.0, 0.0, 0.0, velSigma
69+
};
70+
m_kf.R.setData(R);
71+
} else {
72+
m_kf.R.setIdentity();
73+
m_kf.R.scale(posSigma);
74+
}
6775
}
6876

6977
private void rebuildQ(double dtUpdate,
@@ -74,6 +82,7 @@ private void rebuildQ(double dtUpdate,
7482
double velDev = accDev * m_predictCount;
7583
double posDev = velDev * m_predictCount / 2;
7684
double covDev = velDev * posDev;
85+
7786
double posSig = posDev * posDev;
7887
double velSig = velDev * velDev;
7988

‎madlocationmanager/src/main/java/com/example/gpsacckalmanfusion/Lib/Loggers/GeohashRTFilter.java‎

Lines changed: 5 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -7,9 +7,6 @@
77
import com.example.gpsacckalmanfusion.Lib.Commons.Utils;
88
import com.example.gpsacckalmanfusion.Lib.Filters.GeoHash;
99
import com.example.gpsacckalmanfusion.Lib.Interfaces.ILogger;
10-
import com.example.gpsacckalmanfusion.Lib.Interfaces.LocationServiceInterface;
11-
import com.example.gpsacckalmanfusion.Lib.Interfaces.LocationServiceStatusInterface;
12-
import com.example.gpsacckalmanfusion.Lib.Services.ServicesHelper;
1310

1411
import java.util.ArrayList;
1512
import java.util.Arrays;
@@ -19,7 +16,7 @@
1916
* Created by lezh1k on 2/13/18.
2017
*/
2118

22-
public class GeohashRTFilter implements LocationServiceInterface, LocationServiceStatusInterface {
19+
public class GeohashRTFilter {
2320

2421
private double m_distanceGeoFiltered = 0.0;
2522
private double m_distanceGeoFilteredHP = 0.0;
@@ -53,8 +50,6 @@ public GeohashRTFilter(int geohashPrecision,
5350
m_geohashMinPointCount = geohashMinPointCount;
5451
m_geoFilteredTrack = new ArrayList<>();
5552
reset(null);
56-
ServicesHelper.addLocationServiceInterface(this);
57-
ServicesHelper.addLocationServiceStatusInterface(this);
5853
}
5954

6055
public double getDistanceGeoFiltered() {
@@ -91,8 +86,7 @@ public void reset(ILogger logger) {
9186
private float hpResBuffAsIs[] = new float[3];
9287
private float hpResBuffGeo[] = new float[3];
9388

94-
@Override
95-
public void locationChanged(Location loc) {
89+
public void filter(Location loc) {
9690
if (m_logger != null) {
9791
String toLog = String.format("%d%d FKS : lat=%f, lon=%f, alt=%f",
9892
Utils.LogMessageType.FILTERED_GPS_DATA.ordinal(),
@@ -114,7 +108,7 @@ public void locationChanged(Location loc) {
114108
return;
115109
}
116110

117-
m_distanceAsIs += Coordinates.geoDistanceMeters(
111+
m_distanceAsIs += Coordinates.distanceBetween(
118112
lastGeoPointAsIs.Longitude,
119113
lastGeoPointAsIs.Latitude,
120114
pi.Longitude,
@@ -139,7 +133,7 @@ public void locationChanged(Location loc) {
139133
currentGeoPoint.Longitude /= pointsInCurrentGeohashCount;
140134

141135
if (lastApprovedGeoPoint.Latitude != COORD_NOT_INITIALIZED) {
142-
double dd1 = Coordinates.geoDistanceMeters(
136+
double dd1 = Coordinates.distanceBetween(
143137
lastApprovedGeoPoint.Longitude,
144138
lastApprovedGeoPoint.Latitude,
145139
currentGeoPoint.Longitude,
@@ -185,7 +179,7 @@ public void stop() {
185179
currentGeoPoint.Longitude /= pointsInCurrentGeohashCount;
186180

187181
if (lastApprovedGeoPoint.Latitude != COORD_NOT_INITIALIZED) {
188-
double dd1 = Coordinates.geoDistanceMeters(
182+
double dd1 = Coordinates.distanceBetween(
189183
lastApprovedGeoPoint.Longitude,
190184
lastApprovedGeoPoint.Latitude,
191185
currentGeoPoint.Longitude,
@@ -209,24 +203,4 @@ public void stop() {
209203
currentGeoPoint.Latitude = currentGeoPoint.Longitude = 0.0;
210204
}
211205
}
212-
213-
@Override
214-
public void serviceStatusChanged(int status) {
215-
216-
}
217-
218-
@Override
219-
public void GPSStatusChanged(int activeSatellites) {
220-
221-
}
222-
223-
@Override
224-
public void GPSEnabledChanged(boolean enabled) {
225-
226-
}
227-
228-
@Override
229-
public void lastLocationAccuracyChanged(float accuracy) {
230-
231-
}
232206
}

0 commit comments

Comments
 (0)