class Fusion:
def __init__(self, acc_n=1e-2, gyr_n=1e-4, acc_w=1e-6, gyr_w=1e-8):
self.initialized = False
self.init_lla_ = None
self.I_p_Gps_ = np.array([.0, .0, .0])
self.imu_buff = deque([], maxlen=100)
self.ekf = EKF(acc_n, gyr_n, acc_w, gyr_w)
rospy.Subscriber("imu/data", Imu, self.imu_callback)
rospy.Subscriber("/fix", NavSatFix, self.gnss_callback)
self.pub_path = rospy.Publisher("nav_path", Path, queue_size=10)
self.pub_odom = rospy.Publisher("nav_odom", Odometry, queue_size=10)
self.nav_path = Path()
self.last_imu = None
self.p_G_Gps_ = None
def imu_callback(self, msg):
imu = ImuData()
imu.timestamp = msg.header.stamp.to_sec()
imu.acc = np.array([
msg.linear_acceleration.x, msg.linear_acceleration.y,
msg.linear_acceleration.z
])
imu.gyr = np.array([
msg.angular_velocity.x, msg.angular_velocity.y,
msg.angular_velocity.z
])
if (not self.initialized):
self.imu_buff.append(imu)
return
self.ekf.predict(self.last_imu, imu)
self.last_imu = imu
self.pub_state()
def gnss_callback(self, msg):
if (msg.status.status != 2):
print("Error with GPS!!")
return
gps = GPSData()
gps.timestamp = msg.header.stamp.to_sec()
gps.lla = np.array([msg.latitude, msg.longitude, msg.altitude])
gps.cov = np.array(msg.position_covariance).reshape((3, 3))
if (not self.initialized):
if (len(self.imu_buff) < 100):
print("not enough imu data!!")
return
self.last_imu = self.imu_buff[99]
if (abs(gps.timestamp - self.last_imu.timestamp) > 0.5):
print("GPS and imu not synchronized!!")
return
self.ekf.state.timestamp = self.last_imu.timestamp
if (not self.init_rot_from_imu_data()):
return
self.init_lla_ = gps.lla
self.initialized = True
return
self.p_G_Gps_ = gps.lla2enu(self.init_lla_, gps.lla)
p_GI = self.ekf.state.p_GI
r_GI = self.ekf.state.r_GI
residual = self.p_G_Gps_ - (p_GI + r_GI.dot(self.I_p_Gps_))
H = np.zeros((3, 15))
H[:3, :3] = np.eye(3)
H[:3, 6:9] = -r_GI.dot(skew_matrix(self.I_p_Gps_))
V = gps.cov
self.ekf.update_measurement(H, V, residual)
def init_rot_from_imu_data(self):
## 初始化姿态，加速的方差不能太大
sum_acc = np.array([0., 0., 0.])
for imu_data in self.imu_buff:
sum_acc += imu_data.acc
mean_acc = sum_acc / len(self.imu_buff)
sum_err2 = np.array([0., 0., 0.])
for imu_data in self.imu_buff:
sum_err2 += np.power(imu_data.acc - mean_acc, 2)
std_acc = np.power(sum_err2 / len(self.imu_buff), 0.5)
if (np.max(std_acc) > 3.0):
print("acc std is too big !!")
return False
## 这里获得旋转矩阵的原理是？
z_axis = mean_acc / np.linalg.norm(mean_acc)
z_axis = z_axis.reshape((3, 1))
x_axis = np.array([1, 0, 0]).reshape(
(3, 1)) - z_axis.dot(z_axis.T).dot(
np.array([1, 0, 0]).reshape((3, 1)))
x_axis = x_axis / np.linalg.norm(x_axis)
y_axis = np.cross(z_axis.reshape(3), x_axis.reshape(3)).reshape(3, 1)
y_axis = y_axis / np.linalg.norm(y_axis)
r_IG = np.zeros((3, 3))
r_IG[:3, 0] = x_axis.reshape(3)
r_IG[:3, 1] = y_axis.reshape(3)
r_IG[:3, 2] = z_axis.reshape(3)
self.ekf.state.r_GI = r_IG.T ## 初始化姿态
return True
def pub_state(self):
if (self.p_G_Gps_ is None):
return
odom_msg = Odometry()
odom_msg.header.frame_id = fixed_id
odom_msg.header.stamp = rospy.Time.now()
odom_msg.pose.pose = Pose(
Point(self.ekf.state.p_GI[0], self.ekf.state.p_GI[1],
self.ekf.state.p_GI[2]), Quaternion(0, 0, 0, 1))
odom_msg.twist.twist = Twist(
Vector3(self.ekf.state.v_GI[0], self.ekf.state.v_GI[1],
self.ekf.state.v_GI[2]), Vector3(0, 0, 0))
self.pub_odom.publish(odom_msg)
pose_msg = PoseStamped()
pose_msg.header = odom_msg.header
pose_msg.pose = odom_msg.pose.pose
self.nav_path.header = pose_msg.header
self.nav_path.poses.append(pose_msg)
self.pub_path.publish(self.nav_path)
ukf_mses = []
ekf_mses = []
lstm_mses = []
lstm_stacked_mses = []
bar = ProgressBar()
for s in bar(range(num_sims)):
#x_0 = np.random.normal(0, x_var, 1)
x_0 = np.array([0])
sim = UNGM(x_0, R, Q, x_var)
ukf = UKF(sim.f, sim.F, sim.h, sim.H, sim.Q, sim.R, 5., first_x_0, 1)
ekf = EKF(sim.f, sim.F, sim.h, sim.H, sim.Q, sim.R, first_x_0, 1)
for t in range(T):
x, y = sim.process_next()
ukf.predict()
ukf.update(y)
ekf.predict()
ekf.update(y)
ukf_mses.append(MSE(ukf.get_all_predictions(), sim.get_all_x()))
ekf_mses.append(MSE(ekf.get_all_predictions(), sim.get_all_x()))
all_x.append(np.array(sim.get_all_x()))
all_y.append(np.array(sim.get_all_y()))
X = np.array(all_y)[:, :-1, :]
y = np.array(all_x)[:, 1:, :]
lstm10_pred = lstm10.predict(X)
lstm100_pred = lstm100.predict(X)
rnn10_pred = rnn10.predict(X)
rnn100_pred = rnn100.predict(X)
class ImuData:
def __init__(self):
self.timestamp = 0.0
self.acc = np.array([.0, .0, .0])
self.gyr = np.array([.0, .0, .0])
ekf = EKF(acc_n=1e-2, gyr_n=1e-4, acc_w=1e-6, gyr_w=1e-8)
last_imu = ImuData()
last_imu.timestamp = 1
curr_imu = ImuData()
curr_imu.timestamp = 1.1
curr_imu.acc = np.array([0.1, 0, 0.])
curr_imu.gyr = np.array([0.0, 0., 0])
ekf.predict(last_imu, curr_imu)
H = np.array([[1., 0., 0., 0., 0., 0., -0., -0., -0., 0., 0., 0., 0., 0., 0.],
[0., 1., 0., 0., 0., 0., -0., -0., -0., 0., 0., 0., 0., 0., 0.],
[0., 0., 1., 0., 0., 0., -0., -0., -0., 0., 0., 0., 0., 0., 0.]])
V = np.array([[
0.002304,
0.,
0.,
], [
0.,
0.00553536,
0.,
], [0., 0., 0.09884736]])
r = np.array([1.88561492, 0.08148084, 0.02500535])
ekf.update_measurement(H, V, r)
print("P_GI", ekf.state.p_GI)
measurement_range_variance = 0.3 * 0.3
measurement_angle_variance = 5.0 * math.pi / 180.0 * 5.0 * math.pi / 180.0
ekf = EKF(start_x, start_y, start_yaw)
ekf.add_landmark(2.0, 2.0)
ekf.add_landmark(4.0, -4.0)
ekf.add_landmark(-2.0, -2.0)
ekf.add_landmark(-4.0, 4.0)
ekf.set_odom_noises(5.0, 2.0, 2.0, 8.0)
ekf.set_measurement_variances(measurement_range_variance,
measurement_angle_variance)
ekf.set_min_trace(0.001)
ekf.set_plot_sizes(5.0, 5.0)
while True:
# simulate robot behaviors
robot_sim.update_pose(0.2, -0.2)
delta_dist = robot_sim.sim_v * robot_sim.sim_time_step
delta_yaw = robot_sim.sim_w * robot_sim.sim_time_step
measurements = robot_sim.get_sensor_measurements()
# ekf
ekf.predict(delta_dist, delta_yaw)
ekf.update(measurements)
ekf.print_estimated_pose()
ekf.print_estimated_covariance()
ekf.plot_ekf_world(measurements)
# sleep
time.sleep(robot_sim.sim_time_step)
def predict(self, u):
EKF.predict(self, u, self.Q, self.dt)
import pandas as pd
from init import get_data
from ekf import EKF
import matplotlib.pyplot as plt
import time
lin_acc, ang_vel, vicon_pose, t = get_data()
u_prev = np.concatenate([vicon_pose[0], np.zeros(9)], axis=0)
u_prev = np.expand_dims(u_prev, axis=1)
covar_prev = np.eye(15)
saved_states = np.zeros([15, len(t)])
ekf = EKF()
curr_time = time.time()
for i in range(len(t)):
if (i == 0):
dt = t[i]
covar_est, u_est = ekf.predict(u_prev, covar_prev, ang_vel[i],
lin_acc[i], dt)
z_t = np.expand_dims(vicon_pose[i], axis=1)
u_curr, covar_curr = ekf.update(z_t, covar_est, u_est)
saved_states[:, i] = np.squeeze(u_curr, axis=1)
out = u_curr
u_prev, covar_prev = u_curr, covar_curr
else:
print(time.time() - curr_time)
dt = t[i] - t[i - 1]
covar_est, u_est = ekf.predict(u_prev, covar_prev, ang_vel[i],
lin_acc[i], dt)
z_t = np.expand_dims(vicon_pose[i], axis=1)
u_curr, covar_curr = ekf.update(z_t, covar_est, u_est)
saved_states[:, i] = np.squeeze(u_curr, axis=1)
out = np.concatenate([out, u_curr], axis=1)
u_prev, covar_prev = u_curr, covar_curr
