Fully implemented Kalman filter, removed data gathering program

2 files changed, 30 insertions, 81 deletions

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")
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")