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)
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)