Repository navigation
Expand file tree
/
Copy pathmultilateration.py
More file actions
96 lines (75 loc) · 3.16 KB
/
Copy pathmultilateration.py
File metadata and controls
96 lines (75 loc) · 3.16 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
import pandas as pd
import numpy as np
# ==========================================
# 1. DEFINE YOUR ANCHOR COORDINATES
# ==========================================
# x1, y1 -> physical Tag 2
x1, y1 = -45.0, 0.0
# x2, y2 -> physical Tag 3
x2, y2 = 0.0, 40.0
# x3, y3 -> physical Tag 5
x3, y3 = 45.0, 0.0
# x4, y4 -> physical Tag 7
x4, y4 = 0.0, 0.0
# ==========================================
# 2. CONSTRUCT MATRIX A & PSEUDO-INVERSE
# ==========================================
A = np.array([
[x2 - x1, y2 - y1],
[x3 - x1, y3 - y1],
[x4 - x1, y4 - y1]
])
A_T = A.T
A_pseudo_inv = np.linalg.inv(A_T @ A) @ A_T
def calculate_position(row):
"""
Calculates the target x, y position for a single row of distances.
"""
# ==========================================
# 3. PULL DISTANCES USING THE CORRECT TAG HEADERS
# ==========================================
d1 = pd.to_numeric(row['Tag 2 Measured Dist (cm)'], errors='coerce')
d2 = pd.to_numeric(row['Tag 3 Measured Dist (cm)'], errors='coerce')
d3 = pd.to_numeric(row['Tag 5 Measured Dist (cm)'], errors='coerce')
d4 = pd.to_numeric(row['Tag 7 Measured Dist (cm)'], errors='coerce')
# Skip rows where distance data is missing (blank rows)
if pd.isna(d1) or pd.isna(d2) or pd.isna(d3) or pd.isna(d4):
return pd.Series({'Measured x (cm)': np.nan, 'Measured y (cm)': np.nan})
# Construct Vector b
b = 0.5 * np.array([
d1 ** 2 - d2 ** 2 + (x2 ** 2 + y2 ** 2) - (x1 ** 2 + y1 ** 2),
d1 ** 2 - d3 ** 2 + (x3 ** 2 + y3 ** 2) - (x1 ** 2 + y1 ** 2),
d1 ** 2 - d4 ** 2 + (x4 ** 2 + y4 ** 2) - (x1 ** 2 + y1 ** 2)
])
# Calculate position vector x = [x_t, y_t]
pos = A_pseudo_inv @ b
# Round the results to 2 decimal places before returning
return pd.Series({'Measured x (cm)': round(pos[0], 2), 'Measured y (cm)': round(pos[1], 2)})
def main():
# Load the dataset
input_filename = 'uwb_aerodesign_test.csv'
print(f"Loading data from {input_filename}...")
df = pd.read_csv(input_filename)
# Clean up Excel headers
df.columns = df.columns.str.strip()
# Apply the multilateration function row by row
print("Calculating coordinates...")
results = df.apply(calculate_position, axis=1)
# Force the results specifically into the exact existing columns
df['Measured x (cm)'] = results['Measured x (cm)']
df['Measured y (cm)'] = results['Measured y (cm)']
# Save the results
output_filename = 'uwb_aerodesign_processed.csv'
try:
df.to_csv(output_filename, index=False)
print("==================================================")
print(f"Success! Data has been written directly to the columns.")
print(f"Open '{output_filename}' to see your filled spreadsheet!")
print("==================================================")
except PermissionError:
print("\n==================================================")
print(f"🛑 ERROR: Could not save to '{output_filename}'.")
print("Please CLOSE the file in Excel and run this script again!")
print("==================================================")
if __name__ == "__main__":
main()