class SLAM_Localization(LandmarkLocalization, EKF_SLAM): alpha = 3.0 # factor in estimating covariance. def __init__(self, nodeName="slam_ekf"): super(SLAM_Localization, self).__init__(nodeName) self.icp = SubICP() self.extraction = Extraction() self.isFirstScan = True self.laser_count = 0 # interval self.laser_interval = 5 # State Vector [x y yaw].T, column vector. # self.xOdom = np.zeros((STATE_SIZE,1)) self.xEst = np.zeros((STATE_SIZE, 1)) # Covariance. Initial is very certain. self.PEst = np.zeros((STATE_SIZE, STATE_SIZE)) # landMark Estimation. Like former self.tar_pc self.lEst = np.zeros((LM_SIZE, 0)) # lEst should be of 2*N size # ros topic self.laser_sub = rospy.Subscriber('/course_agv/laser/scan', LaserScan, self.laserCallback) # self.location_pub = rospy.Publisher('ekf_location',Odometry,queue_size=3) ## localization parameters # minimum landmark matches to update. self.min_match = int(rospy.get_param('/slam/min_match', 2)) # minimum number of points for a landmark cluster self.extraction.landMark_min_pt = int( rospy.get_param('/slam/landMark_min_pt', 1)) # maximum radius to be identified as landmark self.extraction.radius_max_th = float( rospy.get_param('/slam/radius_max_th', 0.4)) OBSTACLE_RADIUS = 0.35 # feed icp landmark instead of laser. def laserCallback(self, msg): print('------seq: ', msg.header.seq) if self.isFirstScan: # feed in landmarks. # self.icp.tar_pc=self.extraction.process(msg) self.icp.tar_pc = self.icp.laserToNumpy(msg) self.isFirstScan = False return # Update once every 5 laser scan because the we cannot distinguish rotation if interval is too small. self.laser_count += 1 if self.laser_count < self.laser_interval: return # Updating process self.laser_count = 0 # z is the landmarks in self frame as a 2*n array. z = self.extraction.process(msg, True) self.publishLandMark(z, "b") # relative displacement u = self.calc_odometry(msg) # xEst,lEst,PEst is both predicted and updated in the ekf. self.xEst, self.lEst, self.PEst = self.estimate( self.xEst, self.lEst, self.PEst, z, u) self.publishResult() return def calc_odometry(self, msg): ''' Let icp handle the odometry, returns a relative odometry u. ''' # laser callback is manually fed by its owner class. return self.icp.laserCallback(msg) # EKF virtual function. def observation_model(self, xEst, lEst): ''' returns an 2*n column vector list. [z1,z2,....zn] ''' rotation = tf.transformations.euler_matrix(0, 0, xEst[2, 0])[0:2, 0:2] z = np.dot(rotation.T, lEst - xEst[0:2]) return z def estimate(self, xEst, lEst, PEst, z, u): G, Fx = self.jacob_motion(xEst, lEst, u) # FIXME: the G and Fx transpose is confused. Thanks god they are all symmetric ones... covariance = np.dot(G, np.dot(PEst, G.T)) + np.dot( Fx, np.dot(Cx, Fx.T)) # Predict xPredict = self.odom_model(xEst, u) zEst = self.observation_model(xPredict, lEst) self.publishLandMark(zEst, color="r", namespace="estimate", frame=self.nodeName) neighbour = self.icp.findNearest(z, zEst) # Predicted zPredict = zEst[:, neighbour.tar_indices] self.publishLandMark(zPredict, color="g", namespace="paired", frame=self.nodeName) lSize = np.size(lEst, 1) zSize = np.size(z, 1) # how many observed landmarks are missed, how many new ones will be added. missedIndices = sorted( list(set(range(zSize)) - set(neighbour.src_indices))) paired = len(neighbour.src_indices) missed = len(missedIndices) neighbour.src_indices += missedIndices # the newly-added landmarks neighbour.tar_indices += range(lSize, missed + lSize) zObserved = z[:, neighbour.src_indices] # add new landmarks to newL (new lEst) # newL=np.repeat(xPredict[0:2],missed,axis=1) # delicately select the spawning position... newL = np.dot( tf.transformations.euler_matrix(0, 0, xPredict[2, 0])[0:2, 0:2], z[:, missedIndices]) + xPredict[0:2] newL = np.hstack((lEst, newL)) zEst = self.observation_model(xPredict, newL) zPredict = zEst[:, neighbour.tar_indices] # the newly-added landmarks' uncertainty is very large # block is so powerful! covariance = scilinalg.block_diag(covariance, np.diag([INF] * LM_SIZE * missed)) yPredict = self.XLtoY(xPredict, newL) variance = self.alpha / (zSize + self.alpha) * OBSTACLE_RADIUS print("\n\npaired: {} variance: {} missed: {} landmarks: {}".format( paired, variance, missed, lSize + missed)) if zSize < self.min_match: print("Observed landmarks are too little to execute update.") # only update according to the prediction stage. return xPredict, newL, covariance m = self.jacob_h(newL, neighbour, xPredict) # z (2*n)array->(2n*1) array zPredict = np.vstack(np.hsplit(zPredict, np.size(zPredict, 1))) zObserved = np.vstack(np.hsplit(zObserved, np.size(zObserved, 1))) # print("delta z: \n{}\n".format(zObserved-zPredict)) # print("covariance:\n{}\n".format(covariance)) # Karman factor. Universal formula. K = np.dot( np.dot(covariance, m.T), np.linalg.inv( np.dot(m, np.dot(covariance, m.T)) + np.diag([variance**2] * 2 * zSize))) # Update fix = np.dot(K, zObserved - zPredict) # print("fix: \n{}\n\n".format(fix)) yEst = yPredict + fix PEst = covariance - np.dot(K, np.dot(m, covariance)) xEst, lEst = self.YtoXL(yEst) return xEst, lEst, PEst def XLtoY(self, x, l): # split y (2*N) to 2N*1 y = np.vstack(np.hsplit(l, np.size(l, 1))) y = np.vstack((x, y)) return y def YtoXL(self, y): x, l = np.vsplit(y, [STATE_SIZE]) l = np.hstack(np.vsplit(l, np.size(l) / LM_SIZE)) return x, l
class Localization(): def __init__(self): self.icp = ICP() self.ekf = EKF() self.extraction = Extraction() # odom robot init states self.robot_x = rospy.get_param('/icp/robot_x',0) self.robot_y = rospy.get_param('/icp/robot_y',0) self.robot_theta = rospy.get_param('/icp/robot_theta',0) self.sensor_sta = [self.robot_x,self.robot_y,self.robot_theta] self.isFirstScan = True self.src_pc = [] self.tar_pc = [] # State Vector [x y yaw] self.xOdom = np.zeros((3,1)) self.xEst = np.zeros((3,1)) self.PEst = np.eye(3) # map observation self.obstacle = [] # radius self.obstacle_r = 10 # init map self.updateMap() # ros topic self.laser_sub = rospy.Subscriber('/course_agv/laser/scan',LaserScan,self.laserCallback) self.location_pub = rospy.Publisher('ekf_location',Odometry,queue_size=3) self.odom_pub = rospy.Publisher('icp_odom',Odometry,queue_size=3) self.odom_broadcaster = tf.TransformBroadcaster() self.landMark_pub = rospy.Publisher('/landMarks',MarkerArray,queue_size=1) def updateMap(self): print("debug: try update map obstacle") rospy.wait_for_service('/static_map') try: getMap = rospy.ServiceProxy('/static_map',GetMap) self.map = getMap().map # print(self.map) except: e = sys.exc_info()[0] print('Service call failed: %s'%e) # Update for planning algorithm map_data = np.array(self.map.data).reshape((-1,self.map.info.height)).transpose() tx,ty = np.nonzero((map_data > 20)|(map_data < -0.5)) ox = (tx*self.map.info.resolution+self.map.info.origin.position.x)*1.0 oy = (ty*self.map.info.resolution+self.map.info.origin.position.y)*1.0 self.obstacle = np.vstack((ox,oy)).transpose() self.obstacle_r = self.map.info.resolution print("debug: update map obstacle success! ") def laserCallback(self,msg): u = self.calc_odometry(msg) z = self.ekf.odom_model(self.xEst,self.calc_map_observation(self.laserToNumpy(msg))) self.xEst,self.PEst = self.ekf.estimate(self.xEst,self.PEst,z,u) self.publishResult() pass def laserEstimation(self): # TODO return pc def calc_map_observation(self,msg): landmarks_src = self.extraction.process(msg,True) landmarks_tar = self.extraction.process(self.laserEstimation(),True) print("lm : ",len(landmarks_src.id),len(landmarks_tar.id)) self.publishLandMark(landmarks_src,landmarks_tar) src_pc = self.lm2pc(landmarks_src) tar_pc = self.lm2pc(landmarks_tar) transform_acc = self.icp.process(tar_pc,src_pc) return self.T2u(transform_acc) def calc_odometry(self,msg): if self.isFirstScan: self.tar_pc = self.laserToNumpy(msg) self.isFirstScan = False return np.array([[0.0,0.0,0.0]]).T self.src_pc = self.laserToNumpy(msg) transform_acc = self.icp.process(self.tar_pc,self.src_pc) self.tar_pc = self.laserToNumpy(msg) return self.T2u(transform_acc) def lm2pc(self,lm): total_num = len(lm.id) dy = lm.position_y dx = lm.position_x range_l = np.hypot(dy,dx) angle_l = np.arctan2(dy,dx) pc = np.ones((3,total_num)) pc[0:2,:] = np.vstack((np.multiply(np.cos(angle_l),range_l),np.multiply(np.sin(angle_l),range_l))) print("mdbg ",total_num) return pc def laserToNumpy(self,msg): total_num = len(msg.ranges) pc = np.ones([3,total_num]) range_l = np.array(msg.ranges) angle_l = np.linspace(msg.angle_min,msg.angle_max,total_num) pc[0:2,:] = np.vstack((np.multiply(np.cos(angle_l),range_l),np.multiply(np.sin(angle_l),range_l))) return pc def T2u(self,t): dw = math.atan2(t[1,0],t[0,0]) u = np.array([[t[0,2],t[1,2],dw]]) return u.T def publishResult(self): # tf s = self.xEst.reshape(-1) q = tf.transformations.quaternion_from_euler(0,0,s[2]) self.odom_broadcaster.sendTransform((s[0],s[1],0.001),(q[0],q[1],q[2],q[3]), rospy.Time.now(),"ekf_location","world_base") # odom odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "world_base" odom.pose.pose.position.x = s[0] odom.pose.pose.position.y = s[1] odom.pose.pose.position.z = 0.001 odom.pose.pose.orientation.x = q[0] odom.pose.pose.orientation.y = q[1] odom.pose.pose.orientation.z = q[2] odom.pose.pose.orientation.w = q[3] self.location_pub.publish(odom) s = self.xOdom q = tf.transformations.quaternion_from_euler(0,0,s[2]) # odom odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "world_base" odom.pose.pose.position.x = s[0] odom.pose.pose.position.y = s[1] odom.pose.pose.position.z = 0.001 odom.pose.pose.orientation.x = q[0] odom.pose.pose.orientation.y = q[1] odom.pose.pose.orientation.z = q[2] odom.pose.pose.orientation.w = q[3] self.odom_pub.publish(odom) # self.laser_pub.publish(self.target_laser) pass def publishLandMark(self,msg,msg2): # msg = LandMarkSet() if len(msg.id) <= 0: return landMark_array_msg = MarkerArray() for i in range(len(msg.id)): marker = Marker() marker.header.frame_id = "course_agv__hokuyo__link" marker.header.stamp = rospy.Time(0) marker.ns = "lm" marker.id = i marker.type = Marker.SPHERE marker.action = Marker.ADD marker.pose.position.x = msg.position_x[i] marker.pose.position.y = msg.position_y[i] marker.pose.position.z = 0 # 2D marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.color.a = 0.5 # Don't forget to set the alpha marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 landMark_array_msg.markers.append(marker) for i in range(len(msg2.id)): marker = Marker() marker.header.frame_id = "course_agv__hokuyo__link" marker.header.stamp = rospy.Time(0) marker.ns = "lm" marker.id = i+len(msg.id) marker.type = Marker.SPHERE marker.action = Marker.ADD marker.pose.position.x = msg2.position_x[i] marker.pose.position.y = msg2.position_y[i] marker.pose.position.z = 0 # 2D marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.color.a = 0.5 # Don't forget to set the alpha marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 landMark_array_msg.markers.append(marker) self.landMark_pub.publish(landMark_array_msg)
class SLAM_EKF(): def __init__(self): # ros param self.robot_x = rospy.get_param('/slam/robot_x',0) self.robot_y = rospy.get_param('/slam/robot_y',0) self.robot_theta = rospy.get_param('/slam/robot_theta',0) ## ros param of mapping self.map_x_width = rospy.get_param('/slam/map_width') self.map_y_width = rospy.get_param('/slam/map_height') self.map_reso = rospy.get_param('/slam/map_resolution') self.map_cellx_width = int(round(self.map_x_width/self.map_reso)) self.map_celly_width = int(round(self.map_y_width/self.map_reso)) self.icp = ICP() self.ekf = EKF() self.extraction = Extraction() self.mapping = Mapping(self.map_cellx_width,self.map_celly_width,self.map_reso) # odom robot init states self.sensor_sta = [self.robot_x,self.robot_y,self.robot_theta] self.isFirstScan = True self.src_pc = [] self.tar_pc = [] # State Vector [x y yaw] self.xOdom = np.zeros((STATE_SIZE, 1)) self.xEst = np.zeros((STATE_SIZE, 1)) self.PEst = np.eye(STATE_SIZE) # map observation self.obstacle = [] # radius self.obstacle_r = 10 # ros topic self.laser_sub = rospy.Subscriber('/course_agv/laser/scan',LaserScan,self.laserCallback) self.location_pub = rospy.Publisher('ekf_location',Odometry,queue_size=3) self.odom_pub = rospy.Publisher('icp_odom',Odometry,queue_size=3) self.odom_broadcaster = tf.TransformBroadcaster() self.landMark_pub = rospy.Publisher('/landMarks',MarkerArray,queue_size=1) self.map_pub = rospy.Publisher('/slam_map',OccupancyGrid,queue_size=1) def laserCallback(self,msg): np_msg = self.laserToNumpy(msg) lm = self.extraction.process(np_msg) # u = self.calc_odometry(self.lm2pc(lm)) u = self.calc_odometry(np_msg) z = self.observation(lm) self.xEst,self.PEst = self.ekf.estimate(self.xEst,self.PEst,z,u) # TODO # obs = self.u2T(self.xEst[0:3]).dot(np_msg) # pmap = self.mapping.update(obs[0], obs[1], self.xEst[0], self.xEst[1]) # self.publishMap(pmap) self.publishLandMark(lm) self.publishResult() pass def observation(self,lm): landmarks = lm z = np.zeros((0, 3)) for i in range(len(landmarks.id)): dx = landmarks.position_x[i] dy = landmarks.position_y[i] d = math.hypot(dx, dy) angle = self.ekf.pi_2_pi(math.atan2(dy, dx)) zi = np.array([d, angle, i]) z = np.vstack((z, zi)) return z def calc_odometry(self,np_msg): if self.isFirstScan: self.tar_pc = np_msg self.isFirstScan = False return np.array([[0.0,0.0,0.0]]).T self.src_pc = np_msg transform_acc = self.icp.process(self.tar_pc,self.src_pc) self.tar_pc = np_msg return self.T2u(transform_acc) def laserToNumpy(self,msg): total_num = len(msg.ranges) pc = np.ones([3,total_num]) range_l = np.array(msg.ranges) range_l[range_l == np.inf] = MAX_LASER_RANGE angle_l = np.linspace(msg.angle_min,msg.angle_max,total_num) pc[0:2,:] = np.vstack((np.multiply(np.cos(angle_l),range_l),np.multiply(np.sin(angle_l),range_l))) return pc def T2u(self,t): dw = math.atan2(t[1,0],t[0,0]) u = np.array([[t[0,2],t[1,2],dw]]) return u.T def u2T(self,u): w = u[2] dx = u[0] dy = u[1] return np.array([ [ math.cos(w),-math.sin(w), dx], [ math.sin(w), math.cos(w), dy], [0,0,1] ]) def lm2pc(self,lm): total_num = len(lm.id) dy = lm.position_y dx = lm.position_x range_l = np.hypot(dy,dx) angle_l = np.arctan2(dy,dx) pc = np.ones((3,total_num)) pc[0:2,:] = np.vstack((np.multiply(np.cos(angle_l),range_l),np.multiply(np.sin(angle_l),range_l))) print("mdbg ",total_num) return pc def publishResult(self): # tf s = self.xEst.reshape(-1) q = tf.transformations.quaternion_from_euler(0,0,s[2]) self.odom_broadcaster.sendTransform((s[0],s[1],0.001),(q[0],q[1],q[2],q[3]), rospy.Time.now(),"ekf_location","world_base") # odom odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "world_base" odom.pose.pose.position.x = s[0] odom.pose.pose.position.y = s[1] odom.pose.pose.position.z = 0.001 odom.pose.pose.orientation.x = q[0] odom.pose.pose.orientation.y = q[1] odom.pose.pose.orientation.z = q[2] odom.pose.pose.orientation.w = q[3] self.location_pub.publish(odom) s = self.xOdom q = tf.transformations.quaternion_from_euler(0,0,s[2]) # odom odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "world_base" odom.pose.pose.position.x = s[0] odom.pose.pose.position.y = s[1] odom.pose.pose.position.z = 0.001 odom.pose.pose.orientation.x = q[0] odom.pose.pose.orientation.y = q[1] odom.pose.pose.orientation.z = q[2] odom.pose.pose.orientation.w = q[3] self.odom_pub.publish(odom) print("mdbg ",self.xEst.shape) # self.laser_pub.publish(self.target_laser) pass def publishLandMark(self,msg): # msg = LandMarkSet() if len(msg.id) <= 0: return landMark_array_msg = MarkerArray() for i in range(len(msg.id)): marker = Marker() marker.header.frame_id = "course_agv__hokuyo__link" marker.header.stamp = rospy.Time(0) marker.ns = "lm" marker.id = i marker.type = Marker.SPHERE marker.action = Marker.ADD marker.pose.position.x = msg.position_x[i] marker.pose.position.y = msg.position_y[i] marker.pose.position.z = 0 # 2D marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.color.a = 0.5 # Don't forget to set the alpha marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 landMark_array_msg.markers.append(marker) for i in range(((self.xEst.shape[0]-STATE_SIZE)/2)): marker = Marker() marker.header.frame_id = "world_base" marker.header.stamp = rospy.Time(0) marker.ns = "lm" marker.id = i+len(msg.id) marker.type = Marker.SPHERE marker.action = Marker.ADD marker.pose.position.x = self.xEst[STATE_SIZE+i*2,0] marker.pose.position.y = self.xEst[STATE_SIZE+i*2+1,0] marker.pose.position.z = 0 # 2D marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.color.a = 0.5 # Don't forget to set the alpha marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 landMark_array_msg.markers.append(marker) self.landMark_pub.publish(landMark_array_msg) def publishMap(self,pmap): map_msg = OccupancyGrid() map_msg.header.seq = 1 map_msg.header.stamp = rospy.Time().now() map_msg.header.frame_id = "map" map_msg.info.map_load_time = rospy.Time().now() map_msg.info.resolution = self.map_reso map_msg.info.width = self.map_cellx_width map_msg.info.height = self.map_celly_width map_msg.info.origin.position.x = -self.map_cellx_width*self.map_reso/2.0 map_msg.info.origin.position.y = -self.map_celly_width*self.map_reso/2.0 map_msg.info.origin.position.z = 0 map_msg.info.origin.orientation.x = 0 map_msg.info.origin.orientation.y = 0 map_msg.info.origin.orientation.z = 0 map_msg.info.origin.orientation.w = 1.0 map_msg.data = list(pmap.T.reshape(-1)*100) self.map_pub.publish(map_msg)
class EKF_Landmark_Localization(LandmarkLocalization, EKF_Landmark): alpha = 3.0 # factor in estimating covariance. def __init__(self, nodeName="ekf_icp"): super(EKF_Landmark_Localization, self).__init__(nodeName) self.icp = SubICP() self.extraction = Extraction() self.src_pc = None self.isFirstScan = True self.laser_count = 0 # interval self.laser_interval = 5 # State Vector [x y yaw].T, column vector. self.xEst = np.zeros((STATE_SIZE, 1)) # Covariance. Initial state is certain. # self.PEst=np.eye(STATE_SIZE) self.PEst = np.zeros((STATE_SIZE, STATE_SIZE)) # init map # map observation self.tar_pc = None self.updateMap() # ros topic self.laser_sub = rospy.Subscriber('/course_agv/laser/scan', LaserScan, self.laserCallback) # self.location_pub = rospy.Publisher('ekf_location',Odometry,queue_size=3) # parameters from launch file. # minimum landmark matches to update. Actually even 1 point is acceptable. self.min_match = int(rospy.get_param('/localization/min_match', 1)) # minimum number of points for a landmark cluster self.extraction.landMark_min_pt = int( rospy.get_param('/localization/landMark_min_pt', 2)) # maximum radius to be identified as landmark self.extraction.radius_max_th = float( rospy.get_param('/localization/radius_max_th', 0.4)) def updateMap(self): ''' get all the isolated pixels in the map as landmarks, then extract the positions of landmarks into self.tar_pc ''' print("debug: try update map obstacle") rospy.wait_for_service('/static_map') try: getMap = rospy.ServiceProxy('/static_map', GetMap) self.map = getMap().map # print(self.map) except: e = sys.exc_info()[0] print('Service call failed: %s' % e) # transposed (row,column)-> (x,y) self.map.data = np.array(self.map.data).reshape( (-1, self.map.info.height)).transpose() tx, ty = np.nonzero((self.map.data > 20) | (self.map.data < -0.5)) landmarkList = [] for (x, y) in zip(tx, ty): if self.isolated((x, y)): landmarkList.append((x, y)) originPos = np.array( [self.map.info.origin.position.x, self.map.info.origin.position.y]) # numpy's broadcasting feature self.tar_pc = np.transpose( np.array(landmarkList) * self.map.info.resolution + originPos) print("landmark list:\n{}".format(self.tar_pc)) print("debug: update map obstacle success! ") def isolated(self, pair): # direction: up, down, left, right directions = np.array([(0, 1), (0, -1), (-1, 0), (1, 0)]) pair = np.array(pair) for dr in directions: newPair = pair + dr # detects if the obstacle is on the boundary if newPair[0] < 0 or newPair[0] >= self.map.info.width or newPair[ 1] < 0 or newPair[1] >= self.map.info.height: continue if self.map.data[tuple(newPair)] < -0.5 or self.map.data[tuple( newPair)] > 20: return False return True # feed icp landmark instead of laser. def laserCallback(self, msg): print('------seq: ', msg.header.seq) if self.isFirstScan: # feed in landmarks. # self.icp.tar_pc=self.extraction.process(msg) self.icp.tar_pc = self.icp.laserToNumpy(msg) self.isFirstScan = False return # Update once every 5 laser scan because the we cannot distinguish rotation if interval is too small. self.laser_count += 1 if self.laser_count < self.laser_interval: return # Updating process self.laser_count = 0 landmarks = self.extraction.process(msg, True) self.publishLandMark(landmarks, "b") u = self.calc_odometry(msg) # z is the landmarks as a 2*n array. z = landmarks # xEst is both predicted and updated in the ekf. self.xEst, self.PEst = self.estimate(self.xEst, self.PEst, z, u) self.publishResult() return def calc_odometry(self, msg): ''' Let icp handle the odometry, returns a relative odometry u. ''' # laser callback is manually fed by its owner class. return self.icp.laserCallback(msg) # EKF virtual function. def observation_model(self, xEst): ''' returns an 2*n column vector list. [z1,z2,....zn] ''' rotation = tf.transformations.euler_matrix(0, 0, xEst[2, 0])[0:2, 0:2] z = np.dot(rotation.T, self.tar_pc - xEst[0:2, 0].reshape(2, 1)) return z def estimate(self, xEst, PEst, z, u): G, Fx = self.jacob_motion(xEst, u) # FIXME: the G and Fx transpose is confused. Thanks god they are all symmetric matrices... covariance = np.dot(G, np.dot(PEst, G.T)) + np.dot( Fx, np.dot(Cx, Fx.T)) # Predict xPredict = self.odom_model(xEst, u) zEst = self.observation_model(xPredict) self.publishLandMark(zEst, color="r", namespace="estimate", frame="ekf_icp") neighbour = self.icp.findNearest(z, zEst) zPredict = zEst[:, neighbour.tar_indices] zPrime = z[:, neighbour.src_indices] length = len(neighbour.src_indices) variance = self.alpha / (length + self.alpha) * OBSTACLE_RADIUS print("\n\nlength: {} variance: {}".format(length, variance)) # turns out no matter how little the information is, it is of value somewhat. min_match can be set to 1. if length < self.min_match: print("Matching points are too little to execute update.") # only update according to the prediction stage. return xPredict, covariance self.publishLandMark(zPredict, color="g", namespace="paired", frame="ekf_icp") m = self.jacob_h(self.tar_pc, neighbour, xPredict) # z (2*n)array->(2n*1) array zPredict = np.vstack(np.hsplit(zPredict, np.size(zPredict, 1))) zPrime = np.vstack(np.hsplit(zPrime, np.size(zPrime, 1))) print("delta z: \n{}\n\n".format(zPrime - zPredict)) # Karman factor. Universal formula. K = np.dot( np.dot(covariance, m.T), np.linalg.inv( np.dot(m, np.dot(covariance, m.T)) + np.diag([variance] * 2 * length))) # Update xEst = xPredict + np.dot(K, zPrime - zPredict) PEst = covariance - np.dot(K, np.dot(m, covariance)) return xEst, PEst