Implmented UKF and some course adjusting

3 files changed, 63 insertions, 57 deletions

diff --git a/DWM.py b/DWM.py
index 42a89c0..ec2cea3 100755
--- a/DWM.py
+++ b/DWM.py
@@ -1,38 +1,36 @@
 import serial
 from time import sleep
 from filterpy.kalman import UnscentedKalmanFilter
+from filterpy.kalman import MerweScaledSigmaPoints
 import numpy as np
 import re
 from collections import deque
 
-def h(x):
+def fx(x, t):
     return x
 
-# x_mu = -0.02163601775523146
-# x_std = 0.07074315964054628
-# y_mu = 0.02645106742760512
-# y_std = 0.07415316805017082
+def hx(x):
+    return x
+
+x_mu = -0.02163601775523146
+x_std = 0.07074315964054628
+y_mu = 0.02645106742760512
+y_std = 0.07415316805017082
 
-# kf = UnscentedKalmanFilter(dim_x=2, dim_z=2, alpha=1.05)
+points = MerweScaledSigmaPoints(n=2, alpha=1, beta=2, kappa=0)
 
-# # Initial position
-# kf.x = np.array([[0.],
-#                  [0.]])
+kf = UnscentedKalmanFilter(dim_x=2, dim_z=2, dt=0.1, fx=fx, hx=hx, points=points)
 
-# # State transition matrix
-# kf.F = np.array([[1., 0.],
-#                  [0., 1.]])
+# Initial position
+kf.x = np.array([0., 0.])
 
-# # Measurement function
-# kf.H = np.array([[1., 0.],
-#                  [0., 1.]])
+# Initial error
+kf.P *= 1000
 
-# # Covariance matrix
-# kf.P = np.array([[x_std**2, 0.],
-#                  [0., y_std**2]])
+# Noise matrix
+kf.R = np.diag([25, 25])
 
-# kf.R = np.array([[x_std**2, 0.],
-#                  [0., y_std**2]])
+kf.Q = np.eye(2)
 
 position = deque([(0, 0)])
 
@@ -82,11 +80,10 @@ with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s:
     s.write(b"\r")
     sleep(0.1)
 
-    for i in range(10):
+    for i in range(30):
         s.readline()
 
     for i in range(50):
-        print(i)
         position.append((0, 0))
     
     while True:
@@ -97,12 +94,11 @@ with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s:
             _, x, y, z, qf = dstr.split(",")
             x = int(float(x) * 1000)
             y = int(float(y) * 1000)
-            # kf.predict()
-            # kf.update(np.array([[x],
-            #                     [y]]))
+            kf.predict()
+            kf.update(np.array([x, y]))
 
-            # x = int(kf.x[0] * 1000)
-            # y = int(kf.x[1] * 1000)
+            kfx = int(kf.x[0] * 1000)
+            kfy = int(kf.x[1] * 1000)
 
             position.popleft()
             position.append((x, y))
@@ -114,15 +110,16 @@ with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s:
                 xmean += p_i[0]
                 ymean += p_i[1]
 
-            xmean = xmean / 50
-            ymean = ymean / 50
+            xmean = int(xmean / 50)
+            ymean = int(ymean / 50)
 
             with open("position.txt", "w") as f:
-                print(f"{x},{y}")
+                print(f"{kfx},{kfy}")
                 f.write(f"{x},{y}\n")
                 f.close()
 
             with open("position_mean.txt", "w") as f:
+                #print(f"xmean: {xmean}, ymean: {ymean}")
                 f.write(f"{xmean},{ymean}\n")
                 f.close()
 
diff --git a/bt.py b/bt.py
index 98fc45e..a341f22 100755
--- a/bt.py
+++ b/bt.py
@@ -41,15 +41,15 @@ while read != "1000":
     # Send DWM data
     if read == "10":
         sleep(1)
-            with open("position_mean.txt", "r") as f:
-                for line in f:
-                    line = line.strip("\n")
-                    x, y = line.split(",")
-                    x = int(x)
-                    y = int(y)
-                    recv_sock.send(f"{x}, {y}, ".encode())
-                    # message = message + f"{x}, {y}, "
-                f.close()
+        with open("position_mean.txt", "r") as f:
+            for line in f:
+                line = line.strip("\n")
+                x, y = line.split(",")
+                x = int(x)
+                y = int(y)
+                recv_sock.send(f"{x}, {y}, ".encode())
+                # message = message + f"{x}, {y}, "
+            f.close()
     else:
         with open("position.txt", "r") as f:
             for line in f:
diff --git a/main.py b/main.py
index 08ddc78..00f7c22 100755
--- a/main.py
+++ b/main.py
@@ -37,8 +37,8 @@ print(bto)
 newCommand = False
 
 dwm = ""
-x = ""
-y = ""
+x = 0
+y = 0
 
 oldX = 0
 oldY = 0
@@ -47,6 +47,7 @@ backward = False
 
 curDir = 0
 desDir = 0
+normDir = 0
 
 if bto.find("Connected") != -1:
     GPIO.output(LED, True)
@@ -57,6 +58,8 @@ if bto.find("Connected") != -1:
     # LIDAR Process Code
     LIDARProcess = subprocess.Popen(["/home/pi/TNE107-RPI/LIDARProg", "--channel", "--serial", "/dev/ttyUSB0", "460800"], stdout=open(os.devnull, 'wb'))
     print(f"LIDAR PID: {LIDARProcess.pid}")
+
+    sleep(2)
     
     while bto != "1000":
         btReady, _, _ = select.select([BluetoothProcess.stdout], [], [], 0.0005)
@@ -76,28 +79,27 @@ if bto.find("Connected") != -1:
             y = int(y)
             #print("Quality factor: " + qf)
             # if float(qf) > 80:
-            print(f"x: {x}, y: {y}")
-            print(f"Current direction: {curDir}")
-            print(f"Desired direction: {desDir}")
 
         if newCommand:
             print(f"Command: {bto}")
             if bto == "11":
                 # Store current position => old pos
+                NANO.write(b"Forward\n")
                 if forward == False:
                     oldX = x
                     oldY = y
+                    sleep(2)
                 forward = True
                 backward = False
-                NANO.write(b"Forward\n")
             elif bto == "22":
+                # Store current position => old pos
+                NANO.write(b"Backward\n")
                 if backward == False:
                     oldX = x
                     oldY = y
+                    sleep(2)
                 forward = False
                 backward = True
-                # Store current position => old pos
-                NANO.write(b"Backward\n")
             elif bto == "33":
                 desDir = desDir - 90
                 if desDir < 0:
@@ -143,10 +145,14 @@ if bto.find("Connected") != -1:
                 desDir = 0
             elif bto == "0":
                 NANO.write(b"Stop\n")
+                forward = False
+                backward = False
             elif bto == "10":
                 NANO.write(b"Stop\n")
+                forward = False
+                backward = False
 
-        curDir = math.atan2(y - oldY, x - oldX) * (180/math.pi) + 180
+        curDir = 340# math.atan2(y - oldY, x - oldX) * (180/math.pi)
         if curDir < 0:
             curDir = curDir + 360
         if curDir >= 360:
@@ -155,27 +161,30 @@ if bto.find("Connected") != -1:
 
         normDir = (desDir - curDir) % 360
         if normDir > 180:
-            x -= 360
+            normDir -= 360
 
         if forward or backward:
-            if normDir < -10:
-                print("Adjusting to the right")
-                NANO.write(b"Right\n")
-                sleep(1/normDir)
-
             if normDir > 10:
                 print("Adjusting to the left")
                 NANO.write(b"Left\n")
-                sleep(1/normDir)
+                sleep(normDir / 1000)
+
+            if normDir < -10:
+                print("Adjusting to the right")
+                NANO.write(b"Right\n")
+                sleep(normDir / 1000)
         
 
         if forward:
-            NANO.write(b"Fowrad\n")
+            NANO.write(b"Forward\n")
 
         if backward:
             NANO.write(b"Backward\n")
-
         sleep(0.1)
+        print(f"current pos ({x}, {y}) -> old pos ({oldX}, {oldY})")
+        print(f"Current direction: {curDir}")
+        print(f"Normalized direction: {normDir}")
+        print(f"Desired direction: {desDir}")
         
         with open("angle.txt", "w") as f:
             f.write(f"{desDir}" + '\n')