示例#1
0
    def EKF_estimate2(self, imu_file=None, camera_file=None):
        import sensor_fusion as sf
        import robot_n_measurement_functions as rnmf

        camera_file = 'Datasets/data/task6/camera_tracking_task6.csv'
        imu_file = 'Datasets/data/task6/imu_tracking_task6.csv'

        IMU = pd.read_csv(imu_file, header=None)
        print('IMU ', np.shape(IMU))
        t_robot = IMU.iloc[:, 0]
        GyZ = IMU.iloc[:, 8]
        w = np.array(GyZ).flatten()
        cam = pd.read_csv(camera_file, header=None)
        print('cam ', np.shape(cam))

        self.Camera = sf.Sensor('Camera',
                                sf.CAMERA_COLUMNS,
                                meas_record_file=camera_file,
                                is_linear=False,
                                start_index=0)
        self.x_true = np.array([15.7, 47.5, 90.0])
        self.x_init = self.x_true
        x = np.zeros((self.Camera.meas_record.shape[0] // 3, 3),
                     dtype=np.float)
        print('x shape is ', np.shape(x))
        self.Camera.reset_sampling_index()
        i, vel = 0, 5.5
        landmarks, inputs = [], []
        while (self.Camera.current_sample_index < self.Camera.time.shape[0]
               and i < x.shape[0] - 1):
            i += 1
            y_ = self.Camera.get_measurement()
            if y_.shape[0] < 2:
                continue

            qr_row = y_[:, 0].astype('int')
            camera_time = self.Camera.current_time
            landmark = rnmf.QRCODE_LOCATIONS[qr_row, 1:]
            closest_index = IMU.iloc[:, 0].sub(camera_time).abs().idxmin()
            closest_u = np.array(IMU.iloc[closest_index, 8])
            #print('qr_row len:{}, qr_row:{}, time:{}, landmarks:{},closest_u:{}'.format(np.shape(qr_row),qr_row,camera_time, np.shape(landmark),closest_u))

            u = [vel,
                 closest_u]  # steering command (vel, steering angle radians)
            inputs.append(u)
            landmarks.append(landmark)

        print('landmarks:{},  inputs:{}'.format(np.shape(landmarks),
                                                np.shape(inputs)))
        '''[plt.scatter(landmark[:, 0], landmark[:, 1], marker='s', s=60) for landmark in landmarks]
        phi = np.deg2rad(self.x_init[-1])
        plt.quiver(self.x_init[0], self.x_init[1], np.cos(phi), np.sin(phi),
                   linewidth=2., alpha=1.0, color='black', label='init-pose')

        plt.grid(True)
        plt.legend()
        plt.axis('equal')
        plt.show()'''

        return landmarks, inputs
    def __init__(self):
        self.index = 0
        camera_file = 'Datasets/data/task6/camera_tracking_task6.csv'
        imu_file = 'Datasets/data/task6/imu_tracking_task6.csv'
        IMU = pd.read_csv(imu_file, header=None)
        print('IMU ', np.shape(IMU))

        self.Camera = sf.Sensor('Camera',
                                sf.CAMERA_COLUMNS,
                                meas_record_file=camera_file,
                                is_linear=False,
                                start_index=0)
        print('Camera ', self.Camera.meas_record.shape[0])
        x = np.zeros((self.Camera.meas_record.shape[0] // 3, 3),
                     dtype=np.float)
        self.Camera.reset_sampling_index()
        i, vel = 0, 5.5
        vel /= 2.25
        landmarks, inputs = [], []
        while (self.Camera.current_sample_index < self.Camera.time.shape[0]
               and i < x.shape[0] - 1):
            i += 1
            y_ = self.Camera.get_measurement()
            if y_.shape[0] < 1:
                continue
            qr_row = y_[:, 0].astype('int')
            camera_time = self.Camera.current_time
            landmark = rnmf.QRCODE_LOCATIONS[qr_row, 1:]
            closest_index = IMU.iloc[:, 0].sub(camera_time).abs().idxmin()
            closest_u = np.array(IMU.iloc[closest_index, 8])
            # print('qr_row len:{}, qr_row:{}, time:{}, landmarks:{},closest_u:{}'.format(np.shape(qr_row),qr_row,camera_time, np.shape(landmark),closest_u))

            w_gyro = np.deg2rad(closest_u)
            u = [vel, w_gyro]  # steering command (vel, steering angle radians)
            inputs.append(u)
            landmarks.append(landmark)

        self.DT = .55
        print('landmarks:{},  inputs:{}'.format(np.shape(landmarks),
                                                np.shape(inputs)))

        self.landmarks = landmarks
        self.inputs = np.array(inputs)
        self.N = len(inputs)
示例#3
0
import numpy as np
import matplotlib.pyplot as plt
import sensor_fusion as sf
import robot_n_measurement_functions as rnmf
import pathlib
import seaborn as sns
import matplotlib.patches as mpatches
from scipy.linalg import expm
sns.set()
#%%
n_qr_codes_min = 3
deviation_max = 5e2

parent_path = pathlib.Path.home()
parent_path = parent_path/'Dropbox/09. Aalto Postdoc/DiddyBorg_experiment'
IMU = sf.Sensor('IMU',sf.IMU_COLUMNS,meas_record_file=parent_path/'test-run-imu.csv',is_linear=True,start_index=3686)
IMU_static = sf.Sensor('IMU_static',sf.IMU_COLUMNS,meas_record_file=parent_path/'static_position_IMU.csv',is_linear=True)
Motor_input = sf.Sensor('Motor_input',sf.MOTOR_COLUMNS,meas_record_file=parent_path/'test-run-Motor-Control.csv',is_linear=True)
Camera = sf.Sensor('Camera',sf.CAMERA_COLUMNS,meas_record_file=parent_path/'test-run-camera.csv',is_linear=False,start_index=154)
#%%

bias_omega_z = np.mean(IMU_static.meas_record[:,7])
x_init = np.array([17,60,0.])
x = np.zeros((Motor_input.meas_record.shape[0]+IMU.meas_record.shape[0],3),dtype=np.float)
x[0,:] = x_init
t = np.zeros(x.shape[0])
t[0] = min(IMU.time[0],Motor_input.time[0],Camera.time[0])

u_now = np.zeros(3)
# #initialize deltaT as IMU deltaT
# IMU_measurement = IMU.get_measurement() #time change here
示例#4
0
import numpy as np
import matplotlib.pyplot as plt
import sensor_fusion as sf
import robot_n_measurement_functions as rnmf
import pathlib
import seaborn as sns
import matplotlib.patches as mpatches
from scipy.linalg import expm
import lsqSolve as lsqS

#%%
parent_path = pathlib.Path('/media/muhammad/Data/')
parent_path = parent_path / 'Dropbox/09. Aalto Postdoc/DiddyBorg_experiment'
Camera = sf.Sensor('Camera',
                   sf.CAMERA_COLUMNS,
                   meas_record_file=parent_path / 'test-run-camera.csv',
                   is_linear=False,
                   start_index=154)
#%%
df = pd.read_csv(
    parent_path / 'test-run-camera.csv',
    delimiter=',',
    index_col=0,
)

df.index = pd.to_datetime(df.index,
                          dayfirst=True,
                          errors='coerce',
                          unit='s',
                          utc=True)
import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
import sensor_fusion as sf
import robot_n_measurement_functions as rnmf
import lsqSolve as lsqS

camera_file = 'Datasets/data/task5/camera_localization_task5.csv'
# time, qr number, Cx, Cy, w,h, di, phi
Camera = sf.Sensor('Camera',
                   sf.CAMERA_COLUMNS,
                   meas_record_file=camera_file,
                   is_linear=False,
                   start_index=0)

x_true = np.array([61.0, 33.9, 0.0])
x_true = np.array([61.0, 33.9, 90.0])

x_init = np.array([0, 0, 0])  # x_true + np.random.randn(3)

R_one_diag = np.array([2, 10])

# params = {'x_sensors':np.zeros((1,2))}
I_max = 10
gamma = 1
params_LSQ = {
    'x_sensors': None,
    'R': None,
    'LR': None,
    # cholesky factorization of a matrix (chol(a) in matlab returns an upper triangular matrix, but linalg.cholesky(a) returns a lower triangular matrix)
    'Rinv': None,
'''parent_path = pathlib.Path.home()
parent_path = parent_path/'Dropbox/09. Aalto Postdoc/DiddyBorg_experiment'
IMU = sf.Sensor('IMU',sf.IMU_COLUMNS,meas_record_file=parent_path/'test-run-imu.csv',is_linear=True,start_index=3686)
IMU_static = sf.Sensor('IMU_static',sf.IMU_COLUMNS,meas_record_file=parent_path/'static_position_IMU.csv',is_linear=True)
Motor_input = sf.Sensor('Motor_input',sf.MOTOR_COLUMNS,meas_record_file=parent_path/'test-run-Motor-Control.csv',is_linear=True)
Camera = sf.Sensor('Camera',sf.CAMERA_COLUMNS,meas_record_file=parent_path/'test-run-camera.csv',is_linear=False,start_index=154)
'''

camera_file = 'Datasets/data/task6/camera_tracking_task6.csv'
imu_file = 'Datasets/data/task6/imu_tracking_task6.csv'
imu_static = 'Datasets/data/task1/imu_reading_task1.csv'
motor_file = 'Datasets/data/task6/motor_control_tracking_task6.csv'

IMU = sf.Sensor('IMU',
                sf.IMU_COLUMNS,
                meas_record_file=imu_file,
                is_linear=True,
                start_index=3686)
IMU_static = sf.Sensor('IMU_static',
                       sf.IMU_COLUMNS,
                       meas_record_file=imu_static,
                       is_linear=True)
Motor_input = sf.Sensor('Motor_input',
                        sf.MOTOR_COLUMNS,
                        meas_record_file=motor_file,
                        is_linear=True)
Camera = sf.Sensor('Camera',
                   sf.CAMERA_COLUMNS,
                   meas_record_file=camera_file,
                   is_linear=False,
                   start_index=154)
示例#7
0
    def EKF_estimate(self,motor_file = None, camera_file = None):
        cam = pd.read_csv(camera_file, header=None)
        print('cam ', np.shape(cam))
        # Timestamp, v,w
        motor = pd.read_csv(motor_file, header=None)
        print('motor ', np.shape(motor))
        cam_time = cam.iloc[0,0],cam.iloc[-1,0]
        motor_time = motor.iloc[0, 0], motor.iloc[-1, 0]
        print('cam_time:{},  motor_time:{}'.format(cam_time,motor_time))

        motor = pd.read_csv(motor_file, header=None)
        print('motor ', np.shape(motor))
        '''
            -replace timesteps with int numbers autoincrement
            ---get data from camera first -> index of the detected qr codes
            ---based on current timestamp, get the clossest u from motor
            ---feed, landmarks pos and u to EKF
        '''
        self.Camera = sf.Sensor('Camera', sf.CAMERA_COLUMNS, meas_record_file=camera_file, is_linear=False, start_index=0)
        self.x_true = np.array([15.7, 47.5, 90.0])
        self.x_init = self.x_true  # np.array([0,0,0])
        x = np.zeros((self.Camera.meas_record.shape[0] // 3, 3), dtype=np.float)
        print('x shape is ', np.shape(x))
        self.Camera.reset_sampling_index()
        i = 0
        dt = 1
        dt=3.2
        dt = 6
        sigma_range = 0.05
        sigma_bearing = 0.05
        sigma_vel = 0.01
        sigma_steer = np.radians(1)

        ekf = RobotEKF(dt, wheelbase=0.12, sigma_vel=sigma_vel, sigma_steer=sigma_steer)
        ekf.x = array([[15.7, 47.5, np.deg2rad(90.0)]]).T
        ekf.P = np.diag([1, 1, 1])
        ekf.R = np.diag([sigma_range ** 2, sigma_bearing ** 2])

        while (self.Camera.current_sample_index < self.Camera.time.shape[0] and i < x.shape[0] - 1):
            i += 1
            y_ = self.Camera.get_measurement()
            if y_.shape[0] < 2:
                continue

            qr_row = y_[:, 0].astype('int')
            camera_time = self.Camera.current_time
            landmarks = rnmf.QRCODE_LOCATIONS[qr_row, 1:]
            closest_index = motor.iloc[:, 0].sub(camera_time).abs().idxmin()
            closest_u = np.array(motor.iloc[closest_index, 1:3])
            #print('qr_row len:{}, qr_row:{}, time:{}, landmarks:{},closest_u:{}'.format(np.shape(qr_row),qr_row,camera_time, np.shape(landmarks),closest_u))

            sim_pos = ekf.x.copy()  # simulated position
            u = closest_u  # steering command (vel, steering angle radians)
            #u[1] = np.deg2rad(-u[1])*3
            #u[0] *= 4
            u[1] = np.deg2rad(-u[1])
            #print('u ',u)

            plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60)

            sim_pos = ekf.move(sim_pos, u, dt)  # simulate robot
            plt.plot(sim_pos[0], sim_pos[1], '-ok', linewidth=1, markersize=1)
            phi = sim_pos[-1]
            plt.quiver(sim_pos[0], sim_pos[1], np.cos(phi),
                       np.sin(phi),linewidth=.1, alpha=.5, color='red')

            ekf.predict(u=u)

            p_x, p_y = sim_pos[0, 0], sim_pos[1, 0]
            for lmark in landmarks:
                d = np.sqrt((lmark[0] - p_x) ** 2 + (lmark[1] - p_y) ** 2)
                a = atan2(lmark[1] - p_y, lmark[0] - p_x) - sim_pos[2, 0]
                z = np.array([[d + np.random.randn() * sigma_range], [a + np.random.randn() * sigma_bearing]])

                ekf.update(z, HJacobian=H_of, Hx=Hx, residual=residual,
                           args=(lmark), hx_args=(lmark))

        phi = np.deg2rad(self.x_init[-1])
        plt.quiver(self.x_init[0], self.x_init[1], np.cos(phi), np.sin(phi),
                   linewidth=2., alpha=1.0, color='blue', label='init-pose')

        plt.grid(True)
        plt.legend()
        plt.axis('equal')
        plt.show()
示例#8
0
    def localize(self,csvFile = None):
        self.csvFile = csvFile
        self.Camera = sf.Sensor('Camera', sf.CAMERA_COLUMNS, meas_record_file=csvFile, is_linear=False, start_index=0)

        R_one_diag = np.array([2, 10])
        I_max,gamma,N = 20,1,10
        params_LSQ = {'x_sensors': None,
                      'R': None,
                      'LR': None,
                      'Rinv': None,
                      'gamma': gamma,
                      'I_max': I_max,
                      'Line_search': False,
                      'Line_search_n_points': N,
                      'Jwls': lsqS.Jwls}

        self.Camera.reset_sampling_index()
        self.Camera.get_measurement()

        y_raw = self.Camera.get_measurement()
        w = y_raw[:, 3]
        h = y_raw[:, 4]
        C_x = y_raw[:, 1]

        di = self.h0 * self.f / h       #according to equation 9
        phi = np.arctan2(C_x, self.f)  #according to equation 9

        angle_qr = np.arccos(np.minimum(w, h) / h)
        corrected_dist = di / np.cos(phi) + 0.5 * self.h0 * np.sin(angle_qr)


        y_raw[:, 5] = corrected_dist #update the di column
        phi -= self.x_true[-1] #update the angle
        y_raw[:, 6] = phi

        n_qr_codes = y_raw.shape[0]

        y_measurements = y_raw[:,5:].flatten() #take di and phi as measurements
        print('y_measurements ', np.shape(y_measurements))

        qr_pos = rnmf.QRCODE_LOCATIONS[y_raw[:, 0].astype('int'), 1:]
        params_LSQ['x_sensors'] = qr_pos
        R = np.diag(np.kron(np.ones(n_qr_codes), R_one_diag))
        params_LSQ['R'] = R
        params_LSQ['LR'] = np.linalg.cholesky(R).T
        params_LSQ['Rinv'] = np.diag(1 / np.diag(R))


        g = rnmf.h_cam #measurement model
        G = rnmf.H_cam #Jacobian of the measurement model
        method = 'gauss-newton'

        x_ls, J_ = lsqS.lsqsolve(y=y_measurements, g=g, G=G, x_init=self.x_init,
                                 params=params_LSQ,method=method)

        best_X = x_ls[:,-1].copy()
        best_X[-1] = np.rad2deg(best_X[-1])
        print('x_true ', self.x_true)
        print('best_X ',best_X)
        print('x_ls:{}'.format(np.shape(x_ls)))


        fig, axs = plt.subplots(2, 2, figsize=(8, 6))
        axs[0, 0].plot(x_ls[0, :], label='Px:{}'.format(np.round(best_X[0], 2)))
        axs[0, 0].set_title('X - position')
        axs[0, 0].set(xlabel='time', ylabel='X')
        axs[0, 0].grid(True)
        axs[0, 0].legend()

        axs[1, 0].plot(x_ls[1, :], label='Py:{}'.format(np.round(best_X[1], 2)))
        axs[1, 0].set_title('Y - position')
        axs[1, 0].set(xlabel='time', ylabel='Y')
        axs[1, 0].grid(True)
        axs[1, 0].legend()

        axs[0, 1].plot(x_ls[2, :] * rnmf.RAD_TO_DEG, label='Orientation:{}'.format(np.round(best_X[-1], 2)))
        axs[0, 1].set_title('Orientation')
        axs[0, 1].set(xlabel='time', ylabel='Orientation')
        axs[0, 1].grid(True)
        axs[0, 1].set_ylim([-180, 180])
        axs[0, 1].legend()

        phi = np.deg2rad(best_X[-1])
        print('phi ',phi)


        axs[1, 1].quiver(best_X[0], best_X[1],np.cos(phi), np.sin(phi),
                     linewidth=2.9, alpha=1.0, color='red', label='estimated-pose')
        axs[1, 1].set_title('Final: X,Y and phi')
        axs[1, 1].set(xlabel='X', ylabel='Y')
        axs[1, 1].grid(True)
        axs[1, 1].set_ylim([-5, 50])
        axs[1, 1].set_xlim([-5, 100])

        axs[1, 1].quiver(x_ls[0,:],x_ls[1,:],
                       np.cos(np.deg2rad(x_ls[2,:])),np.sin(np.deg2rad(x_ls[2,:])),
                       linewidth=0.5, alpha=0.5, color='green')

        axs[1, 1].quiver(self.x_init[0], self.x_init[1], np.cos(np.deg2rad(self.x_init[-1])),
                         np.sin(np.deg2rad(self.x_init[-1])),
                         linewidth=2.9, alpha=1.0, color='blue', label='init-pose')

        axs[1, 1].legend(loc='upper left')

        plt.show()
示例#9
0
    def Estimate_pose_based_on_camera(self,cam_csv=None):
        self.Camera = sf.Sensor('Camera',sf.CAMERA_COLUMNS,meas_record_file=cam_csv,is_linear=False,start_index=0)
        self.x_true = np.array([15.7, 47.5, 90.0])
        self.x_init = self.x_true# np.array([0,0,0])
        x = np.zeros((self.Camera.meas_record.shape[0] // 3, 3), dtype=np.float)
        x[0, :] = self.x_init
        t = np.zeros(x.shape[0])
        t[0] = self.Camera.time[0]

        R_one_diag = np.array([2, 20])
        I_max,gamma,N = 10,1, 10
        params_LSQ = {'x_sensors': None,
                      'R': None,
                      'LR': None,
                      'Rinv': None,
                      'gamma': gamma,
                      'I_max': I_max,
                      'Line_search': False,
                      'Line_search_n_points': N,
                      'Jwls': lsqS.Jwls
                      }
        self.Camera.reset_sampling_index()
        i = 0
        while (self.Camera.current_sample_index < self.Camera.time.shape[0] and i < x.shape[0] - 1):
            i += 1
            y_ = self.Camera.get_measurement()
            n_qr_codes = y_.shape[0]

            if n_qr_codes < 3:
                x[i, :] = x[i - 1, :]
                continue

            w,h,c_x = y_[:, 3],y_[:, 4],y_[:, 1]

            di = self.h0 * self.f / h
            phi = np.arctan2(c_x, self.f)
            angle_qr = np.arccos(np.minimum(w, h) / h)

            corrected_dist = di / np.cos(phi) + 0.5 * self.h0 * np.sin(angle_qr)
            y_[:, 5] = corrected_dist
            y = y_[:, 5:].flatten()

            qr_row = y_[:, 0].astype('int')
            qr_pos = rnmf.QRCODE_LOCATIONS[qr_row, 1:]
            params_LSQ['x_sensors'] = qr_pos
            R = np.diag(np.kron(np.ones(n_qr_codes), R_one_diag))
            params_LSQ['R'] = R
            params_LSQ['LR'] = np.linalg.cholesky(R).T
            params_LSQ['Rinv'] = np.diag(1 / np.diag(R))
            xhat_history_GN, J_history_GN = lsqS.lsqsolve(y, rnmf.h_cam, rnmf.H_cam, x[i - 1, :], params_LSQ,
                                                          method='gauss-newton')
            x[i, :] = xhat_history_GN[:, -1]

        plt.figure()
        plt.title('Camera measurements & Jwls estimation')
        plt.plot(x[1:,0],x[1:,1],'-ok',linewidth=1,markersize=3, label='position')
        phi = x[1:,2]
        #plt.quiver(x[1:,0], x[1:,1], np.cos(phi), np.sin(phi),linewidth=2.9, alpha=1.0, color='red', label='Orientation')
        plt.quiver(x[1:, 0], x[1:, 1], -np.sin(phi), np.cos(phi),linewidth=2.9, alpha=1.0, color='red', label='Orientation')
        plt.grid(True)
        plt.quiver(x[-1,0], x[-1,1], -np.sin(phi[-1]), np.cos(phi[-1]),
                         linewidth=3., alpha=1.0, color='green', label='final-pose')

        plt.quiver(self.x_init[0], self.x_init[1], np.cos(np.deg2rad(self.x_init[-1])),
                         np.sin(np.deg2rad(self.x_init[-1])),
                         linewidth=3., alpha=1.0, color='blue', label='init-pose')
        plt.legend()
        plt.show()