[ +4, 12 ],
[ -2, 15 ],
[ +5, 8 ],
[ -4, 15 ],
[ -4, 10 ],
[ -2, 15 ]
]
# Generate INS signals
[ imu_time, imu_accel,
gnss_time, gnss_dist,
real_body_angles,
real_glob_accel, real_glob_speed, real_glob_dist
] = generate_signals(
speed_changes,
imu_period, accel_w_std,
gnss_period, gnss_w_std
)
# Initial params estimation
real_body_alpha0 = real_body_angles[0].item( ( 0, 0 ) )
imu_body_alpha0 = real_body_alpha0 + np.random.normal( 0, imu_body_alpha0_std )
print('Real a0: ' + str(real_body_alpha0 / np.pi * 180) + ', estimated a0: ' + str(imu_body_alpha0 / np.pi * 180) )
# Estimate body motion
ins_state = ins_ext_kfilter(
imu_time, imu_accel, imu_body_alpha0, imu_body_alpha0_std,
gnss_time, gnss_dist, gnss_w_std )
imu_accel_x = [ v.item( (0, 0) ) for v in imu_accel ]
imu_accel_y = [ v.item( (1, 0) ) for v in imu_accel ]
from ins_sig_gen import generate_signals
from ins_kalman import kfilter
# Config
acc_period = 0.01
acc_bias_const = 0.05
acc_bias_const_std = 0.1
acc_w_std = 0.03
gnss_period = 0.5
gnss_w_std = 1
# Generate INS signals
[
time_acc, accel_real, speed_real, dist_real, accel_bias, accel_ins,
speed_ins, dist_ins, time_gnss, dist_gnss
] = generate_signals(acc_period, acc_w_std, acc_bias_const, gnss_period,
gnss_w_std)
# Kalman filter
[kf_bias, kf_speed_err,
kf_dist_err] = kfilter(time_acc, dist_ins, time_gnss, dist_gnss,
acc_bias_const_std, gnss_w_std)
# Corrected motion data
speed_kf = [
ins_val - err_val for ins_val, err_val in zip(speed_ins, kf_speed_err)
]
dist_kf = [
ins_val - err_val for ins_val, err_val in zip(dist_ins, kf_dist_err)
]
accel_kf = [ins_val - err_val for ins_val, err_val in zip(accel_ins, kf_bias)]
real_attitude0 = np.matrix([
# Psi
[0],
# Theta
[0],
# Gamma
[0]
])
# Generate INS signals
[
imu_time, imu_accel, imu_gyro, gnss_time, gnss_speed, gnss_dist,
real_accel_bias, real_gyro_bias, real_glob_attitude, real_glob_accel,
real_glob_speed, real_glob_speed_norm, real_glob_dist
] = generate_signals(speed_changes, rot_changes_x, rot_changes_y,
rot_changes_z, real_attitude0, imu_period, accel_bias0,
accel_w_std, gyro_bias0, gyro_w_std, gnss_period,
gnss_speed_w_std, gnss_dist_w_std)
# Simulate GNSS off
off_cnt = int(gnss_off_time / gnss_period) + 1
gnss_time = gnss_time[:off_cnt]
gnss_speed = gnss_speed[:off_cnt]
gnss_dist = gnss_dist[:off_cnt]
print(len(imu_accel))
# Initial params estimation
imu_attitude0 = real_attitude0 + np.matrix([
# Psi error
[imu_attitude0_err.item((0, 0))],
# Theta error
[imu_attitude0_err.item((1, 0))],
gnss_dist_w_std = 5
# 速度变化
speed_changes = [[+3, 10], [0, 10], [+4, 20], [-2, 10], [0, 30], [+5, 10],
[-4, 20], [0, 20], [-4, 10], [-2, 20]]
angle_changes = [[np.deg2rad(0), 20], [np.deg2rad(+45), 20],
[np.deg2rad(+30), 20], [np.deg2rad(+60), 25],
[np.deg2rad(+10), 5], [np.deg2rad(-20), 10],
[np.deg2rad(-140), 30], [np.deg2rad(0), 30]]
# Generate INS signals
[
imu_time, imu_accel, imu_gyro, gnss_time, gnss_speed, gnss_dist,
real_accel_bias, real_gyro_bias, real_body_angles, real_glob_accel,
real_glob_speed, real_glob_speed_norm, real_glob_dist
] = generate_signals(speed_changes, angle_changes, imu_period, accel_bias_std,
accel_w_std, gyro_bias_std, gyro_w_std, gnss_period,
gnss_speed_w_std, gnss_dist_w_std)
# Initial params estimation
real_body_alpha0 = real_body_angles[0].item((0, 0))
imu_body_alpha0 = real_body_alpha0 + np.random.normal(0, imu_body_alpha0_std)
print('Real a0: ' + str(np.rad2deg(real_body_alpha0)) + ', estimated a0: ' +
str(np.rad2deg(imu_body_alpha0)))
# Estimate body motion
[ins_state,
ins_var] = ins_ext_kfilter(imu_time, imu_accel, imu_gyro, accel_bias_std,
gyro_bias_std, imu_body_alpha0,
imu_body_alpha0_std, gnss_time, gnss_speed,
gnss_dist, gnss_speed_w_std, gnss_dist_w_std)