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
