Exemplo n.º 1
0
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)
Exemplo n.º 3
0
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)
Exemplo n.º 4
0
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)
Exemplo n.º 5
0
 def predict(self, u):
     EKF.predict(self, u, self.Q, self.dt)
Exemplo n.º 6
0
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