Skip to content

Commit ae6217b

Browse files
betaBisonbetaBison
committed
add fde and kaggle demos
1 parent 90ee1be commit ae6217b

3 files changed

Lines changed: 412 additions & 10 deletions

File tree

‎gnss_lib_py/algorithms/fde.py‎

Lines changed: 42 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111

1212
from gnss_lib_py.algorithms.residuals import solve_residuals
1313
from gnss_lib_py.algorithms.snapshot import solve_wls
14+
from gnss_lib_py.utils.coordinates import geodetic_to_ecef
1415

1516
def solve_fde(navdata, method="residual", remove_outliers=False,
1617
max_faults=None, threshold=None,verbose=False,
@@ -49,6 +50,30 @@ def solve_fde(navdata, method="residual", remove_outliers=False,
4950
5051
"""
5152

53+
# corr_pr_m_new = []
54+
# for _, _, navdata_subset in navdata.loop_time("gps_millis"):
55+
# # add ECEF coordinates
56+
# ecef_xyz = geodetic_to_ecef(navdata_subset[["lat_rx_gt_deg",
57+
# "lon_rx_gt_deg",
58+
# "alt_rx_gt_m"
59+
# ]])
60+
# navdata_subset["x_rx_gt_m"] = ecef_xyz[0,:]
61+
# navdata_subset["y_rx_gt_m"] = ecef_xyz[1,:]
62+
# navdata_subset["z_rx_gt_m"] = ecef_xyz[2,:]
63+
# # data.to_csv("example_smartloc_pre_wls.csv")
64+
#
65+
# # estimate receiver clock bias
66+
# wls_state_estimate = solve_wls(navdata_subset,
67+
# only_bias = True,
68+
# # delta_t_decimals=6,
69+
# receiver_state=navdata_subset,
70+
# )
71+
#
72+
# corr_pr_m_new += list(navdata_subset["corr_pr_m"] - wls_state_estimate["b_rx_wls_m"])
73+
#
74+
# navdata["corr_pr_m"] = corr_pr_m_new
75+
76+
5277
if method == "residual":
5378
navdata = fde_residual_old(navdata, max_faults=max_faults,
5479
threshold=threshold,verbose=verbose,
@@ -111,7 +136,7 @@ def fde_edm(navdata, max_faults=None, threshold=1.0, verbose=False,
111136
MIN_SATELLITES = 4
112137

113138
if threshold is None:
114-
threshold = 1E6
139+
threshold = 0.6
115140

116141
# number of check indexes
117142
if max_faults is None:
@@ -127,17 +152,24 @@ def fde_edm(navdata, max_faults=None, threshold=1.0, verbose=False,
127152
time_start = time.time()
128153

129154
nsl = len(navdata_subset)
130-
if verbose:
131-
print("gt faults:",np.argwhere(navdata_subset["fault_gt"]==1)[:,0])
132155
sv_m = navdata_subset[["x_sv_m","y_sv_m","z_sv_m"]]
133156
corr_pr_m = navdata_subset["corr_pr_m"]
134157

135158
# remove NaN indexes
136159
nan_idxs = sorted(list(set(np.arange(nsl)[np.isnan(sv_m).any(axis=0)]).union( \
137160
set(np.arange(nsl)[np.isnan(corr_pr_m)]).union( \
138161
))))[::-1]
162+
navdata_subset.remove(cols=nan_idxs,inplace=True)
163+
sv_m = navdata_subset[["x_sv_m","y_sv_m","z_sv_m"]]
164+
corr_pr_m = navdata_subset["corr_pr_m"]
165+
166+
if verbose:
167+
print("nan_idxs:",nan_idxs)
168+
print("gt faults:",np.argwhere(navdata_subset["NLOS (0 == no, 1 == yes, 2 == No Information)"]==1)[:,0])
169+
139170
fault_idxs = []
140-
orig_idxs = np.arange(nsl+1) # add one for receiver index
171+
orig_idxs = np.arange(len(navdata_subset)+1) # add one for receiver index
172+
pre_nan_idxs = np.delete(np.arange(nsl+1), nan_idxs)
141173

142174
edm = _edm_from_satellites_ranges(sv_m,corr_pr_m)
143175

@@ -151,17 +183,19 @@ def fde_edm(navdata, max_faults=None, threshold=1.0, verbose=False,
151183
or (max_faults is not None and len(fault_idxs) == max_faults):
152184
break
153185

154-
155186
try:
156187
edm_detect = np.delete(np.delete(edm,fault_idxs,0),fault_idxs,1)
157188
detection_statistic_detect, svd_u, svd_s, svd_v = _edm_detection_statistic(edm_detect)
158189
detection_statistic = detection_statistic_detect[0]
159190
except Exception as exception:
160191
if verbose:
192+
print("edm detect:",edm_detect)
161193
print(exception)
162-
print(exception)
163194
break
164195

196+
if verbose:
197+
print("detection statistic:",detection_statistic)
198+
165199
if detection_statistic < threshold:
166200
if verbose:
167201
print("below threshold")
@@ -290,9 +324,9 @@ def fde_edm(navdata, max_faults=None, threshold=1.0, verbose=False,
290324
detection_statistic = detection_statistic_exclude[min_idx]
291325

292326
# important step! remove 1 since index 0 is the receiver index
293-
fault_idxs = [i-1 for i in fault_idxs]
327+
fault_idxs = [pre_nan_idxs[i-1] for i in fault_idxs]
294328

295-
fault_edm_subset = np.array([0] * len(navdata_subset))
329+
fault_edm_subset = np.array([0] * nsl)
296330
fault_edm_subset[fault_idxs] = 1
297331
fault_edm_subset[nan_idxs] = 2
298332
fault_edm += list(fault_edm_subset)
@@ -470,8 +504,6 @@ def fde_residual_old(navdata, max_faults, threshold,
470504
if verbose:
471505
print("threshold fail:")
472506
print("r: ",r)
473-
for rri, rrr in enumerate(residuals):
474-
print(rri, rrr)
475507

476508
ri = list(ri)
477509

‎notebooks/Untitled.ipynb‎

Lines changed: 210 additions & 0 deletions
Large diffs are not rendered by default.

‎notebooks/Untitled1.ipynb‎

Lines changed: 160 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,160 @@
1+
{
2+
"cells": [
3+
{
4+
"cell_type": "code",
5+
"execution_count": 1,
6+
"id": "6bf21419",
7+
"metadata": {},
8+
"outputs": [],
9+
"source": [
10+
"import gnss_lib_py as glp"
11+
]
12+
},
13+
{
14+
"cell_type": "code",
15+
"execution_count": 2,
16+
"id": "dc046aaa",
17+
"metadata": {},
18+
"outputs": [],
19+
"source": [
20+
"def solution_method(navdata):\n",
21+
" \n",
22+
" navdata = glp.solve_fde(navdata, method=\"edm\", remove_outliers=True)\n",
23+
" \n",
24+
" state_estimate = glp.solve_wls(navdata)\n",
25+
" \n",
26+
" return state_estimate"
27+
]
28+
},
29+
{
30+
"cell_type": "code",
31+
"execution_count": 3,
32+
"id": "915d3b7a",
33+
"metadata": {},
34+
"outputs": [
35+
{
36+
"name": "stdout",
37+
"output_type": "stream",
38+
"text": [
39+
"solving: 2021-04-28-US-MTV-2 SamsungGalaxyS20Ultra\n"
40+
]
41+
},
42+
{
43+
"name": "stderr",
44+
"output_type": "stream",
45+
"text": [
46+
"/home/derek/gnss_lib_py/gnss_lib_py/algorithms/snapshot.py:302: RuntimeWarning: Newton Raphson did not converge.\n",
47+
" warnings.warn(\"Newton Raphson did not converge.\", RuntimeWarning)\n"
48+
]
49+
},
50+
{
51+
"name": "stdout",
52+
"output_type": "stream",
53+
"text": [
54+
"solving: 2021-06-22-US-MTV-1 XiaomiMi8\n"
55+
]
56+
},
57+
{
58+
"name": "stderr",
59+
"output_type": "stream",
60+
"text": [
61+
"/home/derek/gnss_lib_py/gnss_lib_py/algorithms/snapshot.py:302: RuntimeWarning: Newton Raphson did not converge.\n",
62+
" warnings.warn(\"Newton Raphson did not converge.\", RuntimeWarning)\n"
63+
]
64+
}
65+
],
66+
"source": [
67+
"submission = glp.solve_kaggle_dataset(folder_path = \"/home/derek/datasets/google-decimeter-2022/test_short/\",\n",
68+
" solver = solution_method,\n",
69+
" verbose = True)"
70+
]
71+
},
72+
{
73+
"cell_type": "code",
74+
"execution_count": 4,
75+
"id": "de4e4b97",
76+
"metadata": {},
77+
"outputs": [
78+
{
79+
"data": {
80+
"text/plain": [
81+
" tripId UnixTimeMillis \\\n",
82+
"0 2021-04-28-US-MTV-2/SamsungGalaxyS20Ultra 1619650832999 \n",
83+
"1 2021-04-28-US-MTV-2/SamsungGalaxyS20Ultra 1619650833999 \n",
84+
"2 2021-04-28-US-MTV-2/SamsungGalaxyS20Ultra 1619650834999 \n",
85+
"3 2021-04-28-US-MTV-2/SamsungGalaxyS20Ultra 1619650835999 \n",
86+
"4 2021-04-28-US-MTV-2/SamsungGalaxyS20Ultra 1619650836999 \n",
87+
"... ... ... \n",
88+
"3117 2021-06-22-US-MTV-1/XiaomiMi8 1624401972999 \n",
89+
"3118 2021-06-22-US-MTV-1/XiaomiMi8 1624401973999 \n",
90+
"3119 2021-06-22-US-MTV-1/XiaomiMi8 1624401974999 \n",
91+
"3120 2021-06-22-US-MTV-1/XiaomiMi8 1624401975999 \n",
92+
"3121 2021-06-22-US-MTV-1/XiaomiMi8 1624401976999 \n",
93+
"\n",
94+
" LatitudeDegrees LongitudeDegrees \n",
95+
"0 37.395828 -122.102977 \n",
96+
"1 37.395812 -122.102960 \n",
97+
"2 37.395813 -122.102971 \n",
98+
"3 37.395815 -122.102929 \n",
99+
"4 37.395840 -122.102959 \n",
100+
"... ... ... \n",
101+
"3117 37.460172 -122.137210 \n",
102+
"3118 37.460174 -122.137275 \n",
103+
"3119 37.460183 -122.137270 \n",
104+
"3120 37.460199 -122.137234 \n",
105+
"3121 37.460185 -122.137247 \n",
106+
"\n",
107+
"[4 rows x 3122 columns]"
108+
]
109+
},
110+
"execution_count": 4,
111+
"metadata": {},
112+
"output_type": "execute_result"
113+
}
114+
],
115+
"source": [
116+
"submission"
117+
]
118+
},
119+
{
120+
"cell_type": "code",
121+
"execution_count": 5,
122+
"id": "16edb094",
123+
"metadata": {},
124+
"outputs": [],
125+
"source": [
126+
"# after the submission NavData object is created, save it to a csv with:\n",
127+
"submission.to_csv(\"file_path.csv\")"
128+
]
129+
},
130+
{
131+
"cell_type": "code",
132+
"execution_count": null,
133+
"id": "9b262167",
134+
"metadata": {},
135+
"outputs": [],
136+
"source": []
137+
}
138+
],
139+
"metadata": {
140+
"kernelspec": {
141+
"display_name": "Python 3 (ipykernel)",
142+
"language": "python",
143+
"name": "python3"
144+
},
145+
"language_info": {
146+
"codemirror_mode": {
147+
"name": "ipython",
148+
"version": 3
149+
},
150+
"file_extension": ".py",
151+
"mimetype": "text/x-python",
152+
"name": "python",
153+
"nbconvert_exporter": "python",
154+
"pygments_lexer": "ipython3",
155+
"version": "3.8.9"
156+
}
157+
},
158+
"nbformat": 4,
159+
"nbformat_minor": 5
160+
}

0 commit comments

Comments
 (0)