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)
