updated comments, working on complimentary filter

controller_util.py

@@ -6,7 +6,7 @@
 import piparty
 import math
 import filterpy
-from filterpy.kalman import ExtendedKalmanFilter
+# from filterpy.kalman import ExtendedKalmanFilter
 #https://thepoorengineer.com/en/ekf-impl/
 #https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/11-Extended-Kalman-Filters.ipynb
 #link 3
@@ -18,6 +18,10 @@
 
 #another good link:
 #https://nitinjsanket.github.io/tutorials/attitudeest/kf
+#http://philsal.co.uk/projects/imu-attitude-estimation
+#http://www.pieter-jan.com/node/11
+
+#maybe we should try a complimentory filter as from above first!
 
 #we are trying to find the best process model of an IMU that contains linear acceleration
 #because that is the value we care most about and the one we want to track
@@ -35,14 +39,14 @@ def FormatVec(v, places=5):
     return ', '.join([ fmt.format(e) for e in v ])
 def VecLen(v):
     return math.sqrt(sum([ e*e for e in v ]))
-def Normalize(v):n
+def Normalize(v):
     m = VecLen(v)
     return tuple([ e / m for e in v ])
 
 async def Loop(plr):
     print("Acceleration   Jerk    Gyro")
     dt = 0.05
-    rk = ExtendedKalmanFilter(dim_x=12,dim_z=6)
+    # rk = ExtendedKalmanFilter(dim_x=12,dim_z=6)
     #initial starting values
     #we care about linear acceleration.
     #orientation, acceleration, velocity?
@@ -52,7 +56,7 @@ async def Loop(plr):
     #for the Joustmania game we don't care so much about position
 
     #there is 4 states each with 3 variables,.
-    rk.x = array([0,0,0,0,0,0,0,0,0,0,0,0])
+    # rk.x = array([0,0,0,0,0,0,0,0,0,0,0,0])
 
     #state transition matrix
     #again Link3
@@ -72,29 +76,42 @@ async def Loop(plr):
     #So the question is, how does orientation affect linear velocity??
 
 
-    rk.F = eye(3) + array([[0, 1, 0],
-                       [0, 0, 0],
-                       [0, 0, 0]]) * dt
-
-    range_std = 5. # meters
-    rk.R = np.diag([range_std**2])
-    rk.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1)
-    rk.Q[2,2] = 0.1
-    rk.P *= 50
-
-    xs, track = [], []
-    for i in range(int(20/dt)):
-        z = radar.get_range()
-        track.append((radar.pos, radar.vel, radar.alt))
+    #we need to estimate the direction of the controller first
+
+    #so for the new direction: complementary filter to get the angle the controller is at
+
+    #we need to then subtract the angle from the acceleration data to get the linear acceleration
+
+    #we are using many more frames, so this actually should be more performant too
+
+
+    # rk.F = eye(3) + array([[0, 1, 0],
+                       # [0, 0, 0],
+                       # [0, 0, 0]]) * dt
+
+    # range_std = 5. # meters
+    # rk.R = np.diag([range_std**2])
+    # rk.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1)
+    # rk.Q[2,2] = 0.1
+    # rk.P *= 50
+
+    # xs, track = [], []
+    # for i in range(int(20/dt)):
+        # z = radar.get_range()
+        # track.append((radar.pos, radar.vel, radar.alt))
 
-        rk.update(array([z]), HJacobian_at, hx)
-        xs.append(rk.x)
-        rk.predict()
-
-    xs = asarray(xs)
-    track = asarray(track)
-    time = np.arange(0, len(xs)*dt, dt)
-    ekf_internal.plot_radar(xs, track, time)
+        # rk.update(array([z]), HJacobian_at, hx)
+        # xs.append(rk.x)
+        # rk.predict()
+
+    # xs = asarray(xs)
+    # track = asarray(track)
+    # time = np.arange(0, len(xs)*dt, dt)
+    # ekf_internal.plot_radar(xs, track, time)
+
+
+    #we should first get the gyro and accelerometer data and plot it
+    #so we can see how noisy it is 
 
 
     while True:
@@ -114,9 +131,9 @@ async def Loop(plr):
 
 
 def Main():
-    piparty.Menu.enable_bt_scanning()
+    # piparty.Menu.enable_bt_scanning()
     move = psmove.PSMove(0)
-    move.enable_orientation(True)
+    # move.enable_orientation(True)
     p1 = player.Player(move)
     asyncio.get_event_loop().run_until_complete(Loop(p1))
 

player.py

@@ -115,6 +115,9 @@ def __init__(self, move):
         self.flush_events_()
 
     def flush_events_(self):
+        #why do we ever want to flush the events????
+        #it seems like this is wasted data?
+
         while self.move_.poll(): pass
 
     def get_events(self) -> typing.Iterator[ControllerEvent]: