diff --git a/Tools/process_sensor_caldata.py b/Tools/process_sensor_caldata.py index 3d9ba7eb4b..3fe989fef2 100644 --- a/Tools/process_sensor_caldata.py +++ b/Tools/process_sensor_caldata.py @@ -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() #################################################################################