Skip to content

Commit e5a7ab7

Browse files
betaBisonbetaBison
committed
update residual method
1 parent 8c3f809 commit e5a7ab7

1 file changed

Lines changed: 113 additions & 81 deletions

File tree

‎gnss_lib_py/algorithms/fde.py‎

Lines changed: 113 additions & 81 deletions
Original file line numberDiff line numberDiff line change
@@ -367,8 +367,8 @@ def _edm_detection_statistic(edm):
367367

368368
return detection_statistic, svd_u, svd_s, svd_v
369369

370-
def fde_residual_old(navdata, receiver_state, max_faults, threshold,
371-
verbose=False):
370+
def fde_residual_old(navdata, max_faults, threshold,
371+
verbose=False, debug=False):
372372
"""Residual-based fault detection and exclusion.
373373
374374
Parameters
@@ -404,10 +404,14 @@ def fde_residual_old(navdata, receiver_state, max_faults, threshold,
404404
if threshold is None:
405405
threshold = 10
406406

407-
solve_residuals(navdata, receiver_state, inplace=True)
407+
408+
compute_times = []
408409

409410
for _, _, navdata_subset in navdata.loop_time('gps_millis'):
410411

412+
if debug:
413+
time_start = time.time()
414+
411415
subset_length = len(navdata_subset)
412416

413417
sv_m = navdata_subset[["x_sv_m","y_sv_m","z_sv_m"]]
@@ -423,9 +427,13 @@ def fde_residual_old(navdata, receiver_state, max_faults, threshold,
423427
navdata_subset.remove(cols=nan_idxs,inplace=True)
424428
original_indexes = np.delete(original_indexes, nan_idxs)
425429

426-
if len(navdata_subset) < 6:
427-
ri = []
428-
else:
430+
431+
# solve for residuals
432+
receiver_state = solve_wls(navdata_subset)
433+
solve_residuals(navdata_subset, receiver_state, inplace=True)
434+
# if len(navdata_subset) < 6:
435+
ri = []
436+
if len(navdata_subset) > 6:
429437
residuals = navdata_subset["residuals_m"].reshape(-1,1)
430438

431439
# test statistic
@@ -470,11 +478,18 @@ def fde_residual_old(navdata, receiver_state, max_faults, threshold,
470478
fault_residual_subset[nan_idxs] = 2
471479
fault_residual += list(fault_residual_subset)
472480

481+
if debug:
482+
time_end = time.time()
483+
compute_times.append(time_end-time_start)
473484

474485
navdata["fault_residual"] = fault_residual
475486
if verbose:
476487
print(navdata["fault_residual"])
477488

489+
debug_info = {}
490+
if debug:
491+
debug_info["compute_times"] = compute_times
492+
return navdata, debug_info
478493
return navdata
479494

480495
def fde_solution_separation_old(navdata, max_faults, threshold,
@@ -881,75 +896,87 @@ def evaluate_fde(navdata, method, fault_truth_row="fault_gt",
881896

882897
# compute position errors
883898

884-
x_rx_wls_all_m = []
885-
y_rx_wls_all_m = []
886-
z_rx_wls_all_m = []
887-
x_rx_wls_gt_m = []
888-
y_rx_wls_gt_m = []
889-
z_rx_wls_gt_m = []
890-
x_rx_wls_edm_m = []
891-
y_rx_wls_edm_m = []
892-
z_rx_wls_edm_m = []
899+
# x_rx_wls_all_m = []
900+
# y_rx_wls_all_m = []
901+
# z_rx_wls_all_m = []
902+
# x_rx_wls_gt_m = []
903+
# y_rx_wls_gt_m = []
904+
# z_rx_wls_gt_m = []
905+
# x_rx_wls_edm_m = []
906+
# y_rx_wls_edm_m = []
907+
# z_rx_wls_edm_m = []
908+
x_rx_wls_residual_m = []
909+
y_rx_wls_residual_m = []
910+
z_rx_wls_residual_m = []
893911
for _, _, navdata_subset in navdata.loop_time("gps_millis"):
894912

895913
# navdata subset length
896914
nsl = len(navdata_subset)
897915

898-
# using all measurements
899-
state_estimate = solve_wls(navdata_subset)
900-
x_rx_wls_all_m += [state_estimate["x_rx_wls_m"]] * nsl
901-
y_rx_wls_all_m += [state_estimate["y_rx_wls_m"]] * nsl
902-
z_rx_wls_all_m += [state_estimate["z_rx_wls_m"]] * nsl
903-
904-
# using gt non-faulty measurements
905-
state_estimate = solve_wls(navdata_subset.where("fault_gt",0))
906-
x_rx_wls_gt_m += [state_estimate["x_rx_wls_m"]] * nsl
907-
y_rx_wls_gt_m += [state_estimate["y_rx_wls_m"]] * nsl
908-
z_rx_wls_gt_m += [state_estimate["z_rx_wls_m"]] * nsl
916+
# # using all measurements
917+
# state_estimate = solve_wls(navdata_subset)
918+
# x_rx_wls_all_m += [state_estimate["x_rx_wls_m"]] * nsl
919+
# y_rx_wls_all_m += [state_estimate["y_rx_wls_m"]] * nsl
920+
# z_rx_wls_all_m += [state_estimate["z_rx_wls_m"]] * nsl
921+
#
922+
# # using gt non-faulty measurements
923+
# state_estimate = solve_wls(navdata_subset.where("fault_gt",0))
924+
# x_rx_wls_gt_m += [state_estimate["x_rx_wls_m"]] * nsl
925+
# y_rx_wls_gt_m += [state_estimate["y_rx_wls_m"]] * nsl
926+
# z_rx_wls_gt_m += [state_estimate["z_rx_wls_m"]] * nsl
927+
#
928+
# # using edm non-faulty measurements
929+
# state_estimate = solve_wls(navdata_subset.where("fault_edm",0))
930+
# x_rx_wls_edm_m += [state_estimate["x_rx_wls_m"]] * nsl
931+
# y_rx_wls_edm_m += [state_estimate["y_rx_wls_m"]] * nsl
932+
# z_rx_wls_edm_m += [state_estimate["z_rx_wls_m"]] * nsl
909933

910934
# using edm non-faulty measurements
911-
state_estimate = solve_wls(navdata_subset.where("fault_edm",0))
912-
x_rx_wls_edm_m += [state_estimate["x_rx_wls_m"]] * nsl
913-
y_rx_wls_edm_m += [state_estimate["y_rx_wls_m"]] * nsl
914-
z_rx_wls_edm_m += [state_estimate["z_rx_wls_m"]] * nsl
915-
916-
navdata["x_rx_wls_all_m"] = x_rx_wls_all_m
917-
navdata["y_rx_wls_all_m"] = y_rx_wls_all_m
918-
navdata["z_rx_wls_all_m"] = z_rx_wls_all_m
919-
navdata["x_rx_wls_gt_m"] = x_rx_wls_gt_m
920-
navdata["y_rx_wls_gt_m"] = y_rx_wls_gt_m
921-
navdata["z_rx_wls_gt_m"] = z_rx_wls_gt_m
922-
navdata["x_rx_wls_edm_m"] = x_rx_wls_edm_m
923-
navdata["y_rx_wls_edm_m"] = y_rx_wls_edm_m
924-
navdata["z_rx_wls_edm_m"] = z_rx_wls_edm_m
925-
926-
navdata["all_pos_error"] = np.linalg.norm(navdata[["x_rx_m",
927-
"y_rx_m",
928-
"z_rx_m"]] -
929-
navdata[["x_rx_wls_all_m",
930-
"y_rx_wls_all_m",
931-
"z_rx_wls_all_m"]],
932-
axis=0)
933-
navdata["all_pos_error"] = np.linalg.norm(navdata[["x_rx_m",
934-
"y_rx_m",
935-
"z_rx_m"]] -
936-
navdata[["x_rx_wls_all_m",
937-
"y_rx_wls_all_m",
938-
"z_rx_wls_all_m"]],
939-
axis=0)
940-
navdata["fault_gt_pos_error"] = np.linalg.norm(navdata[["x_rx_m",
941-
"y_rx_m",
942-
"z_rx_m"]] -
943-
navdata[["x_rx_wls_gt_m",
944-
"y_rx_wls_gt_m",
945-
"z_rx_wls_gt_m"]],
946-
axis=0)
947-
navdata["fault_edm_pos_error"] = np.linalg.norm(navdata[["x_rx_m",
935+
state_estimate = solve_wls(navdata_subset.where("fault_residual",0))
936+
x_rx_wls_residual_m += [state_estimate["x_rx_wls_m"]] * nsl
937+
y_rx_wls_residual_m += [state_estimate["y_rx_wls_m"]] * nsl
938+
z_rx_wls_residual_m += [state_estimate["z_rx_wls_m"]] * nsl
939+
940+
# navdata["x_rx_wls_all_m"] = x_rx_wls_all_m
941+
# navdata["y_rx_wls_all_m"] = y_rx_wls_all_m
942+
# navdata["z_rx_wls_all_m"] = z_rx_wls_all_m
943+
# navdata["x_rx_wls_gt_m"] = x_rx_wls_gt_m
944+
# navdata["y_rx_wls_gt_m"] = y_rx_wls_gt_m
945+
# navdata["z_rx_wls_gt_m"] = z_rx_wls_gt_m
946+
# navdata["x_rx_wls_edm_m"] = x_rx_wls_edm_m
947+
# navdata["y_rx_wls_edm_m"] = y_rx_wls_edm_m
948+
# navdata["z_rx_wls_edm_m"] = z_rx_wls_edm_m
949+
navdata["x_rx_wls_residual_m"] = x_rx_wls_residual_m
950+
navdata["y_rx_wls_residual_m"] = y_rx_wls_residual_m
951+
navdata["z_rx_wls_residual_m"] = z_rx_wls_residual_m
952+
953+
# navdata["all_pos_error"] = np.linalg.norm(navdata[["x_rx_m",
954+
# "y_rx_m",
955+
# "z_rx_m"]] -
956+
# navdata[["x_rx_wls_all_m",
957+
# "y_rx_wls_all_m",
958+
# "z_rx_wls_all_m"]],
959+
# axis=0)
960+
# navdata["fault_gt_pos_error"] = np.linalg.norm(navdata[["x_rx_m",
961+
# "y_rx_m",
962+
# "z_rx_m"]] -
963+
# navdata[["x_rx_wls_gt_m",
964+
# "y_rx_wls_gt_m",
965+
# "z_rx_wls_gt_m"]],
966+
# axis=0)
967+
# navdata["fault_edm_pos_error"] = np.linalg.norm(navdata[["x_rx_m",
968+
# "y_rx_m",
969+
# "z_rx_m"]] -
970+
# navdata[["x_rx_wls_edm_m",
971+
# "y_rx_wls_edm_m",
972+
# "z_rx_wls_edm_m"]],
973+
# axis=0)
974+
navdata["fault_residual_pos_error"] = np.linalg.norm(navdata[["x_rx_m",
948975
"y_rx_m",
949976
"z_rx_m"]] -
950-
navdata[["x_rx_wls_edm_m",
951-
"y_rx_wls_edm_m",
952-
"z_rx_wls_edm_m"]],
977+
navdata[["x_rx_wls_residual_m",
978+
"y_rx_wls_residual_m",
979+
"z_rx_wls_residual_m"]],
953980
axis=0)
954981

955982
metrics = {}
@@ -985,21 +1012,26 @@ def evaluate_fde(navdata, method, fault_truth_row="fault_gt",
9851012
metrics["timestep_mean_ms"] = timestep_mean*1000
9861013
metrics["timestep_median_ms"] = timestep_median*1000
9871014
metrics["timestep_max_ms"] = timestep_max*1000
988-
metrics["all_pos_error_min"] = np.min(navdata["all_pos_error"])
989-
metrics["all_pos_error_median"] = np.median(navdata["all_pos_error"])
990-
metrics["all_pos_error_mean"] = np.mean(navdata["all_pos_error"])
991-
metrics["all_pos_error_max"] = np.max(navdata["all_pos_error"])
992-
metrics["all_pos_error_std"] = np.std(navdata["all_pos_error"])
993-
metrics["fault_gt_pos_error_min"] = np.min(navdata["fault_gt_pos_error"])
994-
metrics["fault_gt_pos_error_median"] = np.median(navdata["fault_gt_pos_error"])
995-
metrics["fault_gt_pos_error_mean"] = np.mean(navdata["fault_gt_pos_error"])
996-
metrics["fault_gt_pos_error_max"] = np.max(navdata["fault_gt_pos_error"])
997-
metrics["fault_gt_pos_error_std"] = np.std(navdata["fault_gt_pos_error"])
998-
metrics["fault_edm_pos_error_min"] = np.min(navdata["fault_edm_pos_error"])
999-
metrics["fault_edm_pos_error_median"] = np.median(navdata["fault_edm_pos_error"])
1000-
metrics["fault_edm_pos_error_mean"] = np.mean(navdata["fault_edm_pos_error"])
1001-
metrics["fault_edm_pos_error_max"] = np.max(navdata["fault_edm_pos_error"])
1002-
metrics["fault_edm_pos_error_std"] = np.std(navdata["fault_edm_pos_error"])
1015+
# metrics["all_pos_error_min"] = np.min(navdata["all_pos_error"])
1016+
# metrics["all_pos_error_median"] = np.median(navdata["all_pos_error"])
1017+
# metrics["all_pos_error_mean"] = np.mean(navdata["all_pos_error"])
1018+
# metrics["all_pos_error_max"] = np.max(navdata["all_pos_error"])
1019+
# metrics["all_pos_error_std"] = np.std(navdata["all_pos_error"])
1020+
# metrics["fault_gt_pos_error_min"] = np.min(navdata["fault_gt_pos_error"])
1021+
# metrics["fault_gt_pos_error_median"] = np.median(navdata["fault_gt_pos_error"])
1022+
# metrics["fault_gt_pos_error_mean"] = np.mean(navdata["fault_gt_pos_error"])
1023+
# metrics["fault_gt_pos_error_max"] = np.max(navdata["fault_gt_pos_error"])
1024+
# metrics["fault_gt_pos_error_std"] = np.std(navdata["fault_gt_pos_error"])
1025+
# metrics["fault_edm_pos_error_min"] = np.min(navdata["fault_edm_pos_error"])
1026+
# metrics["fault_edm_pos_error_median"] = np.median(navdata["fault_edm_pos_error"])
1027+
# metrics["fault_edm_pos_error_mean"] = np.mean(navdata["fault_edm_pos_error"])
1028+
# metrics["fault_edm_pos_error_max"] = np.max(navdata["fault_edm_pos_error"])
1029+
# metrics["fault_edm_pos_error_std"] = np.std(navdata["fault_edm_pos_error"])
1030+
metrics["fault_residual_pos_error_min"] = np.min(navdata["fault_residual_pos_error"])
1031+
metrics["fault_residual_pos_error_median"] = np.median(navdata["fault_residual_pos_error"])
1032+
metrics["fault_residual_pos_error_mean"] = np.mean(navdata["fault_residual_pos_error"])
1033+
metrics["fault_residual_pos_error_max"] = np.max(navdata["fault_residual_pos_error"])
1034+
metrics["fault_residual_pos_error_std"] = np.std(navdata["fault_residual_pos_error"])
10031035

10041036
return metrics, navdata
10051037

0 commit comments

Comments
 (0)