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)