diff options
| -rwxr-xr-x | DWM.py | 48 | ||||
| -rw-r--r-- | gather_pos_data.py | 63 |
2 files changed, 30 insertions, 81 deletions
@@ -1,6 +1,8 @@ import serial from time import sleep from filterpy.kalman import KalmanFilter +import numpy as np +import re x_mu = -0.02163601775523146 x_std = 0.07074315964054628 @@ -10,25 +12,26 @@ y_std = 0.07415316805017082 kf = KalmanFilter(dim_x=2, dim_z=2) # Initial position -kf.x = np.array([0.], - [0.]) +kf.x = np.array([[0.], + [0.]]) # State transition matrix -kf.F = np.array([1., 0.], - [0., 1.]) +kf.F = np.array([[1., 0.], + [0., 1.]]) # Measurement function -kf.H = np.array([1., 0.], - [0., 1.]) +kf.H = np.array([[1., 0.], + [0., 1.]]) # Covariance matrix -kf.P = np.array([x_std**2, 0.], - [0., y_std**2]) +kf.P = np.array([[x_std**2, 0.], + [0., y_std**2]]) -kf.R = np.array([x_mu, 0.], - [0., y_mu]) +kf.R = np.array([[x_mu, 0.], + [0., y_mu]]) + +updRate = "1 1" # Active Idle -updRate = "10 10" # Active Idle with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s: @@ -73,15 +76,24 @@ with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s: sleep(0.1) s.write(b"\r") sleep(0.1) - + while True: - str = s.readline().decode('utf-8').strip('\n') - - if "POS," in str: - str = str.replace("POS,", "") - print(str) + dstr = s.readline().decode('utf-8').strip('\n') + + if "POS," in dstr: + dstr = re.sub('[^0-9,.]', '', dstr) + _, x, y, z, qf = dstr.split(",") + x = float(x) + y = float(y) + kf.predict() + kf.update(np.array([[x], + [y]])) + + x = int(kf.x[0] * 1000) + y = int(kf.x[1] * 1000) + print(f"x: {x}, y: {y}") with open("position.txt", "w") as f: - f.write(str + '\n') + f.write(f"x,y\n") f.close() print("Shutting down serial communication") diff --git a/gather_pos_data.py b/gather_pos_data.py deleted file mode 100644 index d7aa467..0000000 --- a/gather_pos_data.py +++ /dev/null @@ -1,63 +0,0 @@ -import serial -from time import sleep - -# Gathering data like this we should see that measurements have a normal distribution with -# quality factor weighing results further from the ground truth - -updRate = "1 1" # Active Idle -with open("position.txt", "w") as f: - with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s: - - # print(f"Opened serial port {s.name}") - - sleep(1) - s.write(b"\r") - sleep(0.1) - s.write(b"\r") - - s.write(b'nis ') - sleep(0.1) - s.write(b'0x1234') - sleep(0.1) - s.write(b'\r') - # print(f"Set PAN ID to 0x1234") - - sleep(1) - - s.write(b'nmt') - sleep(0.1) - s.write(b'\r') - # print("Configured node as tag") - - sleep(1) - - s.write(b"\r") - sleep(0.1) - s.write(b"\r") - - sleep(2) - - s.write(b"aurs ") - sleep(0.1) - s.write(updRate.encode()) - sleep(0.1) - s.write(b'\r') - # print("Set update rate to " + updRate) - - sleep(0.1) - s.write(b"lep") - sleep(0.1) - s.write(b"\r") - sleep(0.1) - - while True: - str = s.readline().decode('utf-8').strip('\n') - - if "POS," in str: - str = str.replace("POS,", "") - print(str) - - f.write(str + '\n') - f.close() - -print("Shutting down serial communication") |