diff options
Diffstat (limited to 'DWM.py')
| -rwxr-xr-x | DWM.py | 48 |
1 files changed, 30 insertions, 18 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") |