def predict(self, new_data): """ Based on GPS pos of the new data, predict new state X """ pos2D = self.mapdata.attitude.apply(new_data.gps_pos - self.mapdata.gps_pos)[0:2] att2D = rotation_proj(self.mapdata.attitude, new_data.attitude).as_euler('zxy')[0] F1 = np.array([[ 1, 0, -self.trans[0] * np.sin(self.prev_att2D) - self.trans[1] * np.cos(self.prev_att2D), np.cos(self.prev_att2D), -np.sin(self.prev_att2D), 0 ], [ 0, 1, self.trans[0] * np.cos(self.prev_att2D) - self.trans[1] * np.sin(self.prev_att2D), np.sin(self.prev_att2D), np.cos(self.prev_att2D), 0 ], [0, 0, 1, 0, 0, 1]]) #F2 = np.array([[0, 0, -(pos2D-self.prev_pos2D)[0]*np.sin(self.prev_att2D+self.rot)+(pos2D-self.prev_pos2D)[0]*np.cos(self.prev_att2D+self.rot), 0, 0, -(pos2D-self.prev_pos2D)[0]*np.sin(self.prev_att2D+self.rot)+(pos2D-self.prev_pos2D)[0]*np.cos(self.prev_att2D+self.rot)], # [0, 0, -(pos2D-self.prev_pos2D)[1]*np.cos(self.prev_att2D+self.rot)-(pos2D-self.prev_pos2D)[1]*np.sin(self.prev_att2D+self.rot), 0, 0, -(pos2D-self.prev_pos2D)[1]*np.cos(self.prev_att2D+self.rot)-(pos2D-self.prev_pos2D)[1]*np.sin(self.prev_att2D+self.rot)], # [0, 0, -1, 0, 0, 0]]) F2 = np.array([[ -np.cos(self.prev_att2D + self.rot), -np.sin(self.prev_att2D + self.rot), -(pos2D - self.prev_pos2D)[0] * np.sin(self.prev_att2D + self.rot) + (pos2D - self.prev_pos2D)[0] * np.cos(self.prev_att2D + self.rot), -np.cos(self.rot), -np.sin(self.rot), -(pos2D - self.prev_pos2D)[0] * np.sin(self.prev_att2D + self.rot) + (pos2D - self.prev_pos2D)[0] * np.cos(self.prev_att2D + self.rot) + self.trans[0] * np.sin(self.rot) - self.trans[1] * np.cos(self.rot) ], [ np.sin(self.prev_att2D + self.rot), -np.cos(self.prev_att2D + self.rot), -(pos2D - self.prev_pos2D)[1] * np.cos(self.prev_att2D + self.rot) - (pos2D - self.prev_pos2D)[1] * np.sin(self.prev_att2D + self.rot), np.sin(self.rot), -np.cos(self.rot), -(pos2D - self.prev_pos2D)[1] * np.cos(self.prev_att2D + self.rot) - (pos2D - self.prev_pos2D)[1] * np.sin(self.prev_att2D + self.rot) + self.trans[0] * np.cos(self.rot) + self.trans[1] * np.sin(self.rot) ], [0, 0, -1, 0, 0, -1]]) F = np.block([[F1], [F2 - F1]]) self.prev_pos2D = self.prev_pos2D + R(-self.prev_att2D).dot(self.trans) self.prev_att2D = self.prev_att2D + self.rot self.trans = R(self.prev_att2D).dot(pos2D - self.prev_pos2D) self.rot = att2D - self.prev_att2D self.P = F.dot(self.P).dot(F.T) + self.Q
def app(): ''' 业务函数 数据采集由另一脚本爬取完成并存入redis :return: ''' r = R() keys_it = iter(r.r.hkeys("bookshelf")[0:2]) prefix = "https://www.4wens.com" for key in keys_it: book_info = json.loads(r.r.hget("bookshelf", key)) uri = book_info[0] book_name = book_info[1] logging.info("{0} init...".format(book_name.encode("utf-8"))) url = "{0}{1}".format(prefix, uri) wc = WebCrawler() # 获取每本书第一章节url chapter_start_url, total_zhangjie = wc.init_page(url, prefix) if not (chapter_start_url and total_zhangjie): logging.warning("{0}------>init..失败".format(book_name)) continue logging.info("{0} init 完成 download..".format( book_name.encode("utf-8"))) wc.download_book(chapter_start_url, book_name, int(total_zhangjie)) logging.info("--------------------{0}-----------------下载完成..".format( book_name.encode("utf-8"))) r.r.hdel("bookshelf", key)
def polysFromSHP(polysFilename, datafield, polySubset): R.importLibrary('sp') shapes = shapelib.open(polysFilename) data = dbflib.open(polysFilename) polyList = [] for i in range(shapes.info()[0]): shp = shapes.read_object(i) d = data.read_record(i)[datafield] if len(polySubset) != 0: for item in polySubset: if item == d: p = SpatialPolygon(i, shp.vertices(), d) polyList.append(p) else: p = SpatialPolygon(i, shp.vertices(), d) polyList.append(p) return polyList
def polysFromSHP (polysFilename, datafield, polySubset): R.importLibrary ('sp') shapes = shapelib.open(polysFilename) data = dbflib.open (polysFilename) polyList = [] for i in range (shapes.info ()[0]): shp = shapes.read_object (i) d = data.read_record (i)[datafield] if len(polySubset) != 0: for item in polySubset: if item == d: p = SpatialPolygon (i, shp.vertices (), d) polyList.append(p) else: p = SpatialPolygon (i, shp.vertices (), d) polyList.append(p) return polyList
def pushPoints (self, pointTree): for s in self.simplePolys: pList = pointTree.search (s.bounds) v = pList.vector () c = s.coordinates locations = R.function ('point.in.polygon', v.x, v.y, c.x, c.y) for i, val in enumerate(locations): if val != 0: self.points.append (pList[i])
def pushPoints(self, pointTree): for s in self.simplePolys: pList = pointTree.search(s.bounds) v = pList.vector() c = s.coordinates locations = R.function('point.in.polygon', v.x, v.y, c.x, c.y) for i, val in enumerate(locations): if val != 0: self.points.append(pList[i])
def __init__ (self, shp, data, source, geom=None): Polygon.__init__ (self, data, source) self.simplePolys = [] self._geom = geom maxXList = [] maxYList = [] minXList = [] minYList = [] for s in shp: simple = SimpleSpatialPolygon (s) self.simplePolys.append (simple) maxXList.append (simple.max[0]) maxYList.append (simple.max[1]) minXList.append (simple.min[0]) minYList.append (simple.min[1]) maxX = max (maxXList) maxY = max (maxYList) minX = min (minXList) minY = min (minYList) self.bounds = BoundingBox ((maxX, maxY), (minX, minY)) if not SpatialPolygon.pointInPolygon: R.importLibrary ('sp') SpatialPolygon.pointInPolygon = True
def __init__(self, shp, data, source, geom=None): Polygon.__init__(self, data, source) self.simplePolys = [] self._geom = geom maxXList = [] maxYList = [] minXList = [] minYList = [] for s in shp: simple = SimpleSpatialPolygon(s) self.simplePolys.append(simple) maxXList.append(simple.max[0]) maxYList.append(simple.max[1]) minXList.append(simple.min[0]) minYList.append(simple.min[1]) maxX = max(maxXList) maxY = max(maxYList) minX = min(minXList) minY = min(minYList) self.bounds = BoundingBox((maxX, maxY), (minX, minY)) if not SpatialPolygon.pointInPolygon: R.importLibrary('sp') SpatialPolygon.pointInPolygon = True
Thus t could be a period of time """ loc, t = key #loc should also be a numpy sequence? 2x X? assert loc.shape[0] == t.shape[0] #主要是因为前面的形状问题没有解决 return self.earth_magnet(t) + self.drill_magnet(loc) if __name__ == "__main__": earth_stable = np.asarray([29364, 3013, 44520]) rescue_loc = np.asarray([[100, 100, 0]]) target_loc = np.asarray([[0, 0, 0]]) rescue_alpha = R(5.5) target_alpha = R(5.) rescue_theta = R(78) target_theta = R(69) #与z轴夹角,地下为正,那么东为y magneticfield = MagneticField(target_loc, target_theta, target_alpha, earth_stable) from mpl_toolkits.mplot3d import axes3d import matplotlib.pyplot as plt fig = plt.figure(0) ax = fig.gca(projection='3d') ax.invert_xaxis() #ax.invert_yaxis() ax.invert_zaxis()
def process_position(self, new_data): return self.mapdata.gps_pos + self.mapdata.attitude.apply( np.append(self.prev_pos2D + R(-self.prev_att2D).dot(self.trans), 0), True)