Example #1
0
    def test_motion_model_circular_right_1(self):
        '''
        Move from origin (0,0,0) to the RIGHT
        With v = 1, on a circle with R = 2.0 meters with dT = 1 seconds for 1 cycle.
        
        x = Rsin(v*dT/R) = 0.47
        y = Rcos(v*dT/R) = 0.94

        y needs a shifting with R after that
        '''

        x = 0.0
        y = 0.0
        o = 0.0
        v = 1.0
        a = 0.0
        s = 0.0  # R = 2
        dT = 1
        u = np.array([[a], [s]])

        kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT)

        R = get_icr_radius(s)

        x = kf.motion_model(u)

        self.assertLessEqual(kf.posx, R)
        self.assertLessEqual(kf.posy, 0)

        self.assertGreaterEqual(kf.posx, -R)
        self.assertGreaterEqual(kf.posy, -2 * R)

        if self.debug is True:
            print(kf.x)
Example #2
0
    def test_motion_model_circular_loop_left_5times(self):
        '''
        Move from origin (0,0,0) to the LEFT
        With v = 1, on a circle with R = 1.75 meters with dT = 1 seconds with 5 cycles
        '''
        x = 0.0
        y = 0.0
        o = 0.0
        v = 1.0
        a = 0.0
        s = 14.0  # R = 1.75
        u = np.array([[a], [s]])
        dT = 1.0

        kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT)

        R = get_icr_radius(s)

        for i in range(1, 6):
            x = kf.motion_model(u)

            # if self.debug is True:
            #     print(kf.x)

        self.assertLessEqual(kf.posx, R)
        self.assertLessEqual(kf.posy, 2 * R)

        self.assertGreaterEqual(kf.posx, -R)
        self.assertGreaterEqual(kf.posy, 0)

        if self.debug is True:
            print(kf.x)
Example #3
0
    def test_motion_model_circular_left_4(self):
        '''
        Move from origin (0,0,0) to the LEFT
        With v = 1, on a circle with R = 1.75 meters with dT = 4 seconds for 1 cycle.
        '''
        x = 0.0
        y = 0.0
        o = 0.0
        v = 1.0
        a = 0.0
        s = 14.0  # R = 1.75
        dT = 4
        u = np.array([[a], [s]])

        kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT)

        R = get_icr_radius(s)

        x = kf.motion_model(u)

        self.assertLessEqual(kf.posx, R)
        self.assertLessEqual(kf.posy, 2 * R)

        self.assertGreaterEqual(kf.posx, -R)
        self.assertGreaterEqual(kf.posy, 0)

        if self.debug is True:
            print(kf.x)
Example #4
0
    def test_motion_model_circular_right_0(self):
        '''
        Move from origin (0,0,0) to the RIGHT
        With v = PI, on a circle with R = 2.0 meters with dT = 1 seconds for 1 cycle.
        
        x = [2, -2, pi, pi]
        '''
        x = 0.0
        y = 0.0
        o = 0.0
        v = 0.0
        a = 0.0
        s = 0.0  # R = 2
        dT = 1
        u = np.array([[a], [s]])

        R = get_icr_radius(s)

        v = np.pi * R / 2

        kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT)

        x = kf.motion_model(u)

        self.assertLessEqual(kf.posx, R)
        self.assertLessEqual(kf.posy, 0)

        self.assertGreaterEqual(kf.posx, -R)
        self.assertGreaterEqual(kf.posy, -2 * R)

        self.assertAlmostEqual(kf.posx, R, None, None, 0.2)
        self.assertAlmostEqual(kf.posy, -R, None, None, 0.2)

        if self.debug is True:
            print(kf.x)
Example #5
0
    def __init__(self):
        self.icp = ICP()
        self.ekf = EKF()
        self.extraction = Extraction()

        # odom robot init states
        self.robot_x = rospy.get_param('/icp/robot_x',0)
        self.robot_y = rospy.get_param('/icp/robot_y',0)
        self.robot_theta = rospy.get_param('/icp/robot_theta',0)
        self.sensor_sta = [self.robot_x,self.robot_y,self.robot_theta]
        self.isFirstScan = True
        self.src_pc = []
        self.tar_pc = []

        # State Vector [x y yaw]
        self.xOdom = np.zeros((3,1))
        self.xEst = np.zeros((3,1))
        self.PEst = np.eye(3)
        
        # map observation
        self.obstacle = []
        # radius
        self.obstacle_r = 10

        # init map
        self.updateMap()
        # ros topic
        self.laser_sub = rospy.Subscriber('/course_agv/laser/scan',LaserScan,self.laserCallback)
        self.location_pub = rospy.Publisher('ekf_location',Odometry,queue_size=3)
        self.odom_pub = rospy.Publisher('icp_odom',Odometry,queue_size=3)
        self.odom_broadcaster = tf.TransformBroadcaster()
        self.landMark_pub = rospy.Publisher('/landMarks',MarkerArray,queue_size=1)
class LocalizationNode(object):
    # model in meters
    COEFFS = [-0.1416, 2.311, -14.76, 36.79]

    EVERY_MINUTE = 0.1667
    EVERY_SECOND = 1

    def __init__(self):
        rospy.init_node("position_publisher", anonymous=True)
        self.rate = rospy.Rate(LocalizationNode.EVERY_SECOND)
        self.extractParams()

        self.frame = "/target_" + str(self.tag)
        self.publisher = rospy.Publisher(self.frame, PointStamped, queue_size=10)
        self.model = Poly3(LocalizationNode.COEFFS)
        self.selector = BasestationSelector(self.basestations)
        self.ekf = EKF(self.tag, 1, 0.1, self.basestations, self.model, self.selector)
        self.miss_counter = 2

    def extractParams(self):
        self.tag = rospy.get_param("/localization_node/tag")
        stations = rospy.get_param("/localization_node/basestations")
        self.buildBasestation(stations)

    def buildBasestation(self, stations):
        self.basestations = []
        for station in stations:
            self.basestations.append(Basestation(station[0], float(station[1]), float(station[2])))

    def publishPosition(self, position):
        msg = PointStamped()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = self.frame

        msg.point = Point(position[0], position[1], 0)
        self.publisher.publish(msg)

    def localizationLoop(self):
        while not rospy.is_shutdown():
            try:
                if self.miss_counter >= 2:  # after two misses it is assumed the target is not in range
                    self.rate = rospy.Rate(LocalizationNode.EVERY_MINUTE)  # once in a minute
                    position = self.ekf.setInitialPositionToCloserBasestation()
                else:
                    position = self.ekf.ekfIteration()

                self.publishPosition(position)
                self.rate = rospy.Rate(LocalizationNode.EVERY_SECOND)  # normal rate
                self.miss_counter = 0

            except NoMeasurementException:  # no measurements means the target is not in range
                self.miss_counter += 1

            self.rate.sleep()
Example #7
0
 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
Example #8
0
    def __init__(self, host='localhost', port=4567):
        # Connection details
        self._host = host
        self._port = port

        self.ekf = EKF()

        # Server callbacks definition
        self._sio = socketio.AsyncServer()
        self._sio.on('connect', self._on_sim_connect)
        self._sio.on('disconnect', self._on_sim_disconnect)
        self._sio.on('telemetry', self._on_telemetry)

        # History stacking
        self.estimations_history = []
        self.groundtruth_history = []
def test():
    from math import pi
    import numpy
    # pose in frame A
    a = EKF.column_vector(1, 2, pi / 2)
    # pose in frame B
    b = EKF.column_vector(1, 1, 0)
    # actual origin of B in A
    B = EKF.column_vector(2, 1, pi / 2)
    # calculated origin of B in A
    BB = EKF.get_offset(a, b)
    # use it to convert b into a
    aa = EKF.coord_transform(b, BB)
    # assert that things match
    assert numpy.equal(B, BB).all()
    assert numpy.equal(a, aa).all()
Example #10
0
 def __init__(self, t):
     self.dt = t
     self.R = np.diag([params.sigma_r**2, params.sigma_theta**2])
     self.lm_filters = [
         [EKF(params.dt, i) for i in range(params.num_lms)]
         for i in range(params.M)
     ]  #List of lists. Inside list is EKF for each LM. Outer list is each particle
Example #11
0
def test():
    from math import pi
    import numpy
    # pose in frame A
    a = EKF.column_vector(1, 2, pi / 2)
    # pose in frame B
    b = EKF.column_vector(1, 1, 0)
    # actual origin of B in A
    B = EKF.column_vector(2, 1, pi / 2)
    # calculated origin of B in A
    BB = EKF.get_offset(a, b)
    # use it to convert b into a
    aa = EKF.coord_transform(b, BB)
    # assert that things match
    assert numpy.equal(B, BB).all()
    assert numpy.equal(a, aa).all()
Example #12
0
    def __init__(self, b1, b2, b3, dt):
        EKF.__init__(self, 6, self.f, self.F, self.h, self.H)
        self.b1 = b1
        self.b2 = b2
        self.b3 = b3
        self.dt = dt

        # self.Q = PROCESS_NOISE_VARIANCE * np.eye(6)
        w_psd = PROCESS_NOISE_VARIANCE
        self.Q = np.array([[0, 0, 0, -0.5 * dt**2 * w_psd, 0, 0],
                           [0, 0, 0, 0, -0.5 * dt**2 * w_psd, 0],
                           [0, 0, 0, 0, 0, -0.5 * dt**2 * w_psd],
                           [0, 0, 0, -0.5 * dt**3 * w_psd + dt * w_psd, 0, 0],
                           [0, 0, 0, 0, -0.5 * dt**3 * w_psd + dt * w_psd, 0],
                           [0, 0, 0, 0, 0, -0.5 * dt**3 * w_psd + dt * w_psd]
                           ]) + w_psd * np.eye(self.n)
    def __init__(self, b1, b2, b3, dt):
        EKF.__init__(self, 6, self.f, self.F, self.h, self.H)
        self.b1 = b1
        self.b2 = b2
        self.b3 = b3
        self.dt = dt

        # self.Q = PROCESS_NOISE_VARIANCE * np.eye(6)
        w_psd = PROCESS_NOISE_VARIANCE
        self.Q = np.array([
                    [0, 0, 0, -0.5*dt**2*w_psd, 0, 0],
                    [0, 0, 0, 0, -0.5*dt**2*w_psd, 0],
                    [0, 0, 0, 0, 0, -0.5*dt**2*w_psd],
                    [0, 0, 0, -0.5*dt**3*w_psd + dt*w_psd, 0, 0],
                    [0, 0, 0, 0, -0.5*dt**3*w_psd + dt*w_psd, 0],
                    [0, 0, 0, 0, 0, -0.5*dt**3*w_psd + dt*w_psd]
                ]) + w_psd * np.eye(self.n)
    def __init__(self):
        rospy.init_node("position_publisher", anonymous=True)
        self.rate = rospy.Rate(LocalizationNode.EVERY_SECOND)
        self.extractParams()

        self.frame = "/target_" + str(self.tag)
        self.publisher = rospy.Publisher(self.frame, PointStamped, queue_size=10)
        self.model = Poly3(LocalizationNode.COEFFS)
        self.selector = BasestationSelector(self.basestations)
        self.ekf = EKF(self.tag, 1, 0.1, self.basestations, self.model, self.selector)
        self.miss_counter = 2
Example #15
0
def main():
    global ekf, pose_pub
    rospy.init_node("localization")
    ekf = EKF()
    pose_pub = rospy.Publisher("pose", Pose)
    rospy.Subscriber("odom", Odometry, odom_callback)
    # odom_combined comes from robot_pose_ekf, different message type from
    # regular odom, but seems to have the same basic contents, so we can use
    # the same callback (yay dynamic typing!)
    #rospy.Subscriber("odom_combined", PoseWithCovarianceStamped, odom_callback)
    #rospy.Subscriber("/corobot_pose_ekf/odom_semicombined", PoseWithCovarianceStamped, odom_callback)
    #rospy.Subscriber("laser_pose", Pose, laser_callback)
    rospy.Subscriber("qrcode_pose", Pose, qrcode_callback)
    rospy.spin()
Example #16
0
    def test_motion_model_straight(self):
        '''
        Check motion model for straight forward movement.
        '''
        x = 0.0
        y = 0.0
        o = 0.0
        v = 0.1
        a = 0.0
        # 7.0 means straight forward
        s = 7.0
        dT = 1

        u = np.array([[a], [s]])

        kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT)

        x = kf.motion_model(u)

        self.assertEqual(kf.posx, 0.1)
        self.assertEqual(kf.posy, 0.0)
        self.assertEqual(kf.yaw, o)
        self.assertEqual(kf.vel, v)
Example #17
0
    def test_init_ekf(self):
        '''
        Create an instance of the EKF class and initialize it.
        '''
        x = 0.0
        y = 0.0
        o = 0.0
        v = 0.1

        kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=1)

        self.assertEqual(kf.posx, x)
        self.assertEqual(kf.posy, y)
        self.assertEqual(kf.yaw, o)
        self.assertEqual(kf.vel, v)
Example #18
0
def main():
    global pose_pub, ekf, particles, map, num_particles
    rospy.init_node("localization")
    rospy.wait_for_service('get_map')
    map_srv = rospy.ServiceProxy('get_map', GetCoMap)
    map = map_srv().map
    roslib.load_manifest("corobot_localization")
    ekf = EKF()
    pose_pub = rospy.Publisher("pose", Pose)
    num_particles = 500
    particles = []
    if len(particles) == 0:
        rospy.Subscriber("qrcode_pose", Pose, pf_initialize)
    if len(particles) > 0:
        rospy.Subscriber("odom", Odometry, prediction)
        rospy.Subscriber("scan", LaserScan, update_model)
    rospy.spin()
Example #19
0
 def setUp(self):
     self.empty_ekf = EKF(landmark_threshold=0)
Example #20
0
    initial_transform = vehicle.actor_list[0].get_transform()
    initial_rotation = initial_transform.rotation
    initial_location = initial_transform.location

    ## Convert rotation and location to right-handed
    r_right_handed = R.from_matrix(
        carla_rotation_to_numpy_rotation_matrix(initial_rotation))
    t_right_handed = np.array(
        [initial_location.x, initial_location.y * -1, initial_location.z]).T

    ## Convert the right-handed rotation to a quaternion, roll it to get the form w,x,y,z from x,y,z,w
    initial_quat = np.roll(r_right_handed.as_quat(), 1)

    ## Initialize the EKF system #TODO check initial values
    vio_ekf = EKF(np.array([0, 0, 0]),
                  np.array([0, 0, 0]),
                  initial_quat,
                  debug=False)
    vio_ekf.use_new_data = False

    # --- Set instrument noise parameters
    vio_ekf.setSigmaAccel(0.0)
    vio_ekf.setSigmaGyro(0.0)

    ### Location transform ###############################################
    H_local_to_global = np.eye(4)
    H_local_to_global[:3, :3] = r_right_handed.as_matrix()
    H_local_to_global[:3, 3] = t_right_handed
    H_global_to_local = np.linalg.inv(H_local_to_global)

    r_right_handed = R.from_matrix(H_global_to_local[:3, :3])
    t_right_handed = H_global_to_local[:3, 3]
Example #21
0
#!/usr/bin/env python
import rospy
import sys
import os
import numpy as np
from std_msgs.msg import String
from nav_msgs.msg import Odometry
from marker_msgs.msg import MarkerDetection
from tf.transformations import euler_from_quaternion
sys.path.insert(1, '/home/ros/catkin_ws/src/moro_g12/src/')
from ekf import EKF
from csv_writer import csv_writer

previous_timestamp = None
ekf = EKF(3, 2, 2)
beacons = [(7.3, 3), (1, 1), (9, 9), (1, 8), (5.8, 8)]  # xy coordinates


def get_rotation(quaternion):
    global roll, pitch, yaw
    orientation_list = [quaternion.x, quaternion.y, quaternion.z, quaternion.w]
    (roll, pitch, yaw) = euler_from_quaternion(orientation_list)
    return wrap_to_pi(yaw)


def odom_callback(msg):
    timestamp = msg.header.stamp
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y

    theta = get_rotation(msg.pose.pose.orientation)  # radians
Example #22
0
import random
#from matplotlib.pyplot import *
#from mpl_toolkits.mplot3d import Axes3D
from path_finding import PATH
from median_filter_special import myfilter
from cost_matrix import COSTMtrix
from cost_matrix import Window_LEN, Overall_shiftting_WinLen, Standard_LEN
from scipy.ndimage import gaussian_filter1d
from time import time
import scipy.io
from parrallel_thread import Dual_thread_Overall_shift_NURD
from shift_deploy import Shift_Predict
from basic_trans import Basic_oper
from Path_post_pcs import PATH_POST
from ekf import EKF
myekf = EKF()

device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu")
Resample_size = Window_LEN
R_len = 20

#read_start = 100
read_start = 0

Debug_flag = True
global intergral_flag
intergral_flag = 0

Branch_flag = 0  # 0 fusion, 1 A, 2 B

if (Save_signal_flag == True):
 def predict(self, u):
     EKF.predict(self, u, self.Q, self.dt)
Example #24
0
        z[0, i] = r + np.random.normal(0, params.sigma_r)
        z[1, i] = theta + np.random.normal(0, params.sigma_theta)
        z[1, i] = unwrap(z[1, i])

    return z


if __name__ == "__main__":
    read_file = True
    if read_file:
        t, v, w, x, y, psi = readFile()
        vc, wc = generateVelocities(t)

    Car = CarAnimation()
    ekf = EKF(params.dt)

    x_hist = []
    mu_hist = []
    err_hist = []
    x_covar_hist = []
    y_covar_hist = []
    psi_covar_hist = []
    K_hist = []

    x0 = params.x0
    y0 = params.y0
    phi0 = params.theta0
    state = np.array([x[0], y[0], psi[0]])
    dead_reckon = np.array([x0, y0, phi0])
    mu = np.array([x0, y0, phi0])
        [twr.Getx()[1],
         twr.Getx()[1] + body_radius * np.sin(twr.Getx()[2])])
    robot_head.set_xdata(headx)
    robot_head.set_ydata(heady)

    fig.canvas.draw()
    plt.pause(0.01)


if __name__ == "__main__":

    # Two-Wheeled Robot Init
    twr = TWR()  # TWR model object

    # Kalman Filter Init
    ekf = EKF(twr.c, twr.nl)

    body_radius = 0.3
    fig, lines, lines_est, msensor, robot_body, robot_head = InitPlot(
        twr, body_radius)
    mu = ekf.mu
    K = ekf.K
    two_sig_x = np.array([[2 * np.sqrt(ekf.cov.item((0, 0)))],
                          [-2 * np.sqrt(ekf.cov.item((0, 0)))]])
    two_sig_y = np.array([[2 * np.sqrt(ekf.cov.item((1, 1)))],
                          [-2 * np.sqrt(ekf.cov.item((1, 1)))]])
    two_sig_theta = np.array([[2 * np.sqrt(ekf.cov.item((2, 2)))],
                              [-2 * np.sqrt(ekf.cov.item((2, 2)))]])
    for i in range(int(twr.t_end / twr.dt)):
        # truth model updates
        twr.Propagate()
Example #26
0
    trajectory_vo = [np.array([0, 0, 0])]

    # initialize first ground truth position
    # transform from global frame to robot start frame
    initial_pos = H_global_to_start @ np.append(t_right_handed, 1)
    trajectory_gt = [initial_pos]

    # EKF Initialization #TODO #########################################################
    # Convert the right-handed rotation to a quaternion, roll it to get the form w,x,y,z from x,y,z,w
    initial_quat_wxyz = np.roll(r_right_handed.as_quat(), 1)
    # initial_quat_wxyz = np.roll(R.from_matrix(car_start_r).as_quat(), 1)

    ## Initialize the EKF system #TODO check initial values
    # vio_ekf = EKF(np.array([0,0,0]), np.array([0, 0, 0]), initial_quat_wxyz, debug=False)
    vio_ekf = EKF(np.array([0, 0, 0]),
                  np.array([0, 0, 0]),
                  np.array([1, 0, 0, 0]),
                  debug=False)
    vio_ekf.use_new_data = True
    vio_ekf.setSigmaAccel(1.)
    vio_ekf.setSigmaGyro(0.5)
    vio_ekf.setSigmaVO(100.)

    accel_list = []
    gyro_list = []

    # Main Loop #############################################################
    max_length = 500
    first = True
    step = 0
    while True:
        step += 1
Example #27
0
class KalmanFilterProxy:
    def __init__(self, host='localhost', port=4567):
        # Connection details
        self._host = host
        self._port = port

        self.ekf = EKF()

        # Server callbacks definition
        self._sio = socketio.AsyncServer()
        self._sio.on('connect', self._on_sim_connect)
        self._sio.on('disconnect', self._on_sim_disconnect)
        self._sio.on('telemetry', self._on_telemetry)

        # History stacking
        self.estimations_history = []
        self.groundtruth_history = []

    def run(self):
        # Application creation
        app = web.Application()
        self._sio.attach(app)

        # Run app itself
        web.run_app(app, host=self._host, port=self._port)

    @staticmethod
    def _on_sim_connect(sid, environ):
        print('Connected: ', sid)

    @staticmethod
    def _on_sim_disconnect(sid):
        print('Disconnected: ', sid)

    async def _on_telemetry(self, sid, data):
        # Visualize data received data
        print('Telemetry received: ', data)

        # Communication
        return_msg = dict()
        if data is not None:

            meas_list = str.split(data['sensor_measurement'])

            # Parse data accordingly to sensor type
            if meas_list[0] == "L":
                measurement = Measurement(
                    sensor=Sensor.LIDAR,
                    data=np.array([float(x) for x in meas_list[1:3]]),
                    ts_us=int(meas_list[3]))
                groundtruth = [float(x) for x in meas_list[4:8]]

            elif meas_list[0] == "R":

                measurement = Measurement(
                    sensor=Sensor.RADAR,
                    data=np.array([float(x) for x in meas_list[1:4]]),
                    ts_us=int(meas_list[4]))
                groundtruth = [float(x) for x in meas_list[5:9]]

            else:
                raise ValueError('Not correct sensor type')

            self.ekf.process_measurement(measurement)
            estimation = self.ekf.get_estimation()

            # Stack history of estimation vs groundtruth
            self.estimations_history.append(estimation)
            self.groundtruth_history.append(groundtruth)

            # Calculate RMSE
            estimation_rmse = rmse(self.estimations_history,
                                   self.groundtruth_history)

            # Create message
            return_msg['estimate_x'] = float(estimation[0])
            return_msg['estimate_y'] = float(estimation[1])
            return_msg['rmse_x'] = estimation_rmse[0]
            return_msg['rmse_y'] = estimation_rmse[1]
            return_msg['rmse_vx'] = estimation_rmse[2]
            return_msg['rmse_vy'] = estimation_rmse[3]

            # Message sending
            print("Sending data: ", return_msg)
            await self._sio.emit('estimate_marker', return_msg)

        else:
            # If data is not valid, send dummy response
            await self._sio.emit('manual', {})
Example #28
0
import animation
reload(animation)
from animation import Animation

import plotData
reload(plotData)
from plotData import plotData

import ekf
reload(ekf)
from ekf import EKF

dynamics = Dynamics()
animation = Animation()
dataPlot = plotData()
ekf = EKF()

t = P.t_start
while t < P.t_end:
    t_next_plot = t + P.t_plot
    while t < t_next_plot:
        t = t + P.Ts
        vc = 1 + 0.5 * np.cos(2 * np.pi * (0.2) * t)
        omegac = -0.2 + 2 * np.cos(2 * np.pi * (0.6) * t)
        noise_v = vc + np.random.normal(
            0, np.sqrt(P.alpha1 * vc**2 + P.alpha2 * omegac**2))
        noise_omega = omegac + np.random.normal(
            0, np.sqrt(P.alpha3 * vc**2 + P.alpha4 * omegac**2))

        u = [noise_v, noise_omega]
        dynamics.propagateDynamics(u)
Example #29
0
class TestEKF(unittest.TestCase):
    def setUp(self):
        self.empty_ekf = EKF(landmark_threshold=0)

    def tearDown(self):
        del self.empty_ekf

    def test_init(self):
        self.assertIsInstance(self.empty_ekf, EKF)
        self.assertEqual(self.empty_ekf.landmark_types, [])
        self.assertEqual(self.empty_ekf.landmark_counts, [])

        np.testing.assert_array_equal(self.empty_ekf.system_state,
                                      np.array([[0], [0], [0]]))
        np.testing.assert_array_equal(
            self.empty_ekf.covariance,
            np.array([[0.95, 0, 0], [0, 0.95, 0], [0, 0, 0.95]]))

    def test_noop_move(self):
        # Run the move function, but without actually moving.
        self.empty_ekf.update(radians(0), 0, [])

        np.testing.assert_array_equal(self.empty_ekf.pos(),
                                      np.array([[0], [0], [0]]))
        np.testing.assert_array_equal(
            self.empty_ekf.covariance,
            np.array([[0.95, 0, 0], [0, 0.95, 0], [0, 0, 0.95]]))

    def test_simple_move(self):
        # Turn left 30 degrees and advance 10 units of distance.
        self.empty_ekf.update(radians(30), 10, [])

        np.testing.assert_almost_equal(
            self.empty_ekf.pos(), np.array([[8.66025404], [5], [0.52359878]]))
        np.testing.assert_almost_equal(
            self.empty_ekf.covariance,
            np.array([[75.95, -38.9711432, -8.0005164],
                      [-38.9711432, 25.95, 4.8808997],
                      [-8.0005164, 4.8808997, 0.9637078]]))

    def test_linear_move_and_observe(self):
        # Add a landmark at the start.
        self.empty_ekf.update(radians(0), 0, [(20, 0, 'statue')])

        np.testing.assert_array_equal(self.empty_ekf.pos(),
                                      np.array([[0], [0], [0]]))
        np.testing.assert_array_equal(self.empty_ekf.system_state,
                                      np.array([[0], [0], [0], [20], [0]]))
        np.testing.assert_array_equal(
            self.empty_ekf.covariance,
            np.array([[0.95, 0, 0, 0.95, 0], [0, 0.95, 0, 0, 0.95],
                      [0, 0, 0.95, 0, 0], [0.95, 0, 0, 1.15, 0],
                      [0, 0.95, 0, 0, 0.95]]))

        # Move 5 units, but imply some error by landmark drift.
        self.empty_ekf.update(radians(0), 5, [(19, 0, 'statue')])

        np.testing.assert_almost_equal(
            self.empty_ekf.pos(), np.array([[5.8274554], [0], [-0.042244909]]))
        np.testing.assert_almost_equal(
            self.empty_ekf.system_state,
            np.array([[5.8274554], [0], [-0.042244909], [19.9014031], [0]]))
        np.testing.assert_almost_equal(
            self.empty_ekf.covariance,
            np.array([[25.95, 0, -4.75, 0.95, 0], [0, 0.95, 0, 0, 0.95],
                      [-4.75, 0, 0.95, 0, 0], [0.95, 0, 0, 1.15, 0],
                      [0, 0.95, 0, 0, 0.95]]))

    def test_rotate_and_observe(self):
        # Add a landmark at the start.
        self.empty_ekf.update(radians(0), 0, [(20, 0, 'statue')])

        np.testing.assert_array_equal(self.empty_ekf.pos(),
                                      np.array([[0], [0], [0]]))
        np.testing.assert_array_equal(self.empty_ekf.system_state,
                                      np.array([[0], [0], [0], [20], [0]]))
        np.testing.assert_array_equal(
            self.empty_ekf.covariance,
            np.array([[0.95, 0, 0, 0.95, 0], [0, 0.95, 0, 0, 0.95],
                      [0, 0, 0.95, 0, 0], [0.95, 0, 0, 1.15, 0],
                      [0, 0.95, 0, 0, 0.95]]))

        # Turn 30 degrees, but imply some error by landmark drift.
        self.empty_ekf.update(
            radians(30), 0,
            [(20 * cos(radians(-2)), 20 * sin(radians(-2)), 'statue')])

        np.testing.assert_almost_equal(self.empty_ekf.pos(),
                                       np.array([[0], [0], [0.55788443]]))
        np.testing.assert_almost_equal(
            self.empty_ekf.system_state,
            np.array([[0], [0], [0.55788443], [20], [0]]))
        np.testing.assert_almost_equal(
            self.empty_ekf.covariance,
            np.array([[0.95, 0, 0, 0.95, 0], [0, 0.95, 0, 0, 0.95],
                      [0, 0, 0.9637078, 0, 0], [0.95, 0, 0, 1.15, 0],
                      [0, 0.95, 0, 0, 0.95]]))

    def test_add_two_landmarks(self):
        # Add first landmark.
        self.empty_ekf.update(radians(0), 0, [(20, 0, 'statue')])

        np.testing.assert_array_equal(self.empty_ekf.pos(),
                                      np.array([[0], [0], [0]]))
        np.testing.assert_array_equal(self.empty_ekf.system_state,
                                      np.array([[0], [0], [0], [20], [0]]))
        np.testing.assert_array_equal(
            self.empty_ekf.covariance,
            np.array([[0.95, 0, 0, 0.95, 0], [0, 0.95, 0, 0, 0.95],
                      [0, 0, 0.95, 0, 0], [0.95, 0, 0, 1.15, 0],
                      [0, 0.95, 0, 0, 0.95]]))

        # Add second landmark.
        self.empty_ekf.update(radians(0), 0, [(0, 20, 'fountain')])

        np.testing.assert_array_equal(self.empty_ekf.pos(),
                                      np.array([[0], [0], [0]]))
        np.testing.assert_array_equal(
            self.empty_ekf.system_state,
            np.array([[0], [0], [0], [20], [0], [0], [20]]))
        np.testing.assert_array_equal(
            self.empty_ekf.covariance,
            np.array([[0.95, 0, 0, 0.95, 0, 0.95, 0],
                      [0, 0.95, 0, 0, 0.95, 0, 0.95], [0, 0, 0.95, 0, 0, 0, 0],
                      [0.95, 0, 0, 1.15, 0, 0.95, 0],
                      [0, 0.95, 0, 0, 0.95, 0, 0.95],
                      [0.95, 0, 0, 0.95, 0, 1.15, 0],
                      [0, 0.95, 0, 0, 0.95, 0, 0.95]]))
Example #30
0
#!/usr/bin/env python3
import os, sys
sys.path.append("/home/jc/Downloads/test_ws/src/imu_gnss_fusion/src")

from ekf import EKF
import numpy as np


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.,
], [
Example #31
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)
Example #32
0
robot_sim.add_landmark(4.0, -4.0)
robot_sim.add_landmark(-2.0, -2.0)
robot_sim.add_landmark(-4.0, 4.0)
robot_sim.set_odom_noises(0.33, 0.1, 0.1, 0.33)
robot_sim.set_max_measurement_range(max_measurement_range)
robot_sim.set_measurement_variances(measurement_range_variance_sim,
                                    measurement_angle_variance_sim)
robot_sim.set_plot_sizes(max_measurement_range, max_measurement_range)
robot_sim.set_random_measurement_rate(0.05)
robot_sim.set_sim_time_step(0.1)

# ekf parameters
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
Example #33
0
    ind = np.argwhere(np.abs(z[1]) < params.fov)
    z = z[:, ind][:, :, 0]

    return z, ind


if __name__ == "__main__":
    t = np.arange(0, params.tf, params.dt)
    vc, wc = generateVelocities(t)
    v = vc + np.sqrt(params.alpha1 * vc**2 +
                     params.alpha2 * wc**2) * np.random.randn(vc.size)
    w = wc + np.sqrt(params.alpha3 * vc**2 +
                     params.alpha4 * wc**2) * np.random.randn(wc.size)

    Car = CarAnimation()
    ekf = EKF(params.dt)

    x_hist = []
    mu_hist = []
    err_hist = []
    x_covar_hist = []
    y_covar_hist = []
    psi_covar_hist = []
    K_hist = []

    state = np.zeros(3)
    dead_reckon = np.zeros(3)
    mu = ekf.mu
    Sigma = ekf.Sigma

    for i in range(t.size):
    training_size, T))

all_y = []
all_x = []

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:, :]