Fixed bias reset when switching filters

reset bias when switching to inertial navigation

src/node_sage-husa.py

@@ -73,11 +73,10 @@ def uwb_callback(data):
             filter.uwbCeck = np.zeros(4)
 
 def activation_callback(data):
+    global filter
     filter.mode = data.data
     if data.data == 3:
-        self.x[6:9, :] = 0
-    else:
-        self.x[6:9, :] = 0.1
+        filter.x_[6:9, :] = 0
 
 
 def model_callback(data):
@@ -93,7 +92,7 @@ def model_callback(data):
         x[1] = data.pose[5].position.y
         x[3] = data.twist[5].linear.x
         x[4] = data.twist[5].linear.y
-        x[6:9] = 0.095
+        x[6:9] = 0.0
         q[0] = data.pose[5].orientation.w
         q[1] = 0
         q[2] = 0

src/shfaf.py

@@ -26,7 +26,7 @@ def __init__(self, R=None, Q=None, P=None, x=None, q=None, window_width=12, a=0.
         if Q is None:
             Q = np.diag([0.06**2,0.06**2,0.06**2,0,0,0,0.1,0.1,0.1,0,0,0])
         if P is None:
-            P = np.diag([0.05,0.05,0.05,0,0,0,0,0,0,0.1,0.1,0.1,0,0,0])
+            P = np.diag([0.05,0.05,0.05,0,0,0,0,0,0,0.01,0.01,0.01,0,0,0])
         if x is None:
             x = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
                          np.float)  # position, velocity, acceleration bias, omega bias
@@ -159,7 +159,6 @@ def prediction(self, am, wm, t):
                       [np.zeros((3, 9), np.float), np.identity(3, np.float), np.zeros((3, 3), np.float)],
                       [np.zeros((3, 12), np.float), np.identity(3, np.float)]])
         self.P_ = (F.dot(P.dot(F.T)) + self.lamb.dot(Q.dot(self.lamb.T)))
-        rospy.loginfo(x[6:9, :])
         self.dx_ = F.dot(self.dx)
         # self.innovation_ = push(self.innovation_, d)
         self.k = self.k + 1