Skip to content

Commit cb51564

Browse files
jbrodovskyjbrodovsky
committed
anomaly rbpf paper
1 parent 601a86f commit cb51564

214 files changed

Lines changed: 1412 additions & 1514 deletions

File tree

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎.gitignore‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,3 +61,53 @@ book/book/
6161
data/output/*.png
6262
pixi.lock
6363
Cargo.lock
64+
data/output/ukf/degraded/2023-08-04_21-47-58.csv
65+
data/output/ukf/degraded/2023-08-04_21-47-58.png
66+
data/output/ukf/degraded/2023-08-06_14-48-05.csv
67+
data/output/ukf/degraded/2023-08-06_14-48-05.png
68+
data/output/ukf/degraded/2023-08-09_12-47-42.csv
69+
data/output/ukf/degraded/2023-08-09_12-47-42.png
70+
data/output/ukf/degraded/2023-08-09_16-37-41.csv
71+
data/output/ukf/degraded/2023-08-09_16-37-41.png
72+
data/output/ukf/degraded/2024-06-20_16-55-50.csv
73+
data/output/ukf/degraded/2024-06-20_16-55-50.png
74+
data/output/ukf/degraded/2025-03-01_15-04-26.csv
75+
data/output/ukf/degraded/2025-03-01_15-04-26.png
76+
data/output/ukf/degraded/2025-03-01_16-46-39.csv
77+
data/output/ukf/degraded/2025-03-01_16-46-39.png
78+
data/output/ukf/degraded/2025-06-11_20-34-24.csv
79+
data/output/ukf/degraded/2025-06-11_20-34-24.png
80+
data/output/ukf/degraded/2025-06-14_21-17-02.csv
81+
data/output/ukf/degraded/2025-06-14_21-17-02.png
82+
data/output/ukf/degraded/2025-06-18_15-09-25.csv
83+
data/output/ukf/degraded/2025-06-18_15-09-25.png
84+
data/output/ukf/degraded/2025-06-18_16-52-32.csv
85+
data/output/ukf/degraded/2025-06-18_16-52-32.png
86+
data/output/ukf/degraded/2025-06-27_11-54-35.csv
87+
data/output/ukf/degraded/2025-06-27_11-54-35.png
88+
data/output/ukf/degraded/2025-07-04_17-24-46.csv
89+
data/output/ukf/degraded/2025-07-04_17-24-46.png
90+
data/output/ukf/degraded/2025-07-05_20-59-22.csv
91+
data/output/ukf/degraded/2025-07-05_20-59-22.png
92+
data/output/ukf/degraded/2025-07-08_14-12-53.csv
93+
data/output/ukf/degraded/2025-07-08_14-12-53.png
94+
data/output/ukf/degraded/2025-07-11_13-33-16.csv
95+
data/output/ukf/degraded/2025-07-11_13-33-16.png
96+
data/output/ukf/degraded/2025-07-18_23-13-43.csv
97+
data/output/ukf/degraded/2025-07-18_23-13-43.png
98+
data/output/ukf/degraded/2025-07-31_23-36-03.csv
99+
data/output/ukf/degraded/2025-07-31_23-36-03.png
100+
data/output/ukf/degraded/2025-08-03_18-15-59.csv
101+
data/output/ukf/degraded/2025-08-03_18-15-59.png
102+
data/output/ukf/degraded/2025-09-26_19-03-38.csv
103+
data/output/ukf/degraded/2025-09-26_19-03-38.png
104+
data/output/ukf/degraded/2025-09-27_12-54-35.csv
105+
data/output/ukf/degraded/2025-09-27_12-54-35.png
106+
data/output/ukf/degraded/2025-09-27_18-10-16.csv
107+
data/output/ukf/degraded/2025-09-27_18-10-16.png
108+
data/output/ukf/degraded/2025-09-28_20-23-16.csv
109+
data/output/ukf/degraded/2025-09-28_20-23-16.png
110+
data/output/ukf/degraded/2025-11-09_17-34-01.csv
111+
data/output/ukf/degraded/2025-11-09_17-34-01.png
112+
data/output/ukf/degraded/2025-11-16_16-08-14.csv
113+
data/output/ukf/degraded/2025-11-16_16-08-14.png

‎analysis/src/analysis/__init__.py‎

Lines changed: 156 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,5 @@
11
import os
22

3-
from numpy.char import index
4-
53
# Ensure non-interactive backend for matplotlib to avoid Tkinter GUI usage
64
os.environ.setdefault("MPLBACKEND", "Agg")
75

@@ -13,6 +11,13 @@
1311
from pandas import DataFrame, read_csv
1412
from tqdm import tqdm
1513

14+
from analysis.compare import (
15+
compute_error_statistics,
16+
compute_improvement_statistics,
17+
format_latex_table,
18+
print_summary_statistics,
19+
save_detailed_results_to_csv,
20+
)
1621
from analysis.plotting import plot_performance, plot_relative_performance
1722
from analysis.preprocess import preprocess_data
1823

@@ -110,6 +115,71 @@ def main() -> None:
110115
help="Output directory for the geophysical performance plots.",
111116
default="data/output",
112117
)
118+
geoperformance.add_argument(
119+
"-f",
120+
"--filter-name",
121+
type=str,
122+
default="geophysical",
123+
help="Name of the filter (e.g., rbpf, ukf, ekf) for labeling outputs.",
124+
)
125+
geoperformance.add_argument(
126+
"--geo-type",
127+
type=str,
128+
default="aided",
129+
help="Type of geophysical aiding (e.g., grav, mag, both) for labeling outputs.",
130+
)
131+
geoperformance.add_argument(
132+
"--no-latex",
133+
action="store_true",
134+
help="Disable LaTeX table generation.",
135+
)
136+
geoperformance.add_argument(
137+
"--no-plots",
138+
action="store_true",
139+
help="Disable plot generation (only produce CSV and LaTeX outputs).",
140+
)
141+
142+
# Compare filters command for cross-filter comparison
143+
compare_filters = command.add_parser(
144+
"compare-filters",
145+
help="Compare performance across different filter modalities (e.g., RBPF vs UKF vs EKF).",
146+
)
147+
compare_filters.add_argument(
148+
"-i",
149+
"--input-dirs",
150+
type=str,
151+
nargs="+",
152+
required=True,
153+
help="Input directories containing filter results to compare (one per filter).",
154+
)
155+
compare_filters.add_argument(
156+
"-l",
157+
"--labels",
158+
type=str,
159+
nargs="+",
160+
required=True,
161+
help="Labels for each filter (must match number of input directories).",
162+
)
163+
compare_filters.add_argument(
164+
"-r",
165+
"--reference",
166+
type=str,
167+
default="data/input",
168+
help="Directory containing GPS ground truth CSVs (default: data/input).",
169+
)
170+
compare_filters.add_argument(
171+
"-o",
172+
"--output",
173+
type=str,
174+
default="data/output/filter_comparison",
175+
help="Output directory for comparison results.",
176+
)
177+
compare_filters.add_argument(
178+
"--geo-type",
179+
type=str,
180+
default="comparison",
181+
help="Geophysical type label for outputs (e.g., grav, mag, both).",
182+
)
113183

114184
args = parser.parse_args()
115185

@@ -202,28 +272,48 @@ def performance_analysis(args):
202272

203273

204274
def geophysical_performance_analysis(args):
205-
"""Generate geophysical performance plots."""
275+
"""Generate geophysical performance analysis with plots, CSV summaries, and LaTeX tables.
276+
277+
This function processes geophysical-aided navigation results, comparing them against
278+
baseline degraded solutions and GPS ground truth. It produces:
279+
- Performance plots (PNG) showing error differences over time
280+
- CSV summary with error statistics per trajectory
281+
- Detailed CSV with per-trajectory statistics for geo, baseline, and differences
282+
- LaTeX table for publication
283+
- Console summary statistics
284+
"""
206285
input_dir = args.processed
207-
print(f"Generating geophysical performance plots from data in: {input_dir}")
286+
filter_name = args.filter_name
287+
geo_type = args.geo_type
288+
generate_plots = not args.no_plots
289+
generate_latex = not args.no_latex
290+
291+
print("=" * 80)
292+
print(f"Geophysical Performance Analysis: {filter_name.upper()} {geo_type}")
293+
print("=" * 80)
294+
print(f"Geophysical-aided data: {input_dir}")
208295

209296
datasets = list(Path(input_dir).glob("*.csv"))
210297
print(f"Found {len(datasets)} datasets to process.")
211298

212-
print(f"Comparing to reference data in: {args.reference}")
299+
print(f"Reference (truth) data: {args.reference}")
213300
references = list(Path(args.reference).glob("*.csv"))
214301
print(f"Found {len(references)} reference datasets.")
215302

216-
print(f"Comparing to degraded data in: {args.degraded}")
303+
print(f"Degraded baseline data: {args.degraded}")
217304
degradeds = list(Path(args.degraded).glob("*.csv"))
218305
print(f"Found {len(degradeds)} degraded datasets.")
219306

220307
output_path = Path(args.output)
221308
output_path.mkdir(parents=True, exist_ok=True)
222-
print(f"Saving geophysical performance plots to: {args.output}")
309+
print(f"Output directory: {args.output}")
310+
print(f"Generate plots: {generate_plots}")
311+
print(f"Generate LaTeX: {generate_latex}")
223312

224313
reference_path = Path(args.reference)
225314
degraded_path = Path(args.degraded)
226315

316+
# Original summary DataFrame (error differences)
227317
summary_df = DataFrame(
228318
columns=[
229319
"Min Horizontal Error (m)",
@@ -240,9 +330,12 @@ def geophysical_performance_analysis(args):
240330
"RMSE 3D Error (m)",
241331
], # ty:ignore[invalid-argument-type]
242332
index=[dataset.stem for dataset in datasets], # ty:ignore[invalid-argument-type]
243-
# index.name = "Dataset" # ty:ignore[unknown-argument]
244333
)
245334

335+
# For LaTeX table and detailed results
336+
latex_results = [] # List of (traj_name, improvement_stats)
337+
detailed_results = [] # List of (traj_name, geo_stats, baseline_stats, improvement_stats)
338+
246339
for dataset in tqdm(datasets):
247340
geo = read_csv(dataset, parse_dates=True, index_col=0)
248341
try:
@@ -260,53 +353,30 @@ def geophysical_performance_analysis(args):
260353

261354
output_plot = output_path / f"{dataset.stem}_geophysical_performance.png"
262355
nav = nav.iloc[1:].copy()
263-
# print(
264-
# f"Processing dataset {dataset} ({len(geo)}) with reference {reference_file.name} ({len(nav)}) and degraded {degraded_file.name} ({len(degraded_nav)})"
265-
# )
266356

267-
# Check to make sure all three datasets have the same length. If geo is shorter than add the first row of reference to geo to align.
268-
# Merge in via index to ensure proper alignment.
357+
# Align datasets by index
269358
if not (len(nav) == len(geo)):
270-
# print("Correcting geo to match reference nav.")
271-
# print(
272-
# f"Dataset length mismatch for {dataset.name}: geo({len(geo)}), nav({len(nav)}), degraded_nav({len(degraded_nav)}). Attempting to align."
273-
# )
274-
# Check if the first index of reference is in geo, if not add it.
275359
if nav.index[0] not in geo.index:
276360
first_row = geo.iloc[[0]][["latitude", "longitude", "altitude"]].copy()
277361
first_row.index = [nav.index[0]]
278362
geo.loc[first_row.index] = first_row
279363
geo = geo.sort_index()
280-
# Now reindex geo to match nav
281364
geo = geo.reindex(nav.index)
282-
# print(
283-
# f"After alignment, dataset lengths: geo({len(geo)}), nav({len(nav)}), degraded_nav({len(degraded_nav)})"
284-
# )
285-
# print(geo.head(10))
286-
# print(nav.head(10))
287-
# print(degraded_nav.head(10))
365+
288366
if not (len(nav) == len(degraded_nav)):
289-
# print("Correcting degraded_nav to match reference nav.")
290-
# print(
291-
# f"Dataset length mismatch for {dataset.name}: geo({len(geo)}), nav({len(nav)}), degraded_nav({len(degraded_nav)}). Attempting to align."
292-
# )
293-
# Check if the first index of reference is in degraded_nav, if not add it.
294367
if nav.index[0] not in degraded_nav.index:
295368
first_row = degraded_nav.iloc[[0]][["latitude", "longitude", "altitude"]].copy()
296369
first_row.index = [nav.index[0]]
297370
degraded_nav.loc[first_row.index] = first_row
298371
degraded_nav = degraded_nav.sort_index()
299-
# Now reindex degraded_nav to match nav
300372
degraded_nav = degraded_nav.reindex(nav.index)
301-
# print(
302-
# f"After alignment, dataset lengths: geo({len(geo)}), nav({len(nav)}), degraded_nav({len(degraded_nav)})"
303-
# )
304-
# print(geo.head(10))
305-
# print(nav.head(10))
306-
# print(degraded_nav.head(10))
307373

308374
try:
309-
plot_relative_performance(geo, degraded_nav, nav, output_plot)
375+
# Generate plot if enabled
376+
if generate_plots:
377+
plot_relative_performance(geo, degraded_nav, nav, output_plot)
378+
379+
# Compute haversine errors
310380
geo_error = haversine_vector(
311381
geo[["latitude", "longitude"]].to_numpy(dtype=np.float64, copy=False),
312382
nav[["latitude", "longitude"]].to_numpy(),
@@ -318,9 +388,21 @@ def geophysical_performance_analysis(args):
318388
nav[["latitude", "longitude"]].to_numpy(),
319389
Unit.METERS,
320390
)
391+
392+
# Compute statistics for detailed output
393+
geo_stats = compute_error_statistics(geo_error)
394+
baseline_stats = compute_error_statistics(deg_error)
395+
improvement_stats = compute_improvement_statistics(geo_stats, baseline_stats)
396+
397+
# Store for LaTeX and detailed CSV
398+
latex_results.append((dataset.stem, improvement_stats))
399+
detailed_results.append((dataset.stem, geo_stats, baseline_stats, improvement_stats))
400+
401+
# Original summary DataFrame calculations
321402
err_diff = geo_error - deg_error
322-
geo_rmse = np.sqrt(np.nanmean(geo_error**2))
323-
deg_rmse = np.sqrt(np.nanmean(deg_error**2))
403+
geo_rmse = geo_stats["rmse"]
404+
deg_rmse = baseline_stats["rmse"]
405+
324406
summary_df.loc[dataset.stem] = [
325407
np.nanmin(err_diff),
326408
np.nanmax(err_diff),
@@ -346,17 +428,47 @@ def geophysical_performance_analysis(args):
346428
geo_rmse - deg_rmse,
347429
]
348430
except Exception as e:
349-
print(
350-
f"Error plotting geophysical performance for {dataset.name}, possible dimension mismatch or missing data: {e}"
351-
)
431+
print(f"Error processing {dataset.name}, possible dimension mismatch or missing data: {e}")
352432
continue
433+
434+
# Add summary statistics to DataFrame
353435
summary_df.loc["median"] = summary_df.median()
354436
summary_df.loc["mean"] = summary_df.mean()
355437
summary_df.loc["std"] = summary_df.std()
356438

439+
# Save original summary CSV
357440
summary_file = output_path / "geophysical_performance_summary.csv"
358441
summary_df.to_csv(summary_file)
359-
print("Geophysical performance analysis completed.")
442+
print(f"\nSaved performance summary to {summary_file}")
443+
444+
# Save detailed results CSV
445+
if detailed_results:
446+
detailed_file = output_path / f"{filter_name}_{geo_type}_detailed_results.csv"
447+
save_detailed_results_to_csv(detailed_results, detailed_file)
448+
print(f"Saved detailed results to {detailed_file}")
449+
450+
# Generate and save LaTeX table
451+
if generate_latex and latex_results:
452+
table_title = (
453+
f"{filter_name.upper()} {geo_type.capitalize()}-Aided Performance "
454+
"vs Baseline (Geo - Baseline, negative = improvement)"
455+
)
456+
table_label = f"tab:{filter_name}_{geo_type}_results"
457+
latex_table = format_latex_table(latex_results, table_title, table_label)
458+
459+
tex_file = output_path / f"{filter_name}_{geo_type}_table.tex"
460+
with open(tex_file, "w") as f:
461+
f.write(latex_table)
462+
print(f"Saved LaTeX table to {tex_file}")
463+
464+
# Print summary statistics
465+
if latex_results:
466+
print("\n" + "=" * 80)
467+
print("SUMMARY STATISTICS")
468+
print("=" * 80)
469+
print_summary_statistics(latex_results, f"{filter_name.upper()} {geo_type}-aided")
470+
471+
print("\nGeophysical performance analysis completed.")
360472

361473

362474
if __name__ == "__main__":

0 commit comments

Comments
 (0)