From 7b75f8477e6591c5897e2c2d7fa21e69a6a2fc15 Mon Sep 17 00:00:00 2001 From: Edvin Date: Tue, 6 May 2025 11:07:59 +0200 Subject: Fully implemented Kalman filter, removed data gathering program --- DWM.py | 48 ++++++++++++++++++++++++++++++------------------ 1 file changed, 30 insertions(+), 18 deletions(-) (limited to 'DWM.py') diff --git a/DWM.py b/DWM.py index 9c42303..6db4582 100755 --- a/DWM.py +++ b/DWM.py @@ -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") -- cgit v1.2.3