forked from Archive/PX4-Autopilot
Tools: Enable offline calibrator to handle less than 3 inertial sensors
This commit is contained in:
Paul Riseborough
Lorenz Meier
parent
4d163eebb9
commit
f86347f1e2
|
|
@ -50,32 +50,40 @@ data = ulog.data_list
|
|||
|
||||
# extract gyro data
|
||||
sensor_instance = 0
|
||||
num_gyros = 0
|
||||
for d in data:
|
||||
if d.name == 'sensor_gyro':
|
||||
if sensor_instance == 0:
|
||||
sensor_gyro_0 = d.data
|
||||
print('found gyro 0 data')
|
||||
num_gyros = 1
|
||||
if sensor_instance == 1:
|
||||
sensor_gyro_1 = d.data
|
||||
print('found gyro 1 data')
|
||||
if sensor_instance == 2:
|
||||
num_gyros = 2
|
||||
if sensor_instance == 2:
|
||||
sensor_gyro_2 = d.data
|
||||
print('found gyro 2 data')
|
||||
sensor_instance = sensor_instance +1
|
||||
num_gyros = 3
|
||||
sensor_instance = sensor_instance +1
|
||||
|
||||
# extract accel data
|
||||
sensor_instance = 0
|
||||
num_accels = 0
|
||||
for d in data:
|
||||
if d.name == 'sensor_accel':
|
||||
if sensor_instance == 0:
|
||||
sensor_accel_0 = d.data
|
||||
print('found accel 0 data')
|
||||
num_accels = 1
|
||||
if sensor_instance == 1:
|
||||
sensor_accel_1 = d.data
|
||||
print('found accel 1 data')
|
||||
num_accels = 2
|
||||
if sensor_instance == 2:
|
||||
sensor_accel_2 = d.data
|
||||
print('found accel 2 data')
|
||||
num_accels = 3
|
||||
sensor_instance = sensor_instance +1
|
||||
|
||||
# extract baro data
|
||||
|
|
@ -124,72 +132,73 @@ gyro_0_params = {
|
|||
}
|
||||
|
||||
# curve fit the data for gyro 0 corrections
|
||||
gyro_0_params['TC_G0_ID'] = int(np.median(sensor_gyro_0['device_id']))
|
||||
if num_gyros >= 1:
|
||||
gyro_0_params['TC_G0_ID'] = int(np.median(sensor_gyro_0['device_id']))
|
||||
|
||||
# find the min, max and reference temperature
|
||||
gyro_0_params['TC_G0_TMIN'] = np.amin(sensor_gyro_0['temperature'])
|
||||
gyro_0_params['TC_G0_TMAX'] = np.amax(sensor_gyro_0['temperature'])
|
||||
gyro_0_params['TC_G0_TREF'] = 0.5 * (gyro_0_params['TC_G0_TMIN'] + gyro_0_params['TC_G0_TMAX'])
|
||||
temp_rel = sensor_gyro_0['temperature'] - gyro_0_params['TC_G0_TREF']
|
||||
temp_rel_resample = np.linspace(gyro_0_params['TC_G0_TMIN']-gyro_0_params['TC_G0_TREF'], gyro_0_params['TC_G0_TMAX']-gyro_0_params['TC_G0_TREF'], 100)
|
||||
temp_resample = temp_rel_resample + gyro_0_params['TC_G0_TREF']
|
||||
# find the min, max and reference temperature
|
||||
gyro_0_params['TC_G0_TMIN'] = np.amin(sensor_gyro_0['temperature'])
|
||||
gyro_0_params['TC_G0_TMAX'] = np.amax(sensor_gyro_0['temperature'])
|
||||
gyro_0_params['TC_G0_TREF'] = 0.5 * (gyro_0_params['TC_G0_TMIN'] + gyro_0_params['TC_G0_TMAX'])
|
||||
temp_rel = sensor_gyro_0['temperature'] - gyro_0_params['TC_G0_TREF']
|
||||
temp_rel_resample = np.linspace(gyro_0_params['TC_G0_TMIN']-gyro_0_params['TC_G0_TREF'], gyro_0_params['TC_G0_TMAX']-gyro_0_params['TC_G0_TREF'], 100)
|
||||
temp_resample = temp_rel_resample + gyro_0_params['TC_G0_TREF']
|
||||
|
||||
# fit X axis
|
||||
coef_gyro_0_x = np.polyfit(temp_rel,sensor_gyro_0['x'],3)
|
||||
gyro_0_params['TC_G0_X3_0'] = coef_gyro_0_x[0]
|
||||
gyro_0_params['TC_G0_X2_0'] = coef_gyro_0_x[1]
|
||||
gyro_0_params['TC_G0_X1_0'] = coef_gyro_0_x[2]
|
||||
gyro_0_params['TC_G0_X0_0'] = coef_gyro_0_x[3]
|
||||
fit_coef_gyro_0_x = np.poly1d(coef_gyro_0_x)
|
||||
gyro_0_x_resample = fit_coef_gyro_0_x(temp_rel_resample)
|
||||
# fit X axis
|
||||
coef_gyro_0_x = np.polyfit(temp_rel,sensor_gyro_0['x'],3)
|
||||
gyro_0_params['TC_G0_X3_0'] = coef_gyro_0_x[0]
|
||||
gyro_0_params['TC_G0_X2_0'] = coef_gyro_0_x[1]
|
||||
gyro_0_params['TC_G0_X1_0'] = coef_gyro_0_x[2]
|
||||
gyro_0_params['TC_G0_X0_0'] = coef_gyro_0_x[3]
|
||||
fit_coef_gyro_0_x = np.poly1d(coef_gyro_0_x)
|
||||
gyro_0_x_resample = fit_coef_gyro_0_x(temp_rel_resample)
|
||||
|
||||
# fit Y axis
|
||||
coef_gyro_0_y = np.polyfit(temp_rel,sensor_gyro_0['y'],3)
|
||||
gyro_0_params['TC_G0_X3_1'] = coef_gyro_0_y[0]
|
||||
gyro_0_params['TC_G0_X2_1'] = coef_gyro_0_y[1]
|
||||
gyro_0_params['TC_G0_X1_1'] = coef_gyro_0_y[2]
|
||||
gyro_0_params['TC_G0_X0_1'] = coef_gyro_0_y[3]
|
||||
fit_coef_gyro_0_y = np.poly1d(coef_gyro_0_y)
|
||||
gyro_0_y_resample = fit_coef_gyro_0_y(temp_rel_resample)
|
||||
# fit Y axis
|
||||
coef_gyro_0_y = np.polyfit(temp_rel,sensor_gyro_0['y'],3)
|
||||
gyro_0_params['TC_G0_X3_1'] = coef_gyro_0_y[0]
|
||||
gyro_0_params['TC_G0_X2_1'] = coef_gyro_0_y[1]
|
||||
gyro_0_params['TC_G0_X1_1'] = coef_gyro_0_y[2]
|
||||
gyro_0_params['TC_G0_X0_1'] = coef_gyro_0_y[3]
|
||||
fit_coef_gyro_0_y = np.poly1d(coef_gyro_0_y)
|
||||
gyro_0_y_resample = fit_coef_gyro_0_y(temp_rel_resample)
|
||||
|
||||
# fit Z axis
|
||||
coef_gyro_0_z = np.polyfit(temp_rel,sensor_gyro_0['z'],3)
|
||||
gyro_0_params['TC_G0_X3_2'] = coef_gyro_0_z[0]
|
||||
gyro_0_params['TC_G0_X2_2'] = coef_gyro_0_z[1]
|
||||
gyro_0_params['TC_G0_X1_2'] = coef_gyro_0_z[2]
|
||||
gyro_0_params['TC_G0_X0_2'] = coef_gyro_0_z[3]
|
||||
fit_coef_gyro_0_z = np.poly1d(coef_gyro_0_z)
|
||||
gyro_0_z_resample = fit_coef_gyro_0_z(temp_rel_resample)
|
||||
# fit Z axis
|
||||
coef_gyro_0_z = np.polyfit(temp_rel,sensor_gyro_0['z'],3)
|
||||
gyro_0_params['TC_G0_X3_2'] = coef_gyro_0_z[0]
|
||||
gyro_0_params['TC_G0_X2_2'] = coef_gyro_0_z[1]
|
||||
gyro_0_params['TC_G0_X1_2'] = coef_gyro_0_z[2]
|
||||
gyro_0_params['TC_G0_X0_2'] = coef_gyro_0_z[3]
|
||||
fit_coef_gyro_0_z = np.poly1d(coef_gyro_0_z)
|
||||
gyro_0_z_resample = fit_coef_gyro_0_z(temp_rel_resample)
|
||||
|
||||
# gyro0 vs temperature
|
||||
plt.figure(1,figsize=(20,13))
|
||||
# gyro0 vs temperature
|
||||
plt.figure(1,figsize=(20,13))
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,1)
|
||||
plt.plot(sensor_gyro_0['temperature'],sensor_gyro_0['x'],'b')
|
||||
plt.plot(temp_resample,gyro_0_x_resample,'r')
|
||||
plt.title('Gyro 0 Bias vs Temperature')
|
||||
plt.ylabel('X bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,1)
|
||||
plt.plot(sensor_gyro_0['temperature'],sensor_gyro_0['x'],'b')
|
||||
plt.plot(temp_resample,gyro_0_x_resample,'r')
|
||||
plt.title('Gyro 0 Bias vs Temperature')
|
||||
plt.ylabel('X bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,2)
|
||||
plt.plot(sensor_gyro_0['temperature'],sensor_gyro_0['y'],'b')
|
||||
plt.plot(temp_resample,gyro_0_y_resample,'r')
|
||||
plt.ylabel('Y bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,2)
|
||||
plt.plot(sensor_gyro_0['temperature'],sensor_gyro_0['y'],'b')
|
||||
plt.plot(temp_resample,gyro_0_y_resample,'r')
|
||||
plt.ylabel('Y bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,3)
|
||||
plt.plot(sensor_gyro_0['temperature'],sensor_gyro_0['z'],'b')
|
||||
plt.plot(temp_resample,gyro_0_z_resample,'r')
|
||||
plt.ylabel('Z bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,3)
|
||||
plt.plot(sensor_gyro_0['temperature'],sensor_gyro_0['z'],'b')
|
||||
plt.plot(temp_resample,gyro_0_z_resample,'r')
|
||||
plt.ylabel('Z bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
pp.savefig()
|
||||
pp.savefig()
|
||||
|
||||
#################################################################################
|
||||
|
||||
|
|
@ -219,72 +228,73 @@ gyro_1_params = {
|
|||
}
|
||||
|
||||
# curve fit the data for gyro 1 corrections
|
||||
gyro_1_params['TC_G1_ID'] = int(np.median(sensor_gyro_1['device_id']))
|
||||
if num_gyros >= 2:
|
||||
gyro_1_params['TC_G1_ID'] = int(np.median(sensor_gyro_1['device_id']))
|
||||
|
||||
# find the min, max and reference temperature
|
||||
gyro_1_params['TC_G1_TMIN'] = np.amin(sensor_gyro_1['temperature'])
|
||||
gyro_1_params['TC_G1_TMAX'] = np.amax(sensor_gyro_1['temperature'])
|
||||
gyro_1_params['TC_G1_TREF'] = 0.5 * (gyro_1_params['TC_G1_TMIN'] + gyro_1_params['TC_G1_TMAX'])
|
||||
temp_rel = sensor_gyro_1['temperature'] - gyro_1_params['TC_G1_TREF']
|
||||
temp_rel_resample = np.linspace(gyro_1_params['TC_G1_TMIN']-gyro_1_params['TC_G1_TREF'], gyro_1_params['TC_G1_TMAX']-gyro_1_params['TC_G1_TREF'], 100)
|
||||
temp_resample = temp_rel_resample + gyro_1_params['TC_G1_TREF']
|
||||
# find the min, max and reference temperature
|
||||
gyro_1_params['TC_G1_TMIN'] = np.amin(sensor_gyro_1['temperature'])
|
||||
gyro_1_params['TC_G1_TMAX'] = np.amax(sensor_gyro_1['temperature'])
|
||||
gyro_1_params['TC_G1_TREF'] = 0.5 * (gyro_1_params['TC_G1_TMIN'] + gyro_1_params['TC_G1_TMAX'])
|
||||
temp_rel = sensor_gyro_1['temperature'] - gyro_1_params['TC_G1_TREF']
|
||||
temp_rel_resample = np.linspace(gyro_1_params['TC_G1_TMIN']-gyro_1_params['TC_G1_TREF'], gyro_1_params['TC_G1_TMAX']-gyro_1_params['TC_G1_TREF'], 100)
|
||||
temp_resample = temp_rel_resample + gyro_1_params['TC_G1_TREF']
|
||||
|
||||
# fit X axis
|
||||
coef_gyro_1_x = np.polyfit(temp_rel,sensor_gyro_1['x'],3)
|
||||
gyro_1_params['TC_G1_X3_0'] = coef_gyro_1_x[0]
|
||||
gyro_1_params['TC_G1_X2_0'] = coef_gyro_1_x[1]
|
||||
gyro_1_params['TC_G1_X1_0'] = coef_gyro_1_x[2]
|
||||
gyro_1_params['TC_G1_X0_0'] = coef_gyro_1_x[3]
|
||||
fit_coef_gyro_1_x = np.poly1d(coef_gyro_1_x)
|
||||
gyro_1_x_resample = fit_coef_gyro_1_x(temp_rel_resample)
|
||||
# fit X axis
|
||||
coef_gyro_1_x = np.polyfit(temp_rel,sensor_gyro_1['x'],3)
|
||||
gyro_1_params['TC_G1_X3_0'] = coef_gyro_1_x[0]
|
||||
gyro_1_params['TC_G1_X2_0'] = coef_gyro_1_x[1]
|
||||
gyro_1_params['TC_G1_X1_0'] = coef_gyro_1_x[2]
|
||||
gyro_1_params['TC_G1_X0_0'] = coef_gyro_1_x[3]
|
||||
fit_coef_gyro_1_x = np.poly1d(coef_gyro_1_x)
|
||||
gyro_1_x_resample = fit_coef_gyro_1_x(temp_rel_resample)
|
||||
|
||||
# fit Y axis
|
||||
coef_gyro_1_y = np.polyfit(temp_rel,sensor_gyro_1['y'],3)
|
||||
gyro_1_params['TC_G1_X3_1'] = coef_gyro_1_y[0]
|
||||
gyro_1_params['TC_G1_X2_1'] = coef_gyro_1_y[1]
|
||||
gyro_1_params['TC_G1_X1_1'] = coef_gyro_1_y[2]
|
||||
gyro_1_params['TC_G1_X0_1'] = coef_gyro_1_y[3]
|
||||
fit_coef_gyro_1_y = np.poly1d(coef_gyro_1_y)
|
||||
gyro_1_y_resample = fit_coef_gyro_1_y(temp_rel_resample)
|
||||
# fit Y axis
|
||||
coef_gyro_1_y = np.polyfit(temp_rel,sensor_gyro_1['y'],3)
|
||||
gyro_1_params['TC_G1_X3_1'] = coef_gyro_1_y[0]
|
||||
gyro_1_params['TC_G1_X2_1'] = coef_gyro_1_y[1]
|
||||
gyro_1_params['TC_G1_X1_1'] = coef_gyro_1_y[2]
|
||||
gyro_1_params['TC_G1_X0_1'] = coef_gyro_1_y[3]
|
||||
fit_coef_gyro_1_y = np.poly1d(coef_gyro_1_y)
|
||||
gyro_1_y_resample = fit_coef_gyro_1_y(temp_rel_resample)
|
||||
|
||||
# fit Z axis
|
||||
coef_gyro_1_z = np.polyfit(temp_rel,sensor_gyro_1['z'],3)
|
||||
gyro_1_params['TC_G1_X3_2'] = coef_gyro_1_z[0]
|
||||
gyro_1_params['TC_G1_X2_2'] = coef_gyro_1_z[1]
|
||||
gyro_1_params['TC_G1_X1_2'] = coef_gyro_1_z[2]
|
||||
gyro_1_params['TC_G1_X0_2'] = coef_gyro_1_z[3]
|
||||
fit_coef_gyro_1_z = np.poly1d(coef_gyro_1_z)
|
||||
gyro_1_z_resample = fit_coef_gyro_1_z(temp_rel_resample)
|
||||
# fit Z axis
|
||||
coef_gyro_1_z = np.polyfit(temp_rel,sensor_gyro_1['z'],3)
|
||||
gyro_1_params['TC_G1_X3_2'] = coef_gyro_1_z[0]
|
||||
gyro_1_params['TC_G1_X2_2'] = coef_gyro_1_z[1]
|
||||
gyro_1_params['TC_G1_X1_2'] = coef_gyro_1_z[2]
|
||||
gyro_1_params['TC_G1_X0_2'] = coef_gyro_1_z[3]
|
||||
fit_coef_gyro_1_z = np.poly1d(coef_gyro_1_z)
|
||||
gyro_1_z_resample = fit_coef_gyro_1_z(temp_rel_resample)
|
||||
|
||||
# gyro1 vs temperature
|
||||
plt.figure(2,figsize=(20,13))
|
||||
# gyro1 vs temperature
|
||||
plt.figure(2,figsize=(20,13))
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,1)
|
||||
plt.plot(sensor_gyro_1['temperature'],sensor_gyro_1['x'],'b')
|
||||
plt.plot(temp_resample,gyro_1_x_resample,'r')
|
||||
plt.title('Gyro 1 Bias vs Temperature')
|
||||
plt.ylabel('X bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,1)
|
||||
plt.plot(sensor_gyro_1['temperature'],sensor_gyro_1['x'],'b')
|
||||
plt.plot(temp_resample,gyro_1_x_resample,'r')
|
||||
plt.title('Gyro 1 Bias vs Temperature')
|
||||
plt.ylabel('X bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,2)
|
||||
plt.plot(sensor_gyro_1['temperature'],sensor_gyro_1['y'],'b')
|
||||
plt.plot(temp_resample,gyro_1_y_resample,'r')
|
||||
plt.ylabel('Y bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,2)
|
||||
plt.plot(sensor_gyro_1['temperature'],sensor_gyro_1['y'],'b')
|
||||
plt.plot(temp_resample,gyro_1_y_resample,'r')
|
||||
plt.ylabel('Y bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,3)
|
||||
plt.plot(sensor_gyro_1['temperature'],sensor_gyro_1['z'],'b')
|
||||
plt.plot(temp_resample,gyro_1_z_resample,'r')
|
||||
plt.ylabel('Z bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,3)
|
||||
plt.plot(sensor_gyro_1['temperature'],sensor_gyro_1['z'],'b')
|
||||
plt.plot(temp_resample,gyro_1_z_resample,'r')
|
||||
plt.ylabel('Z bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
pp.savefig()
|
||||
pp.savefig()
|
||||
|
||||
#################################################################################
|
||||
|
||||
|
|
@ -314,72 +324,73 @@ gyro_2_params = {
|
|||
}
|
||||
|
||||
# curve fit the data for gyro 2 corrections
|
||||
gyro_2_params['TC_G2_ID'] = int(np.median(sensor_gyro_2['device_id']))
|
||||
if num_gyros >= 3:
|
||||
gyro_2_params['TC_G2_ID'] = int(np.median(sensor_gyro_2['device_id']))
|
||||
|
||||
# find the min, max and reference temperature
|
||||
gyro_2_params['TC_G2_TMIN'] = np.amin(sensor_gyro_2['temperature'])
|
||||
gyro_2_params['TC_G2_TMAX'] = np.amax(sensor_gyro_2['temperature'])
|
||||
gyro_2_params['TC_G2_TREF'] = 0.5 * (gyro_2_params['TC_G2_TMIN'] + gyro_2_params['TC_G2_TMAX'])
|
||||
temp_rel = sensor_gyro_2['temperature'] - gyro_2_params['TC_G2_TREF']
|
||||
temp_rel_resample = np.linspace(gyro_2_params['TC_G2_TMIN']-gyro_2_params['TC_G2_TREF'], gyro_2_params['TC_G2_TMAX']-gyro_2_params['TC_G2_TREF'], 100)
|
||||
temp_resample = temp_rel_resample + gyro_2_params['TC_G2_TREF']
|
||||
# find the min, max and reference temperature
|
||||
gyro_2_params['TC_G2_TMIN'] = np.amin(sensor_gyro_2['temperature'])
|
||||
gyro_2_params['TC_G2_TMAX'] = np.amax(sensor_gyro_2['temperature'])
|
||||
gyro_2_params['TC_G2_TREF'] = 0.5 * (gyro_2_params['TC_G2_TMIN'] + gyro_2_params['TC_G2_TMAX'])
|
||||
temp_rel = sensor_gyro_2['temperature'] - gyro_2_params['TC_G2_TREF']
|
||||
temp_rel_resample = np.linspace(gyro_2_params['TC_G2_TMIN']-gyro_2_params['TC_G2_TREF'], gyro_2_params['TC_G2_TMAX']-gyro_2_params['TC_G2_TREF'], 100)
|
||||
temp_resample = temp_rel_resample + gyro_2_params['TC_G2_TREF']
|
||||
|
||||
# fit X axis
|
||||
coef_gyro_2_x = np.polyfit(temp_rel,sensor_gyro_2['x'],3)
|
||||
gyro_2_params['TC_G2_X3_0'] = coef_gyro_2_x[0]
|
||||
gyro_2_params['TC_G2_X2_0'] = coef_gyro_2_x[1]
|
||||
gyro_2_params['TC_G2_X1_0'] = coef_gyro_2_x[2]
|
||||
gyro_2_params['TC_G2_X0_0'] = coef_gyro_2_x[3]
|
||||
fit_coef_gyro_2_x = np.poly1d(coef_gyro_2_x)
|
||||
gyro_2_x_resample = fit_coef_gyro_2_x(temp_rel_resample)
|
||||
# fit X axis
|
||||
coef_gyro_2_x = np.polyfit(temp_rel,sensor_gyro_2['x'],3)
|
||||
gyro_2_params['TC_G2_X3_0'] = coef_gyro_2_x[0]
|
||||
gyro_2_params['TC_G2_X2_0'] = coef_gyro_2_x[1]
|
||||
gyro_2_params['TC_G2_X1_0'] = coef_gyro_2_x[2]
|
||||
gyro_2_params['TC_G2_X0_0'] = coef_gyro_2_x[3]
|
||||
fit_coef_gyro_2_x = np.poly1d(coef_gyro_2_x)
|
||||
gyro_2_x_resample = fit_coef_gyro_2_x(temp_rel_resample)
|
||||
|
||||
# fit Y axis
|
||||
coef_gyro_2_y = np.polyfit(temp_rel,sensor_gyro_2['y'],3)
|
||||
gyro_2_params['TC_G2_X3_1'] = coef_gyro_2_y[0]
|
||||
gyro_2_params['TC_G2_X2_1'] = coef_gyro_2_y[1]
|
||||
gyro_2_params['TC_G2_X1_1'] = coef_gyro_2_y[2]
|
||||
gyro_2_params['TC_G2_X0_1'] = coef_gyro_2_y[3]
|
||||
fit_coef_gyro_2_y = np.poly1d(coef_gyro_2_y)
|
||||
gyro_2_y_resample = fit_coef_gyro_2_y(temp_rel_resample)
|
||||
# fit Y axis
|
||||
coef_gyro_2_y = np.polyfit(temp_rel,sensor_gyro_2['y'],3)
|
||||
gyro_2_params['TC_G2_X3_1'] = coef_gyro_2_y[0]
|
||||
gyro_2_params['TC_G2_X2_1'] = coef_gyro_2_y[1]
|
||||
gyro_2_params['TC_G2_X1_1'] = coef_gyro_2_y[2]
|
||||
gyro_2_params['TC_G2_X0_1'] = coef_gyro_2_y[3]
|
||||
fit_coef_gyro_2_y = np.poly1d(coef_gyro_2_y)
|
||||
gyro_2_y_resample = fit_coef_gyro_2_y(temp_rel_resample)
|
||||
|
||||
# fit Z axis
|
||||
coef_gyro_2_z = np.polyfit(temp_rel,sensor_gyro_2['z'],3)
|
||||
gyro_2_params['TC_G2_X3_2'] = coef_gyro_2_z[0]
|
||||
gyro_2_params['TC_G2_X2_2'] = coef_gyro_2_z[1]
|
||||
gyro_2_params['TC_G2_X1_2'] = coef_gyro_2_z[2]
|
||||
gyro_2_params['TC_G2_X0_2'] = coef_gyro_2_z[3]
|
||||
fit_coef_gyro_2_z = np.poly1d(coef_gyro_2_z)
|
||||
gyro_2_z_resample = fit_coef_gyro_2_z(temp_rel_resample)
|
||||
# fit Z axis
|
||||
coef_gyro_2_z = np.polyfit(temp_rel,sensor_gyro_2['z'],3)
|
||||
gyro_2_params['TC_G2_X3_2'] = coef_gyro_2_z[0]
|
||||
gyro_2_params['TC_G2_X2_2'] = coef_gyro_2_z[1]
|
||||
gyro_2_params['TC_G2_X1_2'] = coef_gyro_2_z[2]
|
||||
gyro_2_params['TC_G2_X0_2'] = coef_gyro_2_z[3]
|
||||
fit_coef_gyro_2_z = np.poly1d(coef_gyro_2_z)
|
||||
gyro_2_z_resample = fit_coef_gyro_2_z(temp_rel_resample)
|
||||
|
||||
# gyro2 vs temperature
|
||||
plt.figure(3,figsize=(20,13))
|
||||
# gyro2 vs temperature
|
||||
plt.figure(3,figsize=(20,13))
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,1)
|
||||
plt.plot(sensor_gyro_2['temperature'],sensor_gyro_2['x'],'b')
|
||||
plt.plot(temp_resample,gyro_2_x_resample,'r')
|
||||
plt.title('Gyro 2 Bias vs Temperature')
|
||||
plt.ylabel('X bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,1)
|
||||
plt.plot(sensor_gyro_2['temperature'],sensor_gyro_2['x'],'b')
|
||||
plt.plot(temp_resample,gyro_2_x_resample,'r')
|
||||
plt.title('Gyro 2 Bias vs Temperature')
|
||||
plt.ylabel('X bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,2)
|
||||
plt.plot(sensor_gyro_2['temperature'],sensor_gyro_2['y'],'b')
|
||||
plt.plot(temp_resample,gyro_2_y_resample,'r')
|
||||
plt.ylabel('Y bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,2)
|
||||
plt.plot(sensor_gyro_2['temperature'],sensor_gyro_2['y'],'b')
|
||||
plt.plot(temp_resample,gyro_2_y_resample,'r')
|
||||
plt.ylabel('Y bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,3)
|
||||
plt.plot(sensor_gyro_2['temperature'],sensor_gyro_2['z'],'b')
|
||||
plt.plot(temp_resample,gyro_2_z_resample,'r')
|
||||
plt.ylabel('Z bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,3)
|
||||
plt.plot(sensor_gyro_2['temperature'],sensor_gyro_2['z'],'b')
|
||||
plt.plot(temp_resample,gyro_2_z_resample,'r')
|
||||
plt.ylabel('Z bias (rad/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
pp.savefig()
|
||||
pp.savefig()
|
||||
|
||||
#################################################################################
|
||||
|
||||
|
|
@ -409,75 +420,76 @@ accel_0_params = {
|
|||
}
|
||||
|
||||
# curve fit the data for accel 0 corrections
|
||||
accel_0_params['TC_A0_ID'] = int(np.median(sensor_accel_0['device_id']))
|
||||
if num_accel >= 1:
|
||||
accel_0_params['TC_A0_ID'] = int(np.median(sensor_accel_0['device_id']))
|
||||
|
||||
# find the min, max and reference temperature
|
||||
accel_0_params['TC_A0_TMIN'] = np.amin(sensor_accel_0['temperature'])
|
||||
accel_0_params['TC_A0_TMAX'] = np.amax(sensor_accel_0['temperature'])
|
||||
accel_0_params['TC_A0_TREF'] = 0.5 * (accel_0_params['TC_A0_TMIN'] + accel_0_params['TC_A0_TMAX'])
|
||||
temp_rel = sensor_accel_0['temperature'] - accel_0_params['TC_A0_TREF']
|
||||
temp_rel_resample = np.linspace(accel_0_params['TC_A0_TMIN']-accel_0_params['TC_A0_TREF'], accel_0_params['TC_A0_TMAX']-accel_0_params['TC_A0_TREF'], 100)
|
||||
temp_resample = temp_rel_resample + accel_0_params['TC_A0_TREF']
|
||||
# find the min, max and reference temperature
|
||||
accel_0_params['TC_A0_TMIN'] = np.amin(sensor_accel_0['temperature'])
|
||||
accel_0_params['TC_A0_TMAX'] = np.amax(sensor_accel_0['temperature'])
|
||||
accel_0_params['TC_A0_TREF'] = 0.5 * (accel_0_params['TC_A0_TMIN'] + accel_0_params['TC_A0_TMAX'])
|
||||
temp_rel = sensor_accel_0['temperature'] - accel_0_params['TC_A0_TREF']
|
||||
temp_rel_resample = np.linspace(accel_0_params['TC_A0_TMIN']-accel_0_params['TC_A0_TREF'], accel_0_params['TC_A0_TMAX']-accel_0_params['TC_A0_TREF'], 100)
|
||||
temp_resample = temp_rel_resample + accel_0_params['TC_A0_TREF']
|
||||
|
||||
# fit X axis
|
||||
correction_x = sensor_accel_0['x'] - np.median(sensor_accel_0['x'])
|
||||
coef_accel_0_x = np.polyfit(temp_rel,correction_x,3)
|
||||
accel_0_params['TC_A0_X3_0'] = coef_accel_0_x[0]
|
||||
accel_0_params['TC_A0_X2_0'] = coef_accel_0_x[1]
|
||||
accel_0_params['TC_A0_X1_0'] = coef_accel_0_x[2]
|
||||
accel_0_params['TC_A0_X0_0'] = coef_accel_0_x[3]
|
||||
fit_coef_accel_0_x = np.poly1d(coef_accel_0_x)
|
||||
correction_x_resample = fit_coef_accel_0_x(temp_rel_resample)
|
||||
# fit X axis
|
||||
correction_x = sensor_accel_0['x'] - np.median(sensor_accel_0['x'])
|
||||
coef_accel_0_x = np.polyfit(temp_rel,correction_x,3)
|
||||
accel_0_params['TC_A0_X3_0'] = coef_accel_0_x[0]
|
||||
accel_0_params['TC_A0_X2_0'] = coef_accel_0_x[1]
|
||||
accel_0_params['TC_A0_X1_0'] = coef_accel_0_x[2]
|
||||
accel_0_params['TC_A0_X0_0'] = coef_accel_0_x[3]
|
||||
fit_coef_accel_0_x = np.poly1d(coef_accel_0_x)
|
||||
correction_x_resample = fit_coef_accel_0_x(temp_rel_resample)
|
||||
|
||||
# fit Y axis
|
||||
correction_y = sensor_accel_0['y']-np.median(sensor_accel_0['y'])
|
||||
coef_accel_0_y = np.polyfit(temp_rel,correction_y,3)
|
||||
accel_0_params['TC_A0_X3_1'] = coef_accel_0_y[0]
|
||||
accel_0_params['TC_A0_X2_1'] = coef_accel_0_y[1]
|
||||
accel_0_params['TC_A0_X1_1'] = coef_accel_0_y[2]
|
||||
accel_0_params['TC_A0_X0_1'] = coef_accel_0_y[3]
|
||||
fit_coef_accel_0_y = np.poly1d(coef_accel_0_y)
|
||||
correction_y_resample = fit_coef_accel_0_y(temp_rel_resample)
|
||||
# fit Y axis
|
||||
correction_y = sensor_accel_0['y']-np.median(sensor_accel_0['y'])
|
||||
coef_accel_0_y = np.polyfit(temp_rel,correction_y,3)
|
||||
accel_0_params['TC_A0_X3_1'] = coef_accel_0_y[0]
|
||||
accel_0_params['TC_A0_X2_1'] = coef_accel_0_y[1]
|
||||
accel_0_params['TC_A0_X1_1'] = coef_accel_0_y[2]
|
||||
accel_0_params['TC_A0_X0_1'] = coef_accel_0_y[3]
|
||||
fit_coef_accel_0_y = np.poly1d(coef_accel_0_y)
|
||||
correction_y_resample = fit_coef_accel_0_y(temp_rel_resample)
|
||||
|
||||
# fit Z axis
|
||||
correction_z = sensor_accel_0['z']-np.median(sensor_accel_0['z'])
|
||||
coef_accel_0_z = np.polyfit(temp_rel,correction_z,3)
|
||||
accel_0_params['TC_A0_X3_2'] = coef_accel_0_z[0]
|
||||
accel_0_params['TC_A0_X2_2'] = coef_accel_0_z[1]
|
||||
accel_0_params['TC_A0_X1_2'] = coef_accel_0_z[2]
|
||||
accel_0_params['TC_A0_X0_2'] = coef_accel_0_z[3]
|
||||
fit_coef_accel_0_z = np.poly1d(coef_accel_0_z)
|
||||
correction_z_resample = fit_coef_accel_0_z(temp_rel_resample)
|
||||
# fit Z axis
|
||||
correction_z = sensor_accel_0['z']-np.median(sensor_accel_0['z'])
|
||||
coef_accel_0_z = np.polyfit(temp_rel,correction_z,3)
|
||||
accel_0_params['TC_A0_X3_2'] = coef_accel_0_z[0]
|
||||
accel_0_params['TC_A0_X2_2'] = coef_accel_0_z[1]
|
||||
accel_0_params['TC_A0_X1_2'] = coef_accel_0_z[2]
|
||||
accel_0_params['TC_A0_X0_2'] = coef_accel_0_z[3]
|
||||
fit_coef_accel_0_z = np.poly1d(coef_accel_0_z)
|
||||
correction_z_resample = fit_coef_accel_0_z(temp_rel_resample)
|
||||
|
||||
# accel 0 vs temperature
|
||||
plt.figure(4,figsize=(20,13))
|
||||
# accel 0 vs temperature
|
||||
plt.figure(4,figsize=(20,13))
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,1)
|
||||
plt.plot(sensor_accel_0['temperature'],correction_x,'b')
|
||||
plt.plot(temp_resample,correction_x_resample,'r')
|
||||
plt.title('Accel 0 Bias vs Temperature')
|
||||
plt.ylabel('X bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,1)
|
||||
plt.plot(sensor_accel_0['temperature'],correction_x,'b')
|
||||
plt.plot(temp_resample,correction_x_resample,'r')
|
||||
plt.title('Accel 0 Bias vs Temperature')
|
||||
plt.ylabel('X bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,2)
|
||||
plt.plot(sensor_accel_0['temperature'],correction_y,'b')
|
||||
plt.plot(temp_resample,correction_y_resample,'r')
|
||||
plt.ylabel('Y bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,2)
|
||||
plt.plot(sensor_accel_0['temperature'],correction_y,'b')
|
||||
plt.plot(temp_resample,correction_y_resample,'r')
|
||||
plt.ylabel('Y bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,3)
|
||||
plt.plot(sensor_accel_0['temperature'],correction_z,'b')
|
||||
plt.plot(temp_resample,correction_z_resample,'r')
|
||||
plt.ylabel('Z bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,3)
|
||||
plt.plot(sensor_accel_0['temperature'],correction_z,'b')
|
||||
plt.plot(temp_resample,correction_z_resample,'r')
|
||||
plt.ylabel('Z bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
pp.savefig()
|
||||
pp.savefig()
|
||||
|
||||
#################################################################################
|
||||
|
||||
|
|
@ -507,75 +519,76 @@ accel_1_params = {
|
|||
}
|
||||
|
||||
# curve fit the data for accel 1 corrections
|
||||
accel_1_params['TC_A1_ID'] = int(np.median(sensor_accel_1['device_id']))
|
||||
if num_accel >= 2:
|
||||
accel_1_params['TC_A1_ID'] = int(np.median(sensor_accel_1['device_id']))
|
||||
|
||||
# find the min, max and reference temperature
|
||||
accel_1_params['TC_A1_TMIN'] = np.amin(sensor_accel_1['temperature'])
|
||||
accel_1_params['TC_A1_TMAX'] = np.amax(sensor_accel_1['temperature'])
|
||||
accel_1_params['TC_A1_TREF'] = 0.5 * (accel_1_params['TC_A1_TMIN'] + accel_1_params['TC_A1_TMAX'])
|
||||
temp_rel = sensor_accel_1['temperature'] - accel_1_params['TC_A1_TREF']
|
||||
temp_rel_resample = np.linspace(accel_1_params['TC_A1_TMIN']-accel_1_params['TC_A1_TREF'], accel_1_params['TC_A1_TMAX']-accel_1_params['TC_A1_TREF'], 100)
|
||||
temp_resample = temp_rel_resample + accel_1_params['TC_A1_TREF']
|
||||
# find the min, max and reference temperature
|
||||
accel_1_params['TC_A1_TMIN'] = np.amin(sensor_accel_1['temperature'])
|
||||
accel_1_params['TC_A1_TMAX'] = np.amax(sensor_accel_1['temperature'])
|
||||
accel_1_params['TC_A1_TREF'] = 0.5 * (accel_1_params['TC_A1_TMIN'] + accel_1_params['TC_A1_TMAX'])
|
||||
temp_rel = sensor_accel_1['temperature'] - accel_1_params['TC_A1_TREF']
|
||||
temp_rel_resample = np.linspace(accel_1_params['TC_A1_TMIN']-accel_1_params['TC_A1_TREF'], accel_1_params['TC_A1_TMAX']-accel_1_params['TC_A1_TREF'], 100)
|
||||
temp_resample = temp_rel_resample + accel_1_params['TC_A1_TREF']
|
||||
|
||||
# fit X axis
|
||||
correction_x = sensor_accel_1['x']-np.median(sensor_accel_1['x'])
|
||||
coef_accel_1_x = np.polyfit(temp_rel,correction_x,3)
|
||||
accel_1_params['TC_A1_X3_0'] = coef_accel_1_x[0]
|
||||
accel_1_params['TC_A1_X2_0'] = coef_accel_1_x[1]
|
||||
accel_1_params['TC_A1_X1_0'] = coef_accel_1_x[2]
|
||||
accel_1_params['TC_A1_X0_0'] = coef_accel_1_x[3]
|
||||
fit_coef_accel_1_x = np.poly1d(coef_accel_1_x)
|
||||
correction_x_resample = fit_coef_accel_1_x(temp_rel_resample)
|
||||
# fit X axis
|
||||
correction_x = sensor_accel_1['x']-np.median(sensor_accel_1['x'])
|
||||
coef_accel_1_x = np.polyfit(temp_rel,correction_x,3)
|
||||
accel_1_params['TC_A1_X3_0'] = coef_accel_1_x[0]
|
||||
accel_1_params['TC_A1_X2_0'] = coef_accel_1_x[1]
|
||||
accel_1_params['TC_A1_X1_0'] = coef_accel_1_x[2]
|
||||
accel_1_params['TC_A1_X0_0'] = coef_accel_1_x[3]
|
||||
fit_coef_accel_1_x = np.poly1d(coef_accel_1_x)
|
||||
correction_x_resample = fit_coef_accel_1_x(temp_rel_resample)
|
||||
|
||||
# fit Y axis
|
||||
correction_y = sensor_accel_1['y']-np.median(sensor_accel_1['y'])
|
||||
coef_accel_1_y = np.polyfit(temp_rel,correction_y,3)
|
||||
accel_1_params['TC_A1_X3_1'] = coef_accel_1_y[0]
|
||||
accel_1_params['TC_A1_X2_1'] = coef_accel_1_y[1]
|
||||
accel_1_params['TC_A1_X1_1'] = coef_accel_1_y[2]
|
||||
accel_1_params['TC_A1_X0_1'] = coef_accel_1_y[3]
|
||||
fit_coef_accel_1_y = np.poly1d(coef_accel_1_y)
|
||||
correction_y_resample = fit_coef_accel_1_y(temp_rel_resample)
|
||||
# fit Y axis
|
||||
correction_y = sensor_accel_1['y']-np.median(sensor_accel_1['y'])
|
||||
coef_accel_1_y = np.polyfit(temp_rel,correction_y,3)
|
||||
accel_1_params['TC_A1_X3_1'] = coef_accel_1_y[0]
|
||||
accel_1_params['TC_A1_X2_1'] = coef_accel_1_y[1]
|
||||
accel_1_params['TC_A1_X1_1'] = coef_accel_1_y[2]
|
||||
accel_1_params['TC_A1_X0_1'] = coef_accel_1_y[3]
|
||||
fit_coef_accel_1_y = np.poly1d(coef_accel_1_y)
|
||||
correction_y_resample = fit_coef_accel_1_y(temp_rel_resample)
|
||||
|
||||
# fit Z axis
|
||||
correction_z = (sensor_accel_1['z'])-np.median(sensor_accel_1['z'])
|
||||
coef_accel_1_z = np.polyfit(temp_rel,correction_z,3)
|
||||
accel_1_params['TC_A1_X3_2'] = coef_accel_1_z[0]
|
||||
accel_1_params['TC_A1_X2_2'] = coef_accel_1_z[1]
|
||||
accel_1_params['TC_A1_X1_2'] = coef_accel_1_z[2]
|
||||
accel_1_params['TC_A1_X0_2'] = coef_accel_1_z[3]
|
||||
fit_coef_accel_1_z = np.poly1d(coef_accel_1_z)
|
||||
correction_z_resample = fit_coef_accel_1_z(temp_rel_resample)
|
||||
# fit Z axis
|
||||
correction_z = (sensor_accel_1['z'])-np.median(sensor_accel_1['z'])
|
||||
coef_accel_1_z = np.polyfit(temp_rel,correction_z,3)
|
||||
accel_1_params['TC_A1_X3_2'] = coef_accel_1_z[0]
|
||||
accel_1_params['TC_A1_X2_2'] = coef_accel_1_z[1]
|
||||
accel_1_params['TC_A1_X1_2'] = coef_accel_1_z[2]
|
||||
accel_1_params['TC_A1_X0_2'] = coef_accel_1_z[3]
|
||||
fit_coef_accel_1_z = np.poly1d(coef_accel_1_z)
|
||||
correction_z_resample = fit_coef_accel_1_z(temp_rel_resample)
|
||||
|
||||
# accel 1 vs temperature
|
||||
plt.figure(5,figsize=(20,13))
|
||||
# accel 1 vs temperature
|
||||
plt.figure(5,figsize=(20,13))
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,1)
|
||||
plt.plot(sensor_accel_1['temperature'],correction_x,'b')
|
||||
plt.plot(temp_resample,correction_x_resample,'r')
|
||||
plt.title('Accel 1 Bias vs Temperature')
|
||||
plt.ylabel('X bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,1)
|
||||
plt.plot(sensor_accel_1['temperature'],correction_x,'b')
|
||||
plt.plot(temp_resample,correction_x_resample,'r')
|
||||
plt.title('Accel 1 Bias vs Temperature')
|
||||
plt.ylabel('X bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,2)
|
||||
plt.plot(sensor_accel_1['temperature'],correction_y,'b')
|
||||
plt.plot(temp_resample,correction_y_resample,'r')
|
||||
plt.ylabel('Y bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,2)
|
||||
plt.plot(sensor_accel_1['temperature'],correction_y,'b')
|
||||
plt.plot(temp_resample,correction_y_resample,'r')
|
||||
plt.ylabel('Y bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,3)
|
||||
plt.plot(sensor_accel_1['temperature'],correction_z,'b')
|
||||
plt.plot(temp_resample,correction_z_resample,'r')
|
||||
plt.ylabel('Z bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,3)
|
||||
plt.plot(sensor_accel_1['temperature'],correction_z,'b')
|
||||
plt.plot(temp_resample,correction_z_resample,'r')
|
||||
plt.ylabel('Z bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
pp.savefig()
|
||||
pp.savefig()
|
||||
|
||||
#################################################################################
|
||||
|
||||
|
|
@ -605,75 +618,76 @@ accel_2_params = {
|
|||
}
|
||||
|
||||
# curve fit the data for accel 2 corrections
|
||||
accel_2_params['TC_A2_ID'] = int(np.median(sensor_accel_2['device_id']))
|
||||
if num_accels >= 3:
|
||||
accel_2_params['TC_A2_ID'] = int(np.median(sensor_accel_2['device_id']))
|
||||
|
||||
# find the min, max and reference temperature
|
||||
accel_2_params['TC_A2_TMIN'] = np.amin(sensor_accel_2['temperature'])
|
||||
accel_2_params['TC_A2_TMAX'] = np.amax(sensor_accel_2['temperature'])
|
||||
accel_2_params['TC_A2_TREF'] = 0.5 * (accel_2_params['TC_A2_TMIN'] + accel_2_params['TC_A2_TMAX'])
|
||||
temp_rel = sensor_accel_2['temperature'] - accel_2_params['TC_A2_TREF']
|
||||
temp_rel_resample = np.linspace(accel_2_params['TC_A2_TMIN']-accel_2_params['TC_A2_TREF'], accel_2_params['TC_A2_TMAX']-accel_2_params['TC_A2_TREF'], 100)
|
||||
temp_resample = temp_rel_resample + accel_2_params['TC_A2_TREF']
|
||||
# find the min, max and reference temperature
|
||||
accel_2_params['TC_A2_TMIN'] = np.amin(sensor_accel_2['temperature'])
|
||||
accel_2_params['TC_A2_TMAX'] = np.amax(sensor_accel_2['temperature'])
|
||||
accel_2_params['TC_A2_TREF'] = 0.5 * (accel_2_params['TC_A2_TMIN'] + accel_2_params['TC_A2_TMAX'])
|
||||
temp_rel = sensor_accel_2['temperature'] - accel_2_params['TC_A2_TREF']
|
||||
temp_rel_resample = np.linspace(accel_2_params['TC_A2_TMIN']-accel_2_params['TC_A2_TREF'], accel_2_params['TC_A2_TMAX']-accel_2_params['TC_A2_TREF'], 100)
|
||||
temp_resample = temp_rel_resample + accel_2_params['TC_A2_TREF']
|
||||
|
||||
# fit X axis
|
||||
correction_x = sensor_accel_2['x']-np.median(sensor_accel_2['x'])
|
||||
coef_accel_2_x = np.polyfit(temp_rel,correction_x,3)
|
||||
accel_2_params['TC_A2_X3_0'] = coef_accel_2_x[0]
|
||||
accel_2_params['TC_A2_X2_0'] = coef_accel_2_x[1]
|
||||
accel_2_params['TC_A2_X1_0'] = coef_accel_2_x[2]
|
||||
accel_2_params['TC_A2_X0_0'] = coef_accel_2_x[3]
|
||||
fit_coef_accel_2_x = np.poly1d(coef_accel_2_x)
|
||||
correction_x_resample = fit_coef_accel_2_x(temp_rel_resample)
|
||||
# fit X axis
|
||||
correction_x = sensor_accel_2['x']-np.median(sensor_accel_2['x'])
|
||||
coef_accel_2_x = np.polyfit(temp_rel,correction_x,3)
|
||||
accel_2_params['TC_A2_X3_0'] = coef_accel_2_x[0]
|
||||
accel_2_params['TC_A2_X2_0'] = coef_accel_2_x[1]
|
||||
accel_2_params['TC_A2_X1_0'] = coef_accel_2_x[2]
|
||||
accel_2_params['TC_A2_X0_0'] = coef_accel_2_x[3]
|
||||
fit_coef_accel_2_x = np.poly1d(coef_accel_2_x)
|
||||
correction_x_resample = fit_coef_accel_2_x(temp_rel_resample)
|
||||
|
||||
# fit Y axis
|
||||
correction_y = sensor_accel_2['y']-np.median(sensor_accel_2['y'])
|
||||
coef_accel_2_y = np.polyfit(temp_rel,correction_y,3)
|
||||
accel_2_params['TC_A2_X3_1'] = coef_accel_2_y[0]
|
||||
accel_2_params['TC_A2_X2_1'] = coef_accel_2_y[1]
|
||||
accel_2_params['TC_A2_X1_1'] = coef_accel_2_y[2]
|
||||
accel_2_params['TC_A2_X0_1'] = coef_accel_2_y[3]
|
||||
fit_coef_accel_2_y = np.poly1d(coef_accel_2_y)
|
||||
correction_y_resample = fit_coef_accel_2_y(temp_rel_resample)
|
||||
# fit Y axis
|
||||
correction_y = sensor_accel_2['y']-np.median(sensor_accel_2['y'])
|
||||
coef_accel_2_y = np.polyfit(temp_rel,correction_y,3)
|
||||
accel_2_params['TC_A2_X3_1'] = coef_accel_2_y[0]
|
||||
accel_2_params['TC_A2_X2_1'] = coef_accel_2_y[1]
|
||||
accel_2_params['TC_A2_X1_1'] = coef_accel_2_y[2]
|
||||
accel_2_params['TC_A2_X0_1'] = coef_accel_2_y[3]
|
||||
fit_coef_accel_2_y = np.poly1d(coef_accel_2_y)
|
||||
correction_y_resample = fit_coef_accel_2_y(temp_rel_resample)
|
||||
|
||||
# fit Z axis
|
||||
correction_z = sensor_accel_2['z']-np.median(sensor_accel_2['z'])
|
||||
coef_accel_2_z = np.polyfit(temp_rel,correction_z,3)
|
||||
accel_2_params['TC_A2_X3_2'] = coef_accel_2_z[0]
|
||||
accel_2_params['TC_A2_X2_2'] = coef_accel_2_z[1]
|
||||
accel_2_params['TC_A2_X1_2'] = coef_accel_2_z[2]
|
||||
accel_2_params['TC_A2_X0_2'] = coef_accel_2_z[3]
|
||||
fit_coef_accel_2_z = np.poly1d(coef_accel_2_z)
|
||||
correction_z_resample = fit_coef_accel_2_z(temp_rel_resample)
|
||||
# fit Z axis
|
||||
correction_z = sensor_accel_2['z']-np.median(sensor_accel_2['z'])
|
||||
coef_accel_2_z = np.polyfit(temp_rel,correction_z,3)
|
||||
accel_2_params['TC_A2_X3_2'] = coef_accel_2_z[0]
|
||||
accel_2_params['TC_A2_X2_2'] = coef_accel_2_z[1]
|
||||
accel_2_params['TC_A2_X1_2'] = coef_accel_2_z[2]
|
||||
accel_2_params['TC_A2_X0_2'] = coef_accel_2_z[3]
|
||||
fit_coef_accel_2_z = np.poly1d(coef_accel_2_z)
|
||||
correction_z_resample = fit_coef_accel_2_z(temp_rel_resample)
|
||||
|
||||
# accel 2 vs temperature
|
||||
plt.figure(6,figsize=(20,13))
|
||||
# accel 2 vs temperature
|
||||
plt.figure(6,figsize=(20,13))
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,1)
|
||||
plt.plot(sensor_accel_2['temperature'],correction_x,'b')
|
||||
plt.plot(temp_resample,correction_x_resample,'r')
|
||||
plt.title('Accel 2 Bias vs Temperature')
|
||||
plt.ylabel('X bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,1)
|
||||
plt.plot(sensor_accel_2['temperature'],correction_x,'b')
|
||||
plt.plot(temp_resample,correction_x_resample,'r')
|
||||
plt.title('Accel 2 Bias vs Temperature')
|
||||
plt.ylabel('X bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,2)
|
||||
plt.plot(sensor_accel_2['temperature'],correction_y,'b')
|
||||
plt.plot(temp_resample,correction_y_resample,'r')
|
||||
plt.ylabel('Y bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,2)
|
||||
plt.plot(sensor_accel_2['temperature'],correction_y,'b')
|
||||
plt.plot(temp_resample,correction_y_resample,'r')
|
||||
plt.ylabel('Y bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
# draw plots
|
||||
plt.subplot(3,1,3)
|
||||
plt.plot(sensor_accel_2['temperature'],correction_z,'b')
|
||||
plt.plot(temp_resample,correction_z_resample,'r')
|
||||
plt.ylabel('Z bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
# draw plots
|
||||
plt.subplot(3,1,3)
|
||||
plt.plot(sensor_accel_2['temperature'],correction_z,'b')
|
||||
plt.plot(temp_resample,correction_z_resample,'r')
|
||||
plt.ylabel('Z bias (m/s/s)')
|
||||
plt.xlabel('temperature (degC)')
|
||||
plt.grid()
|
||||
|
||||
pp.savefig()
|
||||
pp.savefig()
|
||||
|
||||
#################################################################################
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue