def test_stateful_double_lstm_mode_2(): """ test_stateful_double_lstm_mode()函数的batch_size有问题,重新生成. :return: 无返回值 """ # 仿真数据 batch_size = 30 step_num = 10_020 model_time_step = 30 traj_batch = np.zeros(shape=(batch_size, step_num, 3)) length = 10 * 1.13 width = 6 * 1.13 high = 3.5 z_high = 2.0 anchors_loc = np.array([[-3.29, 1.13, 1.66], [3.57, -1.13, 0.925], [3.57, 2.26, 1.950], [-2.26, 3.39, 2.230]]) origin_coordinate = np.array([4 * 1.13, 2 * 1.13, 0]) los_sd = 20e-3 nlos_bias = 0.6 nlos_sd = 45e-3 ranging_batch = np.zeros(shape=(batch_size, step_num, len(anchors_loc))) for i in range(batch_size): traj_batch[i] = generator_3d_trajectory(step_num=step_num, step_mode=i // 2, length=length, width=width, high=high, z_high=z_high) ranging_batch[i], traj_batch[i] = generator_3d_ranging_data( traj_batch[i], anchors_loc, origin_coordinate, los_sd, nlos_bias, nlos_sd, 1) ranging_batch = ranging_batch.reshape( (step_num // model_time_step * batch_size, model_time_step, len(anchors_loc))) traj_batch = traj_batch.reshape( (step_num // model_time_step * batch_size, model_time_step, 3)) # 当前x-(batch_size, step_num, anchors_loc), y-(batch_size, step_num, 3) # 每个x输入维度与基站个数有关 # model model = ModelFactory.product_model(ModelType.STATEFUL_DOUBLE_LSTM_MODEL_2, input_dim=4) model.fit(x=ranging_batch, y=traj_batch, batch_size=batch_size, epochs=100) # 测试数据生成, batch_size = 1, step_num = 30, 三维坐标 traj_test = np.zeros(shape=(1, 30, 3)) traj_test = generator_3d_trajectory(step_num=30, step_mode=1, length=length, width=width, high=high, z_high=z_high) ranging_test, traj_test = generator_3d_ranging_data( traj_test, anchors_loc, origin_coordinate, los_sd, nlos_bias, nlos_sd, 1) model.save("./save_model/stateful_double_lstm_mode_2_1.h5")
def test_stateful_double_lstm_mode(): # 仿真数据 batch_size = 30 step_num = 10000 traj_batch = np.zeros(shape=(batch_size, step_num, 3)) length = 10 * 1.13 width = 6 * 1.13 high = 3.5 z_high = 2.0 anchors_loc = np.array([[-3.29, 1.13, 1.66], [3.57, -1.13, 0.925], [3.57, 2.26, 1.950], [-2.26, 3.39, 2.230]]) origin_coordinate = np.array([4 * 1.13, 2 * 1.13, 0]) los_sd = 20e-3 nlos_bias = 0.6 nlos_sd = 45e-3 ranging_batch = np.zeros(shape=(batch_size, step_num, len(anchors_loc))) for i in range(batch_size): traj_batch[i] = generator_3d_trajectory(step_num=step_num, step_mode=i // 2, length=length, width=width, high=high, z_high=z_high) ranging_batch[i], traj_batch[i] = generator_3d_ranging_data( traj_batch[i], anchors_loc, origin_coordinate, los_sd, nlos_bias, nlos_sd, 1) print(ranging_batch) # model model = ModelFactory.product_model(ModelType.STATEFUL_DOUBLE_LSTM_MODEL) # TODO: batch_size和time_step不匹配,需要进行调整 model.fit(x=ranging_batch, y=traj_batch, batch_size=batch_size, epochs=100)
def test_double_lstm_mode_line_traj_nlos(): """ 210311测试网络训练直线性能。加入nlos误差, 更改了基站z(扩展) :return: 无返回值 """ # 仿真数据 batch_size = 30 step_num = 10_020 model_time_step = 30 traj_batch = np.zeros(shape=(batch_size, step_num, 3)) # 30 10_020 3 length = 10 * 1.13 width = 6 * 1.13 high = 3.5 z_high = 2.0 anchors_loc = np.array([[-3, 1, 0.5], [4, -1., 0.], [3, 3, 2], [-2., 3, 2.5]]) origin_coordinate = np.array([4, 2, 0]) los_sd = 50e-3 # los_sd = 0 nlos_bias = 0.4 nlos_sd = 80e-3 ranging_batch = np.zeros(shape=(batch_size, step_num, len(anchors_loc))) for i in range(batch_size): traj_batch[i] = generator_3d_trajectory_2(step_num=step_num, length=length, width=width, high=high, z_high=z_high) ranging_batch[i], traj_batch[i] = generator_3d_ranging_data( traj_batch[i], anchors_loc, origin_coordinate, los_sd, nlos_bias, nlos_sd, mode=1) ranging_batch = ranging_batch.reshape( (step_num // model_time_step * batch_size, model_time_step, len(anchors_loc))) traj_batch = traj_batch.reshape( (step_num // model_time_step * batch_size, model_time_step, 3)) # 当前x-(batch_size, step_num, anchors_loc), y-(batch_size, step_num, 3) # 每个x输入维度与基站个数有关 # model model = ModelFactory.product_model(ModelType.DOUBLE_LSTM_MODEL, input_dim=4) model.fit(x=ranging_batch, y=traj_batch, batch_size=batch_size, epochs=1000) model.save( "./save_model/double_lstm_model_epoch_1000_0318_line_traj_nlos_3.h5")
def test_double_lstm_mode_line_traj(): """ 210309测试网络训练直线性能。 :return: 无返回值 """ # 仿真数据 batch_size = 30 step_num = 10_020 model_time_step = 30 traj_batch = np.zeros(shape=(batch_size, step_num, 3)) # 30 10_020 3 length = 10 * 1.13 width = 6 * 1.13 high = 3.5 z_high = 2.0 anchors_loc = np.array([[-3.29, 1.13, 1.66], [3.57, -1.13, 0.925], [3.57, 2.26, 1.950], [-2.26, 3.39, 2.230]]) origin_coordinate = np.array([4 * 1.13, 2 * 1.13, 0]) # los_sd = 20e-3 los_sd = 0 nlos_bias = 0.2 nlos_sd = 45e-3 ranging_batch = np.zeros(shape=(batch_size, step_num, len(anchors_loc))) for i in range(batch_size): # traj_batch[i] = generator_3d_trajectory(step_num=step_num, step_mode=1, length=length, width=width, # high=high, # z_high=z_high) traj_batch[i] = generator_3d_trajectory_2(step_num=step_num, length=length, width=width, high=high, z_high=z_high) ranging_batch[i], traj_batch[i] = generator_3d_ranging_data( traj_batch[i], anchors_loc, origin_coordinate, los_sd, nlos_bias, nlos_sd, 0) ranging_batch = ranging_batch.reshape( (step_num // model_time_step * batch_size, model_time_step, len(anchors_loc))) traj_batch = traj_batch.reshape( (step_num // model_time_step * batch_size, model_time_step, 3)) # 当前x-(batch_size, step_num, anchors_loc), y-(batch_size, step_num, 3) # 每个x输入维度与基站个数有关 # model model = ModelFactory.product_model(ModelType.DOUBLE_LSTM_MODEL, input_dim=4) model.fit(x=ranging_batch, y=traj_batch, batch_size=batch_size, epochs=1000) model.save("./save_model/double_lstm_model_epoch_1000_0309_line_traj_1.h5")
def test_save_model_4(): """ 测试直线无误差的模型,210311, 去除了stateful :return: """ # 生成数据 anchors_loc = np.array([[-3, 1, 0.5], [4, -1., 0.], [3, 3, 2], [-2., 3, 2.5]]) origin_coordinate = np.array([4, 2, 0]) length = 10 * 1.13 width = 6 * 1.13 high = 3.5 z_high = 2.0 los_sd = 100e-3 nlos_bias = 0.4 nlos_sd = 80e-3 traj_data = generator_3d_trajectory_2(30, length=length, width=width, high=high, z_high=z_high) traj_data = np.zeros((60, 3)) + np.array([1, 2, 1.5]) ranging_data, traj_data = generator_3d_ranging_data(traj_data, anchors_loc, origin_coordinate, los_sd, nlos_bias, nlos_sd, mode=0) # 载入模型 model = load_model( "./save_model/double_lstm_model_epoch_1000_0318_line_traj_nlos_3.h5") model.summary() plot_model(model, to_file='double_lstm_model_0319.png', show_shapes=True, expand_nested=True) traj_predict = model.predict(ranging_data.reshape(1, 60, 4)) print(traj_predict - traj_data) print(traj_predict) print( Evaluate.calc_mean_rmse(np.array([-3, 0, 1.5]), traj_predict.reshape(60, 3))) # print(ModelUtil.model_predit("../model_expriment/save_model/double_lstm_model_epoch_1000_0318_line_traj_nlos_3.h5", # ranging_data)) pass
def test_generator_3d_ranging_data(self): anchors_loc_list = [[0.0, 0.0, 0.0], [0.0, 5.0, 4.0], [4.0, 5.0, 12.0], [10.0, 5.0, 4.0]] anchors_loc = np.array(anchors_loc_list) origin_coordinate = np.array([2.5, 1.5, 0]) traj = data_generator.generator_3d_trajectory(10000, step_mode=1, random_loc=True, z_high=3) ranging_data, traj = data_generator.generator_3d_ranging_data( traj, anchors_loc, origin_coordinate, 20e-3, 0.6, 45e-3, 1, 0.4) np.set_printoptions(threshold=np.inf) print(ranging_data) print(traj) print(ranging_data.shape) print(traj.shape)
def test_save_model_1(): """ 测试 ./save_model/stateful_double_lstm_mode_2_1.h5 中模型, time_step=30, x_dim = 4(基站数目), y_dim = 3(三维坐标, 在序列中, time_step * y_dim) :return: """ # 仿真数据 batch_size = 30 # step_num = 1_020 step_num = 30 model_time_step = 30 traj_batch = np.zeros(shape=(batch_size, step_num, 3)) length = 10 * 1.13 width = 6 * 1.13 high = 3.5 z_high = 2.0 anchors_loc = np.array([[-3.29, 1.13, 1.66], [3.57, -1.13, 0.925], [3.57, 2.26, 1.950], [-2.26, 3.39, 2.230]]) origin_coordinate = np.array([4 * 1.13, 2 * 1.13, 0]) los_sd = 20e-3 nlos_bias = 0.2 nlos_sd = 45e-3 ranging_batch = np.zeros(shape=(batch_size, step_num, len(anchors_loc))) for i in range(batch_size): traj_batch[i] = generator_3d_trajectory(step_num=step_num, step_mode=i // 2, length=length, width=width, high=high, z_high=z_high) ranging_batch[i], traj_batch[i] = generator_3d_ranging_data( traj_batch[i], anchors_loc, origin_coordinate, los_sd, nlos_bias, nlos_sd, 1) ranging_batch = ranging_batch.reshape( (step_num // model_time_step * batch_size, model_time_step, len(anchors_loc))) traj_batch = traj_batch.reshape( (step_num // model_time_step * batch_size, model_time_step, 3)) # model model = load_model("./save_model/double_lstm_model_epoch_1000.h5+") model.summary() traj_predict = model.predict(ranging_batch, batch_size=30, steps=30) print(traj_predict) print(traj_batch) print(traj_predict - traj_batch)
def test_save_model_3(): """ 测试直线无误差的模型,210310 :return: """ # 生成数据 anchors_loc = np.array([[-3.29, 1.13, 1.66], [3.57, -1.13, 0.925], [3.57, 2.26, 1.950], [-2.26, 3.39, 2.230]]) origin_coordinate = np.array([4 * 1.13, 2 * 1.13, 0]) length = 10 * 1.13 width = 6 * 1.13 high = 3.5 z_high = 2.0 # los_sd = 20e-3 los_sd = 0 nlos_bias = 0.2 nlos_sd = 45e-3 traj_batch = np.zeros(shape=(30, 30, 3)) # batch_size, time_step, output_dim ranging_batch = np.zeros(shape=(30, 30, 4)) # batch_size, time_step, input_dim for i in range(len(traj_batch)): traj_batch[i] = generator_3d_trajectory_2(30, length=length, width=width, high=high, z_high=z_high) ranging_batch[i], traj_batch[i] = generator_3d_ranging_data( traj_batch[i], anchors_loc, origin_coordinate, los_sd, nlos_bias, nlos_sd, 0) pass # 载入模型 model = load_model( "./save_model/double_lstm_model_epoch_1000_0309_line_traj_1.h5") model.summary() traj_predict = model.predict(ranging_batch) print(traj_predict - traj_batch) pass
def test_save_model_2(): """ 测试 ./save_model/stateful_double_lstm_mode_2_1.h5 中模型, time_step=30, x_dim = 4(基站数目), y_dim = 3(三维坐标, 在序列中, time_step * y_dim) :return: """ # 仿真数据 batch_size = 30 step_num = 1_020 model_time_step = 30 traj_batch = np.zeros(shape=(batch_size, step_num, 3)) length = 10 * 1.13 width = 6 * 1.13 high = 3.5 z_high = 2.0 anchors_loc = np.array([[-3.29, 1.13, 1.66], [3.57, -1.13, 0.925], [3.57, 2.26, 1.950], [-2.26, 3.39, 2.230]]) origin_coordinate = np.array([4 * 1.13, 2 * 1.13, 0]) los_sd = 20e-3 nlos_bias = 0.2 nlos_sd = 45e-3 model = load_model("./save_model/stateful_double_lstm_mode_2_1.h5") model.summary() # 测试数据生成, batch_size = 1, step_num = 30, 三维坐标 traj_test = np.zeros(shape=(1, 30, 3)) traj_test[0] = generator_3d_trajectory(step_num=30, step_mode=1, length=length, width=width, high=high, z_high=z_high) ranging_test = np.zeros(shape=(1, 30, 4)) ranging_test[0], traj_test[0] = generator_3d_ranging_data( traj_test[0], anchors_loc, origin_coordinate, los_sd, nlos_bias, nlos_sd, 1) traj_predict = model.__predict(ranging_test, batch_size=1, steps=30)
def move_point(anchor_loc: np.ndarray, point_start: np.ndarray, point_end: np.ndarray, mode: RangingMode, **kwargs): """ 移动定位,有两种模式,LOS NLOs :param anchor_loc: 基站坐标,维度-(基站个数,3) :param point_start: 起始位置,维度(1,3) :param point_end: 终点位置,维度(1,3) :param mode: RangingMode.NLOS, RangingMode.LOS :param kwargs: 配置LOS,和NLOS参数 :return: """ assert anchor_loc.shape == (len(anchor_loc), 3) assert point_start.shape == (1, 3) assert point_end.shape == (1, 3) # 获取参数 los_sd = kwargs.get('los_sd', 0.04) nlos_sd = kwargs.get("nlos_sd", 0.08) nlos_bias = kwargs.get("nlos_bias", 0.4) nlos_prob = kwargs.get("nlos_prob", 0.4) # 模式 if mode == RangingMode.LOS: print("LOS Mode, los_sd = " + str(los_sd)) elif mode == RangingMode.NLOS: print("NLOS Mode, los_sd = " + str(los_sd) + ", nlos_prob = " + str(nlos_prob) + ", nlos_sd = " + str(nlos_sd) + ", nlos_bias = " + str(nlos_bias)) # 获取轨迹 traj = walk_line_a2b(point_start, point_end, speed=1.5, delta_t=0.2) len_traj = len(traj) ranging_data, traj, nlos_record = generator_3d_ranging_data( traj, anchor_loc, np.array([0, 0, 0]), los_sd, nlos_bias, nlos_sd, mode, nlos_prob) traj_nlos = CommonUtil.nlos_traj_generate(traj, nlos_record) # 神经网络预测 pred_traj_lstm = ModelUtil.model_predit( "../model_expriment/save_model/double_lstm_model_epoch_1000_0318_line_traj_nlos_3.h5", ranging_data) # 参数 cov_mat = los_sd * los_sd * np.eye(len(anchor_loc)) # 扩展卡尔曼参数 spatial_dimension = 3 num_anchor = 4 Q = np.diag(np.array([0.001, 0.001, 0.001, 0.001, 0.001, 0.001])) R = los_sd * los_sd * np.eye(len(anchor_loc)) P_init = np.eye(2 * spatial_dimension) ekf = EKF(num_anchor, spatial_dimension, anchor_loc, 0.2, Q, R, P_init) # ekf.set_init_position(point_start) # 卡尔曼初始化 Q = np.diag(np.array([0.001, 0.001, 0.001, 0.001, 0.001, 0.001])) R = los_sd * los_sd * np.eye(3) P_init = np.eye(2 * spatial_dimension) kf = KF(spatial_dimension, 0.2, Q, R, P_init) # kf.set_init_position(point_start) # 轨迹 pred_traj_ekf_3d = np.zeros(shape=(len_traj, 3)) pred_traj_c_t_k_3d = np.zeros(shape=(len_traj, 3)) for i in range(len_traj): # ekf pred_traj_ekf_3d[i] = Location.positioning(LocationType.EKF_3d, anchor_loc, ranging_data[i], cov_mat, ekf=ekf) # kf pred_traj_c_t_k_3d[i] = Location.positioning(LocationType.C_T_K_3d, anchor_loc, ranging_data[i], cov_mat, kf=kf) pass dict_traj = { "true_traj": traj, "EKF": pred_traj_ekf_3d, "C-T-K": pred_traj_c_t_k_3d, "LSTM": pred_traj_lstm, "nlos_mark": traj_nlos } dict_color = { "true_traj": "k", "EKF": "r", "C-T-K": "g", "LSTM": "b", "nlos_mark": "y" } Plot.plot_3dtraj_line_chart(dict_traj, dict_color, mode=mode) # 误差 euclid_error_lstm = np.zeros(shape=(len_traj, 2)) euclid_error_ekf = np.zeros(shape=(len_traj, 2)) euclid_error_c_t_k = np.zeros(shape=(len_traj, 2)) euclid_error_lstm[:, 0] = euclid_error_ekf[:, 0] = euclid_error_c_t_k[:, 0] = np.array( range(0, len_traj)) * 0.2 euclid_error_lstm[:, 1] = Evaluate.calc_rmse(traj, pred_traj_lstm) euclid_error_ekf[:, 1] = Evaluate.calc_rmse(traj, pred_traj_ekf_3d) euclid_error_c_t_k[:, 1] = Evaluate.calc_rmse(traj, pred_traj_c_t_k_3d) dict_dist = { "EKF": euclid_error_ekf, "C-T-K": euclid_error_c_t_k, "LSTM": euclid_error_lstm } dict_color = {"EKF": "r", "C-T-K": "g", "LSTM": "b"} Plot.plot_rmse_error_line_chart(dict_dist, dict_color, mode) print(Evaluate.calc_mean_rmse(traj, pred_traj_lstm)) print(Evaluate.calc_mean_rmse(traj, pred_traj_ekf_3d)) print(Evaluate.calc_mean_rmse(traj, pred_traj_c_t_k_3d)) pass
def fixed_point_los(anchor_loc: np.ndarray, itera_num: int = 500, los_sd: float = 0.01, plot_flag: bool = True): """ 视距情况,定点定位。 测试:Chan、Chan-Taylor、Kalman、C-T-K :param anchor_loc: 基站坐标 :param los_sd: 测距标准差 :param itera_num: 迭代次数 :param plot_flag: 是否画图 :return: 无返回值,作图 """ assert anchor_loc.shape == (len(anchor_loc), 3) origin_coordinate = np.array([0, 0, 0]) tag_loc = np.array([[0, 0, 1]]) # 0 0 1.2 # tag_loc = np.array([[0, 0, 1.2]]) # 0 0 1.2 # los_sd = 0.05 nlos_bias = 0.4 nlos_sd = 0.08 cov_mat = los_sd * los_sd * np.eye(len(anchor_loc)) # 扩展卡尔曼参数 spatial_dimension = 3 num_anchor = 4 Q = np.diag(np.array([0.001, 0.001, 0.001, 0.001, 0.001, 0.001])) R = cov_mat P_init = np.eye(2 * spatial_dimension) ekf = EKF(num_anchor, spatial_dimension, anchor_loc, 0.2, Q, R, P_init) # 卡尔曼初始化 Q = np.diag(np.array([0.001, 0.001, 0.001, 0.001, 0.001, 0.001])) R = los_sd * los_sd * np.eye(3) P_init = np.eye(2 * spatial_dimension) kf = KF(spatial_dimension, 0.2, Q, R, P_init) # 数据统计,次数 # itera_num = 510 pred_loc_chan_3d = np.zeros(shape=(itera_num, 3)) pred_loc_chan_taylor_3d = np.zeros(shape=(itera_num, 3)) pred_loc_ekf_3d = np.zeros(shape=(itera_num, 3)) pred_loc_c_t_k_3d = np.zeros(shape=(itera_num, 3)) # 轨迹 traj = np.zeros(shape=(itera_num, 3)) + tag_loc ranging_data, tag_loc = generator_3d_ranging_data(traj, anchor_loc, origin_coordinate, los_sd=los_sd, nlos_bias=nlos_bias, nlos_sd=nlos_sd, mode=1, nlos_prob=0) for i in range(itera_num): # 生成测距数据 # chan pred_loc_chan_3d[i] = Location.positioning( LocationType.Chan_3d, anchors_loc=anchor_loc, ranging_data=ranging_data[i], cov_mat=cov_mat) # chan-taylor pred_loc_chan_taylor_3d[i] = Location.positioning( LocationType.Chan_Taylor_3d, anchors_loc=anchor_loc, ranging_data=ranging_data[i], cov_mat=cov_mat) # ekf pred_loc_ekf_3d[i] = Location.positioning(LocationType.EKF_3d, anchor_loc, ranging_data[i], cov_mat, ekf=ekf) # kf pred_loc_c_t_k_3d[i] = Location.positioning(LocationType.C_T_K_3d, anchor_loc, ranging_data[i], cov_mat, kf=kf) # 神经网络预测 pred_loc_lstm = ModelUtil.model_predit( "../model_expriment/save_model/double_lstm_model_epoch_1000_0318_line_traj_nlos_3.h5", ranging_data) # 计算rmse rmse_chan = Evaluate.calc_mean_rmse(tag_loc, pred_loc_chan_3d) rmse_c_t = Evaluate.calc_mean_rmse(tag_loc, pred_loc_chan_taylor_3d) rmse_ekf = Evaluate.calc_mean_rmse(tag_loc, pred_loc_ekf_3d) rmse_c_t_k = Evaluate.calc_mean_rmse(tag_loc, pred_loc_c_t_k_3d) rmse_lstm = Evaluate.calc_mean_rmse(tag_loc, pred_loc_lstm) if plot_flag: dict = { "Chan": pred_loc_chan_3d, "C-T": pred_loc_chan_taylor_3d, "EKF": pred_loc_ekf_3d, "C-T-K": pred_loc_c_t_k_3d, "pos_true": tag_loc } color_dict = { "Chan": "k", "C-T": "b", "EKF": "r", "C-T-K": "g", "pos_true": "y" } Plot.plot_scatter_3d(dict, color_dict) evaluate_dict = { "Chan": rmse_chan, "C-T": rmse_c_t, "EKF": rmse_ekf, "C-T-K": rmse_c_t_k, "LSTM": rmse_lstm } return evaluate_dict pass