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
This commit is contained in:
Daniel Agar
2021-05-02 13:42:09 -04:00
committed by GitHub
parent 5ec5a12f5e
commit b4e0a8396e

View File

@@ -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