From b4e0a8396e6784891ec6dc281eca1ba6aa4e51fa Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 2 May 2021 13:42:09 -0400 Subject: [PATCH] Tools/process_sensor_caldata.py - median filter sensor data - this makes it a bit easier to see what's going on now that the raw sensor data (sensor_accel, sensor_gyro) is completely unfiltered --- Tools/process_sensor_caldata.py | 142 +++++++++++++++++++++----------- 1 file changed, 93 insertions(+), 49 deletions(-) diff --git a/Tools/process_sensor_caldata.py b/Tools/process_sensor_caldata.py index 561ef29278..fdc0be8f36 100755 --- a/Tools/process_sensor_caldata.py +++ b/Tools/process_sensor_caldata.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 from __future__ import print_function @@ -7,6 +7,7 @@ import os import math import matplotlib.pyplot as plt import numpy as np +import scipy as sp from pyulog import * @@ -63,6 +64,9 @@ def resampleWithDeltaX(x,y): return resampledX,resampledY +def median_filter(data): + return sp.signal.medfilt(data, 31) + parser = argparse.ArgumentParser(description='Reads in IMU data from a static thermal calibration test and performs a curve fit of gyro, accel and baro bias vs temperature') parser.add_argument('filename', metavar='file.ulg', help='ULog input file') parser.add_argument('--no_resample', dest='noResample', action='store_const', @@ -184,12 +188,16 @@ if num_gyros >= 1 and not math.isnan(sensor_gyro_0['temperature'][0]): 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'] + sensor_gyro_0['x'] = median_filter(sensor_gyro_0['x']) + sensor_gyro_0['y'] = median_filter(sensor_gyro_0['y']) + sensor_gyro_0['z'] = median_filter(sensor_gyro_0['z']) + # fit X axis if noResample: - coef_gyro_0_x = np.polyfit(temp_rel,sensor_gyro_0['x'],3) + coef_gyro_0_x = np.polyfit(temp_rel, sensor_gyro_0['x'], 3) else: - temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_0['x']) - coef_gyro_0_x = np.polyfit(temp, sens ,3) + temp, sens = resampleWithDeltaX(temp_rel, sensor_gyro_0['x']) + coef_gyro_0_x = np.polyfit(temp, sens, 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] @@ -200,10 +208,10 @@ if num_gyros >= 1 and not math.isnan(sensor_gyro_0['temperature'][0]): # fit Y axis if noResample: - coef_gyro_0_y = np.polyfit(temp_rel,sensor_gyro_0['y'],3) + coef_gyro_0_y = np.polyfit(temp_rel, sensor_gyro_0['y'], 3) else: temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_0['y']) - coef_gyro_0_y = np.polyfit(temp, sens ,3) + coef_gyro_0_y = np.polyfit(temp, sens, 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] @@ -214,9 +222,9 @@ if num_gyros >= 1 and not math.isnan(sensor_gyro_0['temperature'][0]): # fit Z axis if noResample: - coef_gyro_0_z = np.polyfit(temp_rel,sensor_gyro_0['z'],3) + coef_gyro_0_z = np.polyfit(temp_rel, sensor_gyro_0['z'],3) else: - temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_0['z']) + temp, sens = resampleWithDeltaX(temp_rel, sensor_gyro_0['z']) coef_gyro_0_z = np.polyfit(temp, sens ,3) gyro_0_params['TC_G0_X3_2'] = coef_gyro_0_z[0] @@ -292,6 +300,10 @@ if num_gyros >= 2 and not math.isnan(sensor_gyro_1['temperature'][0]): 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'] + sensor_gyro_1['x'] = median_filter(sensor_gyro_1['x']) + sensor_gyro_1['y'] = median_filter(sensor_gyro_1['y']) + sensor_gyro_1['z'] = median_filter(sensor_gyro_1['z']) + # fit X axis if noResample: coef_gyro_1_x = np.polyfit(temp_rel,sensor_gyro_1['x'],3) @@ -400,6 +412,10 @@ if num_gyros >= 3 and not math.isnan(sensor_gyro_2['temperature'][0]): 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'] + sensor_gyro_2['x'] = median_filter(sensor_gyro_2['x']) + sensor_gyro_2['y'] = median_filter(sensor_gyro_2['y']) + sensor_gyro_2['z'] = median_filter(sensor_gyro_2['z']) + # fit X axis if noResample: coef_gyro_2_x = np.polyfit(temp_rel,sensor_gyro_2['x'],3) @@ -416,10 +432,10 @@ if num_gyros >= 3 and not math.isnan(sensor_gyro_2['temperature'][0]): # fit Y axis if noResample: - coef_gyro_2_y = np.polyfit(temp_rel,sensor_gyro_2['y'],3) + coef_gyro_2_y = np.polyfit(temp_rel, sensor_gyro_2['y'], 3) else: - temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_2['y']) - coef_gyro_2_y = np.polyfit(temp, sens ,3) + temp, sens = resampleWithDeltaX(temp_rel, sensor_gyro_2['y']) + coef_gyro_2_y = np.polyfit(temp, sens, 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] @@ -430,10 +446,10 @@ if num_gyros >= 3 and not math.isnan(sensor_gyro_2['temperature'][0]): # fit Z axis if noResample: - coef_gyro_2_z = np.polyfit(temp_rel,sensor_gyro_2['z'],3) + coef_gyro_2_z = np.polyfit(temp_rel,sensor_gyro_2['z'], 3) else: temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_2['z']) - coef_gyro_2_z = np.polyfit(temp, sens ,3) + coef_gyro_2_z = np.polyfit(temp, sens, 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] @@ -508,8 +524,12 @@ if num_gyros >= 4 and not math.isnan(sensor_gyro_3['temperature'][0]): temp_rel_resample = np.linspace(gyro_3_params['TC_G3_TMIN']-gyro_3_params['TC_G3_TREF'], gyro_3_params['TC_G3_TMAX']-gyro_3_params['TC_G3_TREF'], 100) temp_resample = temp_rel_resample + gyro_3_params['TC_G3_TREF'] + sensor_gyro_3['x'] = median_filter(sensor_gyro_3['x']) + sensor_gyro_3['y'] = median_filter(sensor_gyro_3['y']) + sensor_gyro_3['z'] = median_filter(sensor_gyro_3['z']) + # fit X axis - coef_gyro_3_x = np.polyfit(temp_rel,sensor_gyro_3['x'],3) + coef_gyro_3_x = np.polyfit(temp_rel,sensor_gyro_3['x'], 3) gyro_3_params['TC_G3_X3_0'] = coef_gyro_3_x[0] gyro_3_params['TC_G3_X2_0'] = coef_gyro_3_x[1] gyro_3_params['TC_G3_X1_0'] = coef_gyro_3_x[2] @@ -518,7 +538,7 @@ if num_gyros >= 4 and not math.isnan(sensor_gyro_3['temperature'][0]): gyro_3_x_resample = fit_coef_gyro_3_x(temp_rel_resample) # fit Y axis - coef_gyro_3_y = np.polyfit(temp_rel,sensor_gyro_3['y'],3) + coef_gyro_3_y = np.polyfit(temp_rel,sensor_gyro_3['y'], 3) gyro_3_params['TC_G3_X3_1'] = coef_gyro_3_y[0] gyro_3_params['TC_G3_X2_1'] = coef_gyro_3_y[1] gyro_3_params['TC_G3_X1_1'] = coef_gyro_3_y[2] @@ -527,7 +547,7 @@ if num_gyros >= 4 and not math.isnan(sensor_gyro_3['temperature'][0]): gyro_3_y_resample = fit_coef_gyro_3_y(temp_rel_resample) # fit Z axis - coef_gyro_3_z = np.polyfit(temp_rel,sensor_gyro_3['z'],3) + coef_gyro_3_z = np.polyfit(temp_rel,sensor_gyro_3['z'], 3) gyro_3_params['TC_G3_X3_2'] = coef_gyro_3_z[0] gyro_3_params['TC_G3_X2_2'] = coef_gyro_3_z[1] gyro_3_params['TC_G3_X1_2'] = coef_gyro_3_z[2] @@ -540,8 +560,8 @@ if num_gyros >= 4 and not math.isnan(sensor_gyro_3['temperature'][0]): # draw plots plt.subplot(3,1,1) - plt.plot(sensor_gyro_3['temperature'],sensor_gyro_3['x'],'b') - plt.plot(temp_resample,gyro_3_x_resample,'r') + plt.plot(sensor_gyro_3['temperature'],sensor_gyro_3['x'], 'b') + plt.plot(temp_resample,gyro_3_x_resample, 'r') plt.title('Gyro 2 ({}) Bias vs Temperature'.format(gyro_3_params['TC_G3_ID'])) plt.ylabel('X bias (rad/s)') plt.xlabel('temperature (degC)') @@ -601,13 +621,17 @@ if num_accels >= 1 and not math.isnan(sensor_accel_0['temperature'][0]): 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'] + sensor_accel_0['x'] = median_filter(sensor_accel_0['x']) + sensor_accel_0['y'] = median_filter(sensor_accel_0['y']) + sensor_accel_0['z'] = median_filter(sensor_accel_0['z']) + # fit X axis correction_x = sensor_accel_0['x'] - np.median(sensor_accel_0['x']) if noResample: - coef_accel_0_x = np.polyfit(temp_rel,correction_x,3) + coef_accel_0_x = np.polyfit(temp_rel,correction_x, 3) else: temp, sens = resampleWithDeltaX(temp_rel,correction_x) - coef_accel_0_x = np.polyfit(temp, sens ,3) + coef_accel_0_x = np.polyfit(temp, sens, 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] @@ -617,12 +641,12 @@ if num_accels >= 1 and not math.isnan(sensor_accel_0['temperature'][0]): 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']) + correction_y = sensor_accel_0['y'] - np.median(sensor_accel_0['y']) if noResample: - coef_accel_0_y = np.polyfit(temp_rel,correction_y,3) + coef_accel_0_y = np.polyfit(temp_rel, correction_y, 3) else: temp, sens = resampleWithDeltaX(temp_rel,correction_y) - coef_accel_0_y = np.polyfit(temp, sens ,3) + coef_accel_0_y = np.polyfit(temp, sens, 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] @@ -632,12 +656,12 @@ if num_accels >= 1 and not math.isnan(sensor_accel_0['temperature'][0]): 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']) + correction_z = sensor_accel_0['z'] - np.median(sensor_accel_0['z']) if noResample: - coef_accel_0_z = np.polyfit(temp_rel,correction_z,3) + coef_accel_0_z = np.polyfit(temp_rel,correction_z, 3) else: temp, sens = resampleWithDeltaX(temp_rel,correction_z) - coef_accel_0_z = np.polyfit(temp, sens ,3) + coef_accel_0_z = np.polyfit(temp, sens, 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] @@ -712,13 +736,17 @@ if num_accels >= 2 and not math.isnan(sensor_accel_1['temperature'][0]): 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'] + sensor_accel_1['x'] = median_filter(sensor_accel_1['x']) + sensor_accel_1['y'] = median_filter(sensor_accel_1['y']) + sensor_accel_1['z'] = median_filter(sensor_accel_1['z']) + # fit X axis - correction_x = sensor_accel_1['x']-np.median(sensor_accel_1['x']) + correction_x = sensor_accel_1['x'] - np.median(sensor_accel_1['x']) if noResample: - coef_accel_1_x = np.polyfit(temp_rel,correction_x,3) + coef_accel_1_x = np.polyfit(temp_rel, correction_x, 3) else: - temp, sens = resampleWithDeltaX(temp_rel,correction_x) - coef_accel_1_x = np.polyfit(temp, sens ,3) + temp, sens = resampleWithDeltaX(temp_rel, correction_x) + coef_accel_1_x = np.polyfit(temp, sens, 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] @@ -728,7 +756,7 @@ if num_accels >= 2 and not math.isnan(sensor_accel_1['temperature'][0]): 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']) + correction_y = sensor_accel_1['y'] - np.median(sensor_accel_1['y']) if noResample: coef_accel_1_y = np.polyfit(temp_rel,correction_y,3) else: @@ -743,12 +771,12 @@ if num_accels >= 2 and not math.isnan(sensor_accel_1['temperature'][0]): 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']) + correction_z = sensor_accel_1['z'] - np.median(sensor_accel_1['z']) if noResample: - coef_accel_1_z = np.polyfit(temp_rel,correction_z,3) + coef_accel_1_z = np.polyfit(temp_rel,correction_z, 3) else: temp, sens = resampleWithDeltaX(temp_rel,correction_z) - coef_accel_1_z = np.polyfit(temp, sens ,3) + coef_accel_1_z = np.polyfit(temp, sens, 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] @@ -824,13 +852,17 @@ if num_accels >= 3 and not math.isnan(sensor_accel_2['temperature'][0]): 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'] + sensor_accel_2['x'] = median_filter(sensor_accel_2['x']) + sensor_accel_2['y'] = median_filter(sensor_accel_2['y']) + sensor_accel_2['z'] = median_filter(sensor_accel_2['z']) + # fit X axis - correction_x = sensor_accel_2['x']-np.median(sensor_accel_2['x']) + correction_x = sensor_accel_2['x'] - np.median(sensor_accel_2['x']) if noResample: - coef_accel_2_x = np.polyfit(temp_rel,correction_x,3) + coef_accel_2_x = np.polyfit(temp_rel,correction_x, 3) else: - temp, sens = resampleWithDeltaX(temp_rel,correction_x) - coef_accel_2_x = np.polyfit(temp, sens ,3) + temp, sens = resampleWithDeltaX(temp_rel, correction_x) + coef_accel_2_x = np.polyfit(temp, sens, 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] @@ -840,7 +872,7 @@ if num_accels >= 3 and not math.isnan(sensor_accel_2['temperature'][0]): 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']) + correction_y = sensor_accel_2['y'] - np.median(sensor_accel_2['y']) if noResample: coef_accel_2_y = np.polyfit(temp_rel,correction_y,3) else: @@ -855,7 +887,7 @@ if num_accels >= 3 and not math.isnan(sensor_accel_2['temperature'][0]): 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']) + correction_z = sensor_accel_2['z'] - np.median(sensor_accel_2['z']) if noResample: coef_accel_2_z = np.polyfit(temp_rel,correction_z,3) else: @@ -935,9 +967,13 @@ if num_accels >= 4 and not math.isnan(sensor_accel_3['temperature'][0]): temp_rel_resample = np.linspace(accel_3_params['TC_A3_TMIN']-accel_3_params['TC_A3_TREF'], accel_3_params['TC_A3_TMAX']-accel_3_params['TC_A3_TREF'], 100) temp_resample = temp_rel_resample + accel_3_params['TC_A3_TREF'] + sensor_accel_3['x'] = median_filter(sensor_accel_3['x']) + sensor_accel_3['y'] = median_filter(sensor_accel_3['y']) + sensor_accel_3['z'] = median_filter(sensor_accel_3['z']) + # fit X axis - correction_x = sensor_accel_3['x']-np.median(sensor_accel_3['x']) - coef_accel_3_x = np.polyfit(temp_rel,correction_x,3) + correction_x = sensor_accel_3['x'] - np.median(sensor_accel_3['x']) + coef_accel_3_x = np.polyfit(temp_rel, correction_x, 3) accel_3_params['TC_A3_X3_0'] = coef_accel_3_x[0] accel_3_params['TC_A3_X2_0'] = coef_accel_3_x[1] accel_3_params['TC_A3_X1_0'] = coef_accel_3_x[2] @@ -946,8 +982,8 @@ if num_accels >= 4 and not math.isnan(sensor_accel_3['temperature'][0]): correction_x_resample = fit_coef_accel_3_x(temp_rel_resample) # fit Y axis - correction_y = sensor_accel_3['y']-np.median(sensor_accel_3['y']) - coef_accel_3_y = np.polyfit(temp_rel,correction_y,3) + correction_y = sensor_accel_3['y'] - np.median(sensor_accel_3['y']) + coef_accel_3_y = np.polyfit(temp_rel, correction_y, 3) accel_3_params['TC_A3_X3_1'] = coef_accel_3_y[0] accel_3_params['TC_A3_X2_1'] = coef_accel_3_y[1] accel_3_params['TC_A3_X1_1'] = coef_accel_3_y[2] @@ -956,8 +992,8 @@ if num_accels >= 4 and not math.isnan(sensor_accel_3['temperature'][0]): correction_y_resample = fit_coef_accel_3_y(temp_rel_resample) # fit Z axis - correction_z = sensor_accel_3['z']-np.median(sensor_accel_3['z']) - coef_accel_3_z = np.polyfit(temp_rel,correction_z,3) + correction_z = sensor_accel_3['z'] - np.median(sensor_accel_3['z']) + coef_accel_3_z = np.polyfit(temp_rel, correction_z, 3) accel_3_params['TC_A3_X3_2'] = coef_accel_3_z[0] accel_3_params['TC_A3_X2_2'] = coef_accel_3_z[1] accel_3_params['TC_A3_X1_2'] = coef_accel_3_z[2] @@ -1024,8 +1060,10 @@ temp_rel = sensor_baro_0['temperature'] - baro_0_params['TC_B0_TREF'] temp_rel_resample = np.linspace(baro_0_params['TC_B0_TMIN']-baro_0_params['TC_B0_TREF'], baro_0_params['TC_B0_TMAX']-baro_0_params['TC_B0_TREF'], 100) temp_resample = temp_rel_resample + baro_0_params['TC_B0_TREF'] +sensor_baro_0['pressure'] = median_filter(sensor_baro_0['pressure']) + # fit data -median_pressure = np.median(sensor_baro_0['pressure']); +median_pressure = np.median(sensor_baro_0['pressure']) if noResample: coef_baro_0_x = np.polyfit(temp_rel,100*(sensor_baro_0['pressure']-median_pressure),5) # convert from hPa to Pa else: @@ -1081,8 +1119,10 @@ if num_baros >= 2: temp_rel_resample = np.linspace(baro_1_params['TC_B1_TMIN']-baro_1_params['TC_B1_TREF'], baro_1_params['TC_B1_TMAX']-baro_1_params['TC_B1_TREF'], 100) temp_resample = temp_rel_resample + baro_1_params['TC_B1_TREF'] + sensor_baro_1['pressure'] = median_filter(sensor_baro_1['pressure']) + # fit data - median_pressure = np.median(sensor_baro_1['pressure']); + median_pressure = np.median(sensor_baro_1['pressure']) if noResample: coef_baro_1_x = np.polyfit(temp_rel,100*(sensor_baro_1['pressure']-median_pressure),5) # convert from hPa to Pa else: @@ -1139,8 +1179,10 @@ if num_baros >= 3: temp_rel_resample = np.linspace(baro_2_params['TC_B2_TMIN']-baro_2_params['TC_B2_TREF'], baro_2_params['TC_B2_TMAX']-baro_2_params['TC_B2_TREF'], 100) temp_resample = temp_rel_resample + baro_2_params['TC_B2_TREF'] + sensor_baro_2['pressure'] = median_filter(sensor_baro_2['pressure']) + # fit data - median_pressure = np.median(sensor_baro_2['pressure']); + median_pressure = np.median(sensor_baro_2['pressure']) if noResample: coef_baro_2_x = np.polyfit(temp_rel,100*(sensor_baro_2['pressure']-median_pressure),5) # convert from hPa to Pa else: @@ -1197,6 +1239,8 @@ if num_baros >= 4: temp_rel_resample = np.linspace(baro_3_params['TC_B3_TMIN']-baro_3_params['TC_B3_TREF'], baro_3_params['TC_B3_TMAX']-baro_3_params['TC_B3_TREF'], 100) temp_resample = temp_rel_resample + baro_3_params['TC_B3_TREF'] + sensor_baro_3['pressure'] = median_filter(sensor_baro_3['pressure']) + # fit data median_pressure = np.median(sensor_baro_3['pressure']) coef_baro_3_x = np.polyfit(temp_rel,100*(sensor_baro_3['pressure']-median_pressure),5) # convert from hPa to Pa