yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

Tools/process_sensor_caldata.py - median filter sensor data

Browse Source 
- 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
1 changed files with 93 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
Tools/process_sensor_caldata.py
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,6 +188,10 @@ 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)
@@ -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)
@@ -508,6 +524,10 @@ 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)
gyro_3_params['TC_G3_X3_0'] = coef_gyro_3_x[0]
@@ -601,6 +621,10 @@ 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:
@@ -712,6 +736,10 @@ 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'])
if noResample:
@@ -743,7 +771,7 @@ 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)
else:
@@ -824,6 +852,10 @@ 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'])
if noResample:
@@ -935,6 +967,10 @@ 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)
@@ -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