diff --git a/models/husky_uwb.urdf.xacro b/models/husky_uwb.urdf.xacro index 00f9030..e67e889 100644 --- a/models/husky_uwb.urdf.xacro +++ b/models/husky_uwb.urdf.xacro @@ -177,12 +177,12 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI 100.0 base_link imu/data - 0.1 0.1 0.1 + 0.1 0.07 0.12 0.06 0.06 0.06 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.000 - 0.000 + 0.0 0.0 0.1 + 0.00 + 0.00 diff --git a/rqt_multiplot.xml b/rqt_multiplot.xml index 19057dd..b06d0b0 100644 --- a/rqt_multiplot.xml +++ b/rqt_multiplot.xml @@ -11,13 +11,13 @@ - Untitled Axis - 0 + x(m) + 1 true - Untitled Axis - 0 + y(m) + 1 true @@ -181,19 +181,19 @@ true 30 - Untitled Plot + Plan view - Untitled Axis - 0 + Time(s) + 1 true - Untitled Axis - 0 + bias(m/s^2) + 1 true @@ -250,16 +250,269 @@ 100 bias x + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /filter_bias + geometry_msgs/Vector3 + + + x + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /filter_bias + geometry_msgs/Vector3 + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Estimated bias x + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /filter_bias + geometry_msgs/Vector3 + + + y + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /filter_bias + geometry_msgs/Vector3 + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Estimated bias y + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /imu/data/bias + sensor_msgs/Imu + + + linear_acceleration/y + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /imu/data/bias + sensor_msgs/Imu + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + bias y + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /imu/data/bias + sensor_msgs/Imu + + + linear_acceleration/z + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /imu/data/bias + sensor_msgs/Imu + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Estimated bias z + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /filter_bias + geometry_msgs/Vector3 + + + z + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /filter_bias + geometry_msgs/Vector3 + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + bias z + true 30 - Untitled Plot + Bias estimation - - - + @@ -278,7 +531,7 @@ - pose/5/position/x + 1 1000 @@ -287,11 +540,11 @@ -1000 0 - /gazebo/link_states - gazebo_msgs/LinkStates + /gazebo/model_states + gazebo_msgs/ModelStates - data + twist/5/linear/x 0 1000 @@ -300,8 +553,8 @@ -1000 0 - /filter_error - std_msgs/Float32 + /gazebo/model_states + gazebo_msgs/ModelStates @@ -324,16 +577,472 @@ 0 100 - Error + Vx - - - true - + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /filter_position + nav_msgs/Odometry + + + twist/twist/linear/x + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /filter_position + nav_msgs/Odometry + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Estimated Vx + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /gazebo/model_states + gazebo_msgs/ModelStates + + + twist/5/linear/y + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /gazebo/model_states + gazebo_msgs/ModelStates + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Vy + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /filter_position + nav_msgs/Odometry + + + twist/twist/linear/y + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /filter_position + nav_msgs/Odometry + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Estimated Vy + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /uwb_position + nav_msgs/Odometry + + + twist/twist/linear/x + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /uwb_position + nav_msgs/Odometry + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Vx UWB + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /uwb_position + nav_msgs/Odometry + + + twist/twist/linear/y + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /uwb_position + nav_msgs/Odometry + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Vy UWB + + + + true + 30 - Untitled Plot + Velocity + + + + + + + + Time(s) + 1 + true + + + error(m) + 1 + true + + + + + + + + pose/5/position/x + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /gazebo/link_states + gazebo_msgs/LinkStates + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /filter_error + std_msgs/Float32 + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Error + + + + true + + 30 + Error norm + + + + Time(s) + 1 + true + + + heading(rad) + 1 + true + + + + + + + + data + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /filter_heading + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /filter_heading + std_msgs/Float32 + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Estimated + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /true_heading + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /true_heading + std_msgs/Float32 + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + True + + + + true + + 30 + Heading + + @@ -353,7 +1062,7 @@ 30 Untitled Plot - + false diff --git a/src/node_sage-husa.py b/src/node_sage-husa.py index 301e5fb..ee83fed 100755 --- a/src/node_sage-husa.py +++ b/src/node_sage-husa.py @@ -4,10 +4,12 @@ import rospy +import quaternion import numpy as np from sensor_msgs.msg import Imu from nav_msgs.msg import Odometry from gazebo_msgs.msg import LinkStates +from geometry_msgs.msg import Vector3 from gtec_msgs.msg import Ranging from std_msgs.msg import Int8 from std_msgs.msg import Float32 @@ -15,7 +17,10 @@ pub_filter = rospy.Publisher('filter_position', Odometry, queue_size=1) pub_uwb = rospy.Publisher('uwb_position', Odometry, queue_size=1) +pub_heading = rospy.Publisher('filter_heading', Float32 , queue_size=1) +pub_heading_true = rospy.Publisher('true_heading', Float32 , queue_size=1) pub_error = rospy.Publisher('filter_error', Float32 , queue_size=1) +pub_bias = rospy.Publisher('filter_bias', Vector3 , queue_size=1) init = 1 count = 0 @@ -46,15 +51,24 @@ def imu_callback(data): odom.twist.twist.angular.z = 0 pub_filter.publish(odom) + bias = Vector3() + bias.x = filter.x_[6,0] + bias.y = filter.x_[7,0] + bias.z = filter.x_[8,0] + pub_bias.publish(bias) + + heading = quaternion.as_euler_angles(quaternion.from_float_array(q)) + pub_heading.publish(heading[2]) + x_uwb = filter.posOld vel_uwb = filter.velOld odom_uwb = Odometry() odom_uwb.pose.pose.position.x = x_uwb[0] odom_uwb.pose.pose.position.y = x_uwb[1] - odom_uwb.pose.pose.position.z = 0 + odom_uwb.pose.pose.position.z = x_uwb[2] odom_uwb.twist.twist.linear.x = vel_uwb[0] odom_uwb.twist.twist.linear.y = vel_uwb[1] - odom_uwb.twist.twist.linear.z = 0 + odom_uwb.twist.twist.linear.z = vel_uwb[2] odom_uwb.twist.twist.angular.x = 0 odom_uwb.twist.twist.angular.y = 0 odom_uwb.twist.twist.angular.z = 0 @@ -75,8 +89,8 @@ def uwb_callback(data): def activation_callback(data): global filter filter.mode = data.data - if data.data == 3: - filter.x_[6:9, :] = 0 + # if data.data == 3: + # filter.x_[6:9, :] = 0 def model_callback(data): @@ -97,7 +111,7 @@ def model_callback(data): q[1] = 0 q[2] = 0 q[3] = data.pose[5].orientation.z - filter = shfaf(R=None, Q=None, P=None, x=x, q=q, window_width=10, a=0.9784) + filter = shfaf(R=None, Q=None, P=None, x=x, q=q, window_width=100, a=0.9784) filter.nAnchors = 4 filter.anchorPos = np.array([[-1, -1], [9, -1], @@ -109,20 +123,20 @@ def model_callback(data): rospy.loginfo('q: ') rospy.loginfo(filter.q) init = 0 - filter.q[0] = data.pose[5].orientation.w - filter.q[1] = 0 - filter.q[2] = 0 - filter.q[3] = data.pose[5].orientation.z + true_q = np.array([data.pose[5].orientation.w, 0, 0, data.pose[5].orientation.z]) + heading_true = quaternion.as_euler_angles(quaternion.from_float_array(true_q)) + pub_heading_true.publish(heading_true[2]) if count % 200 == 0 and init==0: out = np.greater(np.random.rand(), 0.95).astype(int) pos_outlier = out * (np.random.rand(3)-0.5)*4 pos_true[0] = data.pose[5].position.x pos_true[1] = data.pose[5].position.y pos_true[2] = data.pose[5].position.z - filter.posOld = pos_true + np.random.normal(0,0.1,3) + pos_outlier - filter.velOld[0] = data.twist[5].linear.x + np.random.normal(0.0, 0.1) + pos_outlier[0]*5 - filter.velOld[1] = data.twist[5].linear.y + np.random.normal(0.0, 0.1) + pos_outlier[1]*5 - filter.velOld[2] = data.twist[5].linear.z + np.random.normal(0.0, 0.1) + pos_outlier[2]*5 + uwb_noise = np.random.normal(0,0.1,3) + filter.posOld = pos_true + uwb_noise + pos_outlier + filter.velOld[0] = data.twist[5].linear.x + np.random.normal(0.0, 0.1) + uwb_noise[0]*5 + pos_outlier[0]*5 + filter.velOld[1] = data.twist[5].linear.y + np.random.normal(0.0, 0.1) + uwb_noise[1]*5 + pos_outlier[1]*5 + filter.velOld[2] = data.twist[5].linear.z + np.random.normal(0.0, 0.1) + uwb_noise[2]*5 + pos_outlier[2]*5 error = np.linalg.norm(pos_true - filter.x_[:3,0]) pub_error.publish(error) if filter.mode != 3: diff --git a/src/shfaf.py b/src/shfaf.py index 52f5f5b..9d714e2 100755 --- a/src/shfaf.py +++ b/src/shfaf.py @@ -22,11 +22,11 @@ class shfaf(object): def __init__(self, R=None, Q=None, P=None, x=None, q=None, window_width=12, a=0.95): if R is None: - R = np.diag([1,1,1,1*5,1*5,1*5]) * 1e-1 + R = np.diag([1, 1, 1, 1 * 5, 1 * 5, 1 * 5]) * 1e-1 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]) + 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.01,0.01,0.01,0,0,0]) + P = np.diag([0.1, 0.1, 0.1, 0.05, 0.05, 0.05, 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 @@ -62,7 +62,7 @@ def __init__(self, R=None, Q=None, P=None, x=None, q=None, window_width=12, a=0. self.pred = 0 # flag which states if prediction or correction has been performed last beta = 0 self.G = 1 - np.square(beta) - self.H_uwb = np.square(1-beta) + self.H_uwb = np.square(1 - beta) self.R = R self.Q = Q @@ -78,8 +78,8 @@ def __init__(self, R=None, Q=None, P=None, x=None, q=None, window_width=12, a=0. np.newaxis].T # position, velocity, orientation, acceleration bias, omega bias self.dx_ = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], np.float)[ np.newaxis].T # position, velocity, orientation, acceleration bias, omega bias - self.innovation = np.random.normal(0.0,0.1,(6, window_width)) - self.error = (np.random.rand(15, window_width)-1)*0.1 + self.innovation = np.random.normal(0.0, 0.1, (6, window_width)) + self.error = (np.random.rand(15, window_width) - 1) * 0.1 # self.innovation_ = np.zeros((6,window_width), np.float) self.k = 0 # Iteration counter self.lambda_ = 1.0119 # Sage Huza ICW coefficent @@ -90,13 +90,13 @@ def __init__(self, R=None, Q=None, P=None, x=None, q=None, window_width=12, a=0. # Fuzzy inference - input = ctrl.Antecedent(np.arange(0,0.9,0.1), 'r') - output = ctrl.Consequent(np.arange(0.8,2.1,0.1), 's') + input = ctrl.Antecedent(np.arange(0, 0.9, 0.1), 'r') + output = ctrl.Consequent(np.arange(0.8, 2.1, 0.1), 's') # Define membership functions - input['less'] = fuzz.trimf(input.universe, [0,0,0.3]) - input['equal'] = fuzz.trimf(input.universe,[0.1,0.4,0.7]) - input['more'] = fuzz.trimf(input.universe,[0.5,0.8,0.8]) + input['less'] = fuzz.trimf(input.universe, [0, 0, 0.3]) + input['equal'] = fuzz.trimf(input.universe, [0.1, 0.4, 0.7]) + input['more'] = fuzz.trimf(input.universe, [0.5, 0.8, 0.8]) output['less'] = fuzz.trimf(output.universe, [0.8, 0.8, 1.2]) output['equal'] = fuzz.trimf(output.universe, [1, 1.4, 1.8]) @@ -119,7 +119,7 @@ def prediction(self, am, wm, t): am = am[np.newaxis].T wm = wm[np.newaxis].T - wm[:2,:] = 0 + wm[:2, :] = 0 if self.uwbInit: dt = t - self.time else: @@ -134,8 +134,8 @@ def prediction(self, am, wm, t): P = self.P q = self.q C = self._rotationMatrix(q) - self.Q = sp.linalg.block_diag(np.eye(3)*((0.06*dt)**2), np.eye(3)*((0.0*dt)**2), - np.eye(3)*((0.001*dt)**2), np.eye(3)*((0.0*dt)**2)) + self.Q = sp.linalg.block_diag(np.eye(3) * ((0.06 * dt) ** 2), np.eye(3) * ((0.0 * dt) ** 2), + np.eye(3) * ((0.001 * dt) ** 2), np.eye(3) * ((0.0 * dt) ** 2)) Q = self.Q w = wm * dt g = np.array([0, 0, 9.8])[np.newaxis].T @@ -147,20 +147,21 @@ def prediction(self, am, wm, t): if np.linalg.norm(w) != 0: qq = quaternion.as_quat_array(q) - wq = quaternion.from_rotation_vector(w[:,0] * np.array((0,0,1))) + wq = quaternion.from_rotation_vector(w[:, 0] * np.array((0, 0, 1))) q_next = quaternion.as_float_array(qq * wq) else: - q_next = q + q_next = q F = np.block([[np.identity(3, np.float), np.identity(3, np.float) * dt, np.zeros((3, 9))], [np.zeros((3, 3), np.float), np.identity(3, np.float), -(C.dot(skew(am - x[6:9, :]))) * dt, -C * dt, np.zeros((3, 3), np.float)], - [np.zeros((3, 6), np.float), self._rotationMatrix(quaternion.as_float_array(quaternion.from_euler_angles(((wm - x[9:12, :])*dt)[:,0]))), np.zeros((3, 3), np.float), + [np.zeros((3, 6), np.float), self._rotationMatrix( + quaternion.as_float_array(quaternion.from_euler_angles(((wm - x[9:12, :]) * dt)[:, 0]))), + np.zeros((3, 3), np.float), -np.identity(3, np.float) * dt], [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))) self.dx_ = F.dot(self.dx) - # self.innovation_ = push(self.innovation_, d) self.k = self.k + 1 self.x_ = x_next self.q_ = q_next @@ -197,7 +198,7 @@ def correction(self): G = S + H.dot(self.dx_.dot(self.dx_.T.dot(H.T))) m = (np.abs(np.diagonal(G) / np.diagonal(D)))[np.newaxis].T check = np.greater(m, self.threshold).astype(int)[np.newaxis].T - z = (z * (check * (1 / np.sqrt(m)-1) + 1)).reshape(-1, 1) + z = (z * (check * (1 / np.sqrt(m) - 1) + 1)).reshape(-1, 1) self.innovation = ep if self.mode == 0: @@ -213,7 +214,7 @@ def correction(self): # Estimate measurement noise covariance self.R = (1 - np.power(s, self.alpha) * d) * self.R + np.power(s, self.alpha) * d * ( - S - H.dot(self.P_.dot(H.T))) + S - H.dot(self.P_.dot(H.T))) # Calculate Kalman gain self.K = self.P_.dot(H.T.dot(np.linalg.inv(H.dot(self.P_.dot(H.T)) + self.R))) @@ -226,6 +227,9 @@ def correction(self): # Update the nominal state dx_pos = np.concatenate((self.dx[:6], self.dx[9:])) self.x = self.x_ + dx_pos + dq = quaternion.from_euler_angles(np.concatenate((np.zeros(2), self.dx[8]))) + self.q = quaternion.as_float_array(quaternion.from_float_array(self.q_) * dq) + # Reset the error state self.dx = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], np.float)[np.newaxis].T @@ -242,8 +246,8 @@ def uwbRange2Pos(self): b = np.square(r[1:]) - np.square(r[0]) + np.square(anc[0, 0]) - np.square(anc[1:, 0]) + np.square( anc[0, 1]) - np.square(anc[1:, 1]) # + np.square(anc[0,2]) - np.square(anc[1:,2]) pos_ = 0.5 * np.linalg.inv(G.T.dot(G)).dot(G.T.dot(b)) - pos = self.posOld + self.velOld * dt + self.G * (pos_ - ( self.posOld + self.velOld * dt)) - vel = self.velOld + self.H_uwb/dt * (pos_ - (self.posOld + self.velOld*dt)) + pos = self.posOld + self.velOld * dt + self.G * (pos_ - (self.posOld + self.velOld * dt)) + vel = self.velOld + self.H_uwb / dt * (pos_ - (self.posOld + self.velOld * dt)) self.uwbTime = t self.posOld = pos self.velOld = vel diff --git a/src/shfaf.pyc b/src/shfaf.pyc index 2946795..968e706 100644 Binary files a/src/shfaf.pyc and b/src/shfaf.pyc differ