mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-02-07 02:32:25 +08:00
e2864e521f
Some checks failed
Build all targets / Scan for Board Targets (push) Has been cancelled
Checks / build (NO_NINJA_BUILD=1 px4_fmu-v5_default) (push) Has been cancelled
Checks / build (NO_NINJA_BUILD=1 px4_sitl_default) (push) Has been cancelled
Checks / build (check_format) (push) Has been cancelled
Checks / build (check_newlines) (push) Has been cancelled
Checks / build (module_documentation) (push) Has been cancelled
Checks / build (px4_fmu-v2_default stack_check) (push) Has been cancelled
Checks / build (px4_sitl_allyes) (push) Has been cancelled
Checks / build (shellcheck_all) (push) Has been cancelled
Checks / build (tests) (push) Has been cancelled
Checks / build (tests_coverage) (push) Has been cancelled
Checks / build (validate_module_configs) (push) Has been cancelled
Clang Tidy / build (push) Has been cancelled
MacOS build / build (px4_fmu-v5_default) (push) Has been cancelled
MacOS build / build (px4_sitl) (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:22.04) (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:24.04) (push) Has been cancelled
Container build / Set Tags and Variables (push) Has been cancelled
EKF Update Change Indicator / unit_tests (push) Has been cancelled
Failsafe Simulator Build / build (failsafe_web) (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v5x (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v6x (push) Has been cancelled
ITCM check / Checking nxp_mr-tropic (push) Has been cancelled
ITCM check / Checking nxp_tropic-community (push) Has been cancelled
ITCM check / Checking px4_fmu-v5x (push) Has been cancelled
ITCM check / Checking px4_fmu-v6xrt (push) Has been cancelled
MAVROS Mission Tests / build (map[mission:MC_mission_box vehicle:iris]) (push) Has been cancelled
MAVROS Offboard Tests / build (map[test_file:mavros_posix_tests_offboard_posctl.test vehicle:iris]) (push) Has been cancelled
Nuttx Target with extra env config / build (px4_fmu-v5_default) (push) Has been cancelled
Python CI Checks / build (push) Has been cancelled
ROS Integration Tests / build (push) Has been cancelled
ROS Translation Node Tests / Build and test (map[ros_version:humble ubuntu:jammy]) (push) Has been cancelled
ROS Translation Node Tests / Build and test (map[ros_version:jazzy ubuntu:noble]) (push) Has been cancelled
SITL Tests / Testing PX4 tailsitter (push) Has been cancelled
SITL Tests / Testing PX4 iris (push) Has been cancelled
SITL Tests / Testing PX4 standard_vtol (push) Has been cancelled
Build all targets / Build [${{ matrix.runner }}][${{ matrix.group }}] (push) Has been cancelled
Build all targets / Upload Artifacts (push) Has been cancelled
Container build / Build Container (amd64) (push) Has been cancelled
Container build / Build Container (arm64) (push) Has been cancelled
Container build / Deploy To Registry (push) Has been cancelled
FLASH usage analysis / Publish Results (push) Has been cancelled
Fuzzing / Fuzzing (push) Has been cancelled
Handle stale issues and PRs / stale (push) Has been cancelled
Docs - Crowdin - Download Guide Translations / Synchronize with Crowdin (ko) (push) Has been cancelled
Docs - Crowdin - Download Guide Translations / Synchronize with Crowdin (uk) (push) Has been cancelled
Docs - Crowdin - Download Guide Translations / Synchronize with Crowdin (zh-CN) (push) Has been cancelled
2046 lines
72 KiB
Python
Executable File
2046 lines
72 KiB
Python
Executable File
#! /usr/bin/env python3
|
|
|
|
from __future__ import print_function
|
|
|
|
import argparse
|
|
import os
|
|
import math
|
|
import matplotlib.pyplot as plt
|
|
import numpy as np
|
|
|
|
from scipy.signal import medfilt
|
|
|
|
from pyulog import *
|
|
|
|
"""
|
|
Reads in IMU data from a static thermal calibration test and performs a curve fit of gyro, accel and baro bias vs temperature
|
|
Data can be gathered using the following sequence:
|
|
|
|
1) Power up the board and set the TC_A_ENABLE, TC_B_ENABLE and TC_G_ENABLE parameters to 1
|
|
2) Set all CAL_ACC*, CAL_GYR*, and CAL_MAG* parameters to defaults
|
|
3) Set the parameter SDLOG_MODE to 2, and SDLOG_PROFILE "Thermal calibration" bit (2) to enable logging of sensor data for calibration and power off
|
|
4) Cold soak the board for 30 minutes
|
|
5) Move to a warm dry, still air, constant pressure environment.
|
|
6) Apply power for 45 minutes, keeping the board still.
|
|
7) Remove power and extract the .ulog file
|
|
8) Open a terminal window in the Firmware/Tools directory and run the python calibration script script file: 'python3 process_sensor_caldata.py <full path name to .ulog file>
|
|
9) Power the board, connect QGC and load the parameter from the generated .params file onto the board using QGC. Due to the number of parameters, loading them may take some time.
|
|
10) TODO - we need a way for user to reliably tell when parameters have all been changed and saved.
|
|
11) After parameters have finished loading, set SDLOG_MODE and SDLOG_PROFILE to their respective values prior to step 4) and remove power.
|
|
12) Power the board and perform a normal gyro and accelerometer sensor calibration using QGC. The board must be repowered after this step before flying due to large parameter changes and the thermal compensation parameters only being read on startup.
|
|
|
|
Outputs thermal compensation parameters in a file named <inputfilename>.params which can be loaded onto the board using QGroundControl
|
|
Outputs summary plots in a pdf file named <inputfilename>.pdf
|
|
"""
|
|
|
|
def resampleWithDeltaX(x,y):
|
|
xMin = np.amin(x)
|
|
xMax = np.amax(x)
|
|
nbInterval = 2000
|
|
interval = (xMax-xMin)/nbInterval
|
|
|
|
resampledY = np.zeros(nbInterval)
|
|
resampledX = np.zeros(nbInterval)
|
|
resampledCount = np.zeros(nbInterval)
|
|
|
|
for idx in range(0,len(x)):
|
|
if x[idx]<xMin:
|
|
binIdx = 0
|
|
elif x[idx]<xMax:
|
|
binIdx = int((x[idx]-xMin)/(interval))
|
|
else:
|
|
binIdx = nbInterval-1
|
|
resampledY[binIdx] += y[idx]
|
|
resampledX[binIdx] += x[idx]
|
|
resampledCount[binIdx] += 1
|
|
|
|
idxNotEmpty = np.where(resampledCount != 0)
|
|
resampledCount = resampledCount[idxNotEmpty]
|
|
resampledY = resampledY[idxNotEmpty]
|
|
resampledX = resampledX[idxNotEmpty]
|
|
|
|
resampledY /= resampledCount
|
|
resampledX /= resampledCount
|
|
|
|
return resampledX,resampledY
|
|
|
|
def median_filter(data):
|
|
return 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',
|
|
const=True, default=False, help='skip resampling and use raw data')
|
|
|
|
def is_valid_directory(parser, arg):
|
|
if os.path.isdir(arg):
|
|
# Directory exists so return the directory
|
|
return arg
|
|
else:
|
|
parser.error('The directory {} does not exist'.format(arg))
|
|
|
|
args = parser.parse_args()
|
|
ulog_file_name = args.filename
|
|
noResample = args.noResample
|
|
|
|
ulog = ULog(ulog_file_name, None)
|
|
data = ulog.data_list
|
|
|
|
# extract accel data
|
|
num_accels = 0
|
|
for d in data:
|
|
if d.name == 'sensor_accel':
|
|
if d.multi_id == 0:
|
|
sensor_accel_0 = d.data
|
|
print('found accel 0 data')
|
|
num_accels += 1
|
|
elif d.multi_id == 1:
|
|
sensor_accel_1 = d.data
|
|
print('found accel 1 data')
|
|
num_accels += 1
|
|
elif d.multi_id == 2:
|
|
sensor_accel_2 = d.data
|
|
print('found accel 2 data')
|
|
num_accels += 1
|
|
elif d.multi_id == 3:
|
|
sensor_accel_3 = d.data
|
|
print('found accel 3 data')
|
|
num_accels += 1
|
|
|
|
# extract gyro data
|
|
num_gyros = 0
|
|
for d in data:
|
|
if d.name == 'sensor_gyro':
|
|
if d.multi_id == 0:
|
|
sensor_gyro_0 = d.data
|
|
print('found gyro 0 data')
|
|
num_gyros += 1
|
|
elif d.multi_id == 1:
|
|
sensor_gyro_1 = d.data
|
|
print('found gyro 1 data')
|
|
num_gyros += 1
|
|
elif d.multi_id == 2:
|
|
sensor_gyro_2 = d.data
|
|
print('found gyro 2 data')
|
|
num_gyros += 1
|
|
elif d.multi_id == 3:
|
|
sensor_gyro_3 = d.data
|
|
print('found gyro 3 data')
|
|
num_gyros += 1
|
|
|
|
# extract mag data
|
|
num_mags = 0
|
|
for d in data:
|
|
if d.name == 'sensor_mag':
|
|
if d.multi_id == 0:
|
|
sensor_mag_0 = d.data
|
|
print('found mag 0 data')
|
|
num_mags += 1
|
|
elif d.multi_id == 1:
|
|
sensor_mag_1 = d.data
|
|
print('found mag 1 data')
|
|
num_mags += 1
|
|
elif d.multi_id == 2:
|
|
sensor_mag_2 = d.data
|
|
print('found mag 2 data')
|
|
num_mags += 1
|
|
elif d.multi_id == 3:
|
|
sensor_mag_3 = d.data
|
|
print('found mag 3 data')
|
|
num_mags += 1
|
|
|
|
# extract baro data
|
|
num_baros = 0
|
|
for d in data:
|
|
if d.name == 'sensor_baro':
|
|
if d.multi_id == 0:
|
|
sensor_baro_0 = d.data
|
|
print('found baro 0 data')
|
|
num_baros += 1
|
|
elif d.multi_id == 1:
|
|
sensor_baro_1 = d.data
|
|
print('found baro 1 data')
|
|
num_baros += 1
|
|
elif d.multi_id == 2:
|
|
sensor_baro_2 = d.data
|
|
print('found baro 2 data')
|
|
num_baros += 1
|
|
elif d.multi_id == 3:
|
|
sensor_baro_3 = d.data
|
|
print('found baro 3 data')
|
|
num_baros += 1
|
|
|
|
# open file to save plots to PDF
|
|
from matplotlib.backends.backend_pdf import PdfPages
|
|
output_plot_filename = ulog_file_name + ".pdf"
|
|
pp = PdfPages(output_plot_filename)
|
|
|
|
|
|
|
|
#################################################################################
|
|
|
|
#################################################################################
|
|
|
|
# define data dictionary of accel 0 thermal correction parameters
|
|
accel_0_params = {
|
|
'TC_A0_ID':0,
|
|
'TC_A0_TMIN':0.0,
|
|
'TC_A0_TMAX':0.0,
|
|
'TC_A0_TREF':0.0,
|
|
'TC_A0_X0_0':0.0,
|
|
'TC_A0_X1_0':0.0,
|
|
'TC_A0_X2_0':0.0,
|
|
'TC_A0_X3_0':0.0,
|
|
'TC_A0_X0_1':0.0,
|
|
'TC_A0_X1_1':0.0,
|
|
'TC_A0_X2_1':0.0,
|
|
'TC_A0_X3_1':0.0,
|
|
'TC_A0_X0_2':0.0,
|
|
'TC_A0_X1_2':0.0,
|
|
'TC_A0_X2_2':0.0,
|
|
'TC_A0_X3_2':0.0
|
|
}
|
|
|
|
# curve fit the data for accel 0 corrections
|
|
if num_accels >= 1 and not math.isnan(sensor_accel_0['temperature'][0]):
|
|
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']
|
|
|
|
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)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,correction_x)
|
|
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]
|
|
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'])
|
|
if noResample:
|
|
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)
|
|
|
|
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'])
|
|
if noResample:
|
|
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)
|
|
|
|
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(1,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'.format(accel_0_params['TC_A0_ID']))
|
|
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,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()
|
|
|
|
#################################################################################
|
|
|
|
#################################################################################
|
|
|
|
# define data dictionary of accel 1 thermal correction parameters
|
|
accel_1_params = {
|
|
'TC_A1_ID':0,
|
|
'TC_A1_TMIN':0.0,
|
|
'TC_A1_TMAX':0.0,
|
|
'TC_A1_TREF':0.0,
|
|
'TC_A1_X0_0':0.0,
|
|
'TC_A1_X1_0':0.0,
|
|
'TC_A1_X2_0':0.0,
|
|
'TC_A1_X3_0':0.0,
|
|
'TC_A1_X0_1':0.0,
|
|
'TC_A1_X1_1':0.0,
|
|
'TC_A1_X2_1':0.0,
|
|
'TC_A1_X3_1':0.0,
|
|
'TC_A1_X0_2':0.0,
|
|
'TC_A1_X1_2':0.0,
|
|
'TC_A1_X2_2':0.0,
|
|
'TC_A1_X3_2':0.0
|
|
}
|
|
|
|
# curve fit the data for accel 1 corrections
|
|
if num_accels >= 2 and not math.isnan(sensor_accel_1['temperature'][0]):
|
|
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']
|
|
|
|
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:
|
|
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)
|
|
|
|
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'])
|
|
if noResample:
|
|
coef_accel_1_y = np.polyfit(temp_rel,correction_y,3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,correction_y)
|
|
coef_accel_1_y = np.polyfit(temp, sens ,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'])
|
|
if noResample:
|
|
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)
|
|
|
|
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(2,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'.format(accel_1_params['TC_A1_ID']))
|
|
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,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()
|
|
|
|
|
|
#################################################################################
|
|
|
|
#################################################################################
|
|
|
|
# define data dictionary of accel 2 thermal correction parameters
|
|
accel_2_params = {
|
|
'TC_A2_ID':0,
|
|
'TC_A2_TMIN':0.0,
|
|
'TC_A2_TMAX':0.0,
|
|
'TC_A2_TREF':0.0,
|
|
'TC_A2_X0_0':0.0,
|
|
'TC_A2_X1_0':0.0,
|
|
'TC_A2_X2_0':0.0,
|
|
'TC_A2_X3_0':0.0,
|
|
'TC_A2_X0_1':0.0,
|
|
'TC_A2_X1_1':0.0,
|
|
'TC_A2_X2_1':0.0,
|
|
'TC_A2_X3_1':0.0,
|
|
'TC_A2_X0_2':0.0,
|
|
'TC_A2_X1_2':0.0,
|
|
'TC_A2_X2_2':0.0,
|
|
'TC_A2_X3_2':0.0
|
|
}
|
|
|
|
# curve fit the data for accel 2 corrections
|
|
if num_accels >= 3 and not math.isnan(sensor_accel_2['temperature'][0]):
|
|
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']
|
|
|
|
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:
|
|
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)
|
|
|
|
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'])
|
|
if noResample:
|
|
coef_accel_2_y = np.polyfit(temp_rel,correction_y,3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,correction_y)
|
|
coef_accel_2_y = np.polyfit(temp, sens ,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'])
|
|
if noResample:
|
|
coef_accel_2_z = np.polyfit(temp_rel,correction_z,3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,correction_z)
|
|
coef_accel_2_z = np.polyfit(temp, sens ,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(3,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'.format(accel_2_params['TC_A2_ID']))
|
|
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,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()
|
|
|
|
#################################################################################
|
|
|
|
#################################################################################
|
|
|
|
# define data dictionary of accel 3 thermal correction parameters
|
|
accel_3_params = {
|
|
'TC_A3_ID':0,
|
|
'TC_A3_TMIN':0.0,
|
|
'TC_A3_TMAX':0.0,
|
|
'TC_A3_TREF':0.0,
|
|
'TC_A3_X0_0':0.0,
|
|
'TC_A3_X1_0':0.0,
|
|
'TC_A3_X2_0':0.0,
|
|
'TC_A3_X3_0':0.0,
|
|
'TC_A3_X0_1':0.0,
|
|
'TC_A3_X1_1':0.0,
|
|
'TC_A3_X2_1':0.0,
|
|
'TC_A3_X3_1':0.0,
|
|
'TC_A3_X0_2':0.0,
|
|
'TC_A3_X1_2':0.0,
|
|
'TC_A3_X2_2':0.0,
|
|
'TC_A3_X3_2':0.0
|
|
}
|
|
|
|
# curve fit the data for accel 2 corrections
|
|
if num_accels >= 4 and not math.isnan(sensor_accel_3['temperature'][0]):
|
|
accel_3_params['TC_A3_ID'] = int(np.median(sensor_accel_3['device_id']))
|
|
|
|
# find the min, max and reference temperature
|
|
accel_3_params['TC_A3_TMIN'] = np.amin(sensor_accel_3['temperature'])
|
|
accel_3_params['TC_A3_TMAX'] = np.amax(sensor_accel_3['temperature'])
|
|
accel_3_params['TC_A3_TREF'] = 0.5 * (accel_3_params['TC_A3_TMIN'] + accel_3_params['TC_A3_TMAX'])
|
|
|
|
temp_rel = sensor_accel_3['temperature'] - accel_3_params['TC_A3_TREF']
|
|
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)
|
|
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]
|
|
accel_3_params['TC_A3_X0_0'] = coef_accel_3_x[3]
|
|
fit_coef_accel_3_x = np.poly1d(coef_accel_3_x)
|
|
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)
|
|
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]
|
|
accel_3_params['TC_A3_X0_1'] = coef_accel_3_y[3]
|
|
fit_coef_accel_3_y = np.poly1d(coef_accel_3_y)
|
|
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)
|
|
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]
|
|
accel_3_params['TC_A3_X0_2'] = coef_accel_3_z[3]
|
|
fit_coef_accel_3_z = np.poly1d(coef_accel_3_z)
|
|
correction_z_resample = fit_coef_accel_3_z(temp_rel_resample)
|
|
|
|
# accel 3 vs temperature
|
|
plt.figure(4,figsize=(20,13))
|
|
|
|
# draw plots
|
|
plt.subplot(3,1,1)
|
|
plt.plot(sensor_accel_3['temperature'],correction_x,'b')
|
|
plt.plot(temp_resample,correction_x_resample,'r')
|
|
plt.title('Accel 3 ({}) Bias vs Temperature'.format(accel_3_params['TC_A3_ID']))
|
|
plt.ylabel('X bias (m/s/s)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
# draw plots
|
|
plt.subplot(3,1,2)
|
|
plt.plot(sensor_accel_3['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_3['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()
|
|
|
|
|
|
#################################################################################
|
|
|
|
#################################################################################
|
|
|
|
# define data dictionary of gyro 0 thermal correction parameters
|
|
gyro_0_params = {
|
|
'TC_G0_ID':0,
|
|
'TC_G0_TMIN':0.0,
|
|
'TC_G0_TMAX':0.0,
|
|
'TC_G0_TREF':0.0,
|
|
'TC_G0_X0_0':0.0,
|
|
'TC_G0_X1_0':0.0,
|
|
'TC_G0_X2_0':0.0,
|
|
'TC_G0_X3_0':0.0,
|
|
'TC_G0_X0_1':0.0,
|
|
'TC_G0_X1_1':0.0,
|
|
'TC_G0_X2_1':0.0,
|
|
'TC_G0_X3_1':0.0,
|
|
'TC_G0_X0_2':0.0,
|
|
'TC_G0_X1_2':0.0,
|
|
'TC_G0_X2_2':0.0,
|
|
'TC_G0_X3_2':0.0
|
|
}
|
|
|
|
# curve fit the data for gyro 0 corrections
|
|
if num_gyros >= 1:
|
|
|
|
#if the gyro has no temperature data, use the corresponding accel instance or the primary baro temp.
|
|
if math.isnan(sensor_gyro_0['temperature'][0]):
|
|
if not math.isnan(sensor_accel_0['temperature'][0]):
|
|
sensor_gyro_0['temperature'] = sensor_accel_0['temperature']
|
|
elif not math.isnan(sensor_baro_0['temperature'][0]):
|
|
sensor_gyro_0['temperature'] = sensor_baro_0['temperature']
|
|
|
|
if not math.isnan(sensor_gyro_0['temperature'][0]):
|
|
|
|
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']
|
|
|
|
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)
|
|
else:
|
|
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]
|
|
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
|
|
if noResample:
|
|
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)
|
|
|
|
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
|
|
if noResample:
|
|
coef_gyro_0_z = np.polyfit(temp_rel, sensor_gyro_0['z'],3)
|
|
else:
|
|
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]
|
|
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(5,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'.format(gyro_0_params['TC_G0_ID']))
|
|
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,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()
|
|
|
|
|
|
#################################################################################
|
|
|
|
#################################################################################
|
|
|
|
# define data dictionary of gyro 1 thermal correction parameters
|
|
gyro_1_params = {
|
|
'TC_G1_ID':0,
|
|
'TC_G1_TMIN':0.0,
|
|
'TC_G1_TMAX':0.0,
|
|
'TC_G1_TREF':0.0,
|
|
'TC_G1_X0_0':0.0,
|
|
'TC_G1_X1_0':0.0,
|
|
'TC_G1_X2_0':0.0,
|
|
'TC_G1_X3_0':0.0,
|
|
'TC_G1_X0_1':0.0,
|
|
'TC_G1_X1_1':0.0,
|
|
'TC_G1_X2_1':0.0,
|
|
'TC_G1_X3_1':0.0,
|
|
'TC_G1_X0_2':0.0,
|
|
'TC_G1_X1_2':0.0,
|
|
'TC_G1_X2_2':0.0,
|
|
'TC_G1_X3_2':0.0
|
|
}
|
|
|
|
# curve fit the data for gyro 1 corrections
|
|
if num_gyros >= 2:
|
|
|
|
#if the gyro has no temperature data, use the corresponding accel instance or the primary baro temp.
|
|
if math.isnan(sensor_gyro_1['temperature'][0]):
|
|
if not math.isnan(sensor_accel_1['temperature'][0]):
|
|
sensor_gyro_1['temperature'] = sensor_accel_1['temperature']
|
|
elif not math.isnan(sensor_baro_0['temperature'][0]):
|
|
sensor_gyro_1['temperature'] = sensor_baro_0['temperature']
|
|
|
|
if not math.isnan(sensor_gyro_1['temperature'][0]):
|
|
|
|
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']
|
|
|
|
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)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_1['x'])
|
|
coef_gyro_1_x = np.polyfit(temp, sens ,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
|
|
if noResample:
|
|
coef_gyro_1_y = np.polyfit(temp_rel,sensor_gyro_1['y'],3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_1['y'])
|
|
coef_gyro_1_y = np.polyfit(temp, sens ,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
|
|
if noResample:
|
|
coef_gyro_1_z = np.polyfit(temp_rel,sensor_gyro_1['z'],3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_1['z'])
|
|
coef_gyro_1_z = np.polyfit(temp, sens ,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(6,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'.format(gyro_1_params['TC_G1_ID']))
|
|
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,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()
|
|
|
|
#################################################################################
|
|
|
|
#################################################################################
|
|
|
|
# define data dictionary of gyro 2 thermal correction parameters
|
|
gyro_2_params = {
|
|
'TC_G2_ID':0,
|
|
'TC_G2_TMIN':0.0,
|
|
'TC_G2_TMAX':0.0,
|
|
'TC_G2_TREF':0.0,
|
|
'TC_G2_X0_0':0.0,
|
|
'TC_G2_X1_0':0.0,
|
|
'TC_G2_X2_0':0.0,
|
|
'TC_G2_X3_0':0.0,
|
|
'TC_G2_X0_1':0.0,
|
|
'TC_G2_X1_1':0.0,
|
|
'TC_G2_X2_1':0.0,
|
|
'TC_G2_X3_1':0.0,
|
|
'TC_G2_X0_2':0.0,
|
|
'TC_G2_X1_2':0.0,
|
|
'TC_G2_X2_2':0.0,
|
|
'TC_G2_X3_2':0.0
|
|
}
|
|
|
|
# curve fit the data for gyro 2 corrections
|
|
if num_gyros >= 3:
|
|
|
|
#if the gyro has no temperature data, use the corresponding accel instance or the primary baro temp.
|
|
if math.isnan(sensor_gyro_2['temperature'][0]):
|
|
if not math.isnan(sensor_accel_2['temperature'][0]):
|
|
sensor_gyro_2['temperature'] = sensor_accel_2['temperature']
|
|
elif not math.isnan(sensor_baro_0['temperature'][0]):
|
|
sensor_gyro_2['temperature'] = sensor_baro_0['temperature']
|
|
|
|
if not math.isnan(sensor_gyro_2['temperature'][0]):
|
|
|
|
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']
|
|
|
|
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)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_2['x'])
|
|
coef_gyro_2_x = np.polyfit(temp, sens ,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
|
|
if noResample:
|
|
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)
|
|
|
|
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
|
|
if noResample:
|
|
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)
|
|
|
|
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(7,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'.format(gyro_2_params['TC_G2_ID']))
|
|
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,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()
|
|
|
|
#################################################################################
|
|
|
|
#################################################################################
|
|
|
|
# define data dictionary of gyro 3 thermal correction parameters
|
|
gyro_3_params = {
|
|
'TC_G3_ID':0,
|
|
'TC_G3_TMIN':0.0,
|
|
'TC_G3_TMAX':0.0,
|
|
'TC_G3_TREF':0.0,
|
|
'TC_G3_X0_0':0.0,
|
|
'TC_G3_X1_0':0.0,
|
|
'TC_G3_X2_0':0.0,
|
|
'TC_G3_X3_0':0.0,
|
|
'TC_G3_X0_1':0.0,
|
|
'TC_G3_X1_1':0.0,
|
|
'TC_G3_X2_1':0.0,
|
|
'TC_G3_X3_1':0.0,
|
|
'TC_G3_X0_2':0.0,
|
|
'TC_G3_X1_2':0.0,
|
|
'TC_G3_X2_2':0.0,
|
|
'TC_G3_X3_2':0.0
|
|
}
|
|
|
|
# curve fit the data for gyro 3 corrections
|
|
if num_gyros >= 4:
|
|
|
|
#if the gyro has no temperature data, use the corresponding accel instance or the primary baro temp.
|
|
if math.isnan(sensor_gyro_3['temperature'][0]):
|
|
if not math.isnan(sensor_accel_3['temperature'][0]):
|
|
sensor_gyro_3['temperature'] = sensor_accel_3['temperature']
|
|
elif not math.isnan(sensor_baro_0['temperature'][0]):
|
|
sensor_gyro_3['temperature'] = sensor_baro_0['temperature']
|
|
|
|
if not math.isnan(sensor_gyro_3['temperature'][0]):
|
|
|
|
gyro_3_params['TC_G3_ID'] = int(np.median(sensor_gyro_3['device_id']))
|
|
|
|
# find the min, max and reference temperature
|
|
gyro_3_params['TC_G3_TMIN'] = np.amin(sensor_gyro_3['temperature'])
|
|
gyro_3_params['TC_G3_TMAX'] = np.amax(sensor_gyro_3['temperature'])
|
|
gyro_3_params['TC_G3_TREF'] = 0.5 * (gyro_2_params['TC_G3_TMIN'] + gyro_2_params['TC_G3_TMAX'])
|
|
|
|
temp_rel = sensor_gyro_3['temperature'] - gyro_2_params['TC_G3_TREF']
|
|
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]
|
|
gyro_3_params['TC_G3_X2_0'] = coef_gyro_3_x[1]
|
|
gyro_3_params['TC_G3_X1_0'] = coef_gyro_3_x[2]
|
|
gyro_3_params['TC_G3_X0_0'] = coef_gyro_3_x[3]
|
|
fit_coef_gyro_3_x = np.poly1d(coef_gyro_3_x)
|
|
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)
|
|
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]
|
|
gyro_3_params['TC_G3_X0_1'] = coef_gyro_3_y[3]
|
|
fit_coef_gyro_3_y = np.poly1d(coef_gyro_3_y)
|
|
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)
|
|
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]
|
|
gyro_3_params['TC_G3_X0_2'] = coef_gyro_3_z[3]
|
|
fit_coef_gyro_3_z = np.poly1d(coef_gyro_3_z)
|
|
gyro_3_z_resample = fit_coef_gyro_3_z(temp_rel_resample)
|
|
|
|
# gyro3 vs temperature
|
|
plt.figure(8,figsize=(20,13))
|
|
|
|
# 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.title('Gyro 2 ({}) Bias vs Temperature'.format(gyro_3_params['TC_G3_ID']))
|
|
plt.ylabel('X bias (rad/s)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
# draw plots
|
|
plt.subplot(3,1,2)
|
|
plt.plot(sensor_gyro_3['temperature'],sensor_gyro_3['y'],'b')
|
|
plt.plot(temp_resample,gyro_3_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_3['temperature'],sensor_gyro_3['z'],'b')
|
|
plt.plot(temp_resample,gyro_3_z_resample,'r')
|
|
plt.ylabel('Z bias (rad/s)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
pp.savefig()
|
|
|
|
#################################################################################
|
|
|
|
#################################################################################
|
|
|
|
# define data dictionary of mag 0 thermal correction parameters
|
|
mag_0_params = {
|
|
'TC_M0_ID':0,
|
|
'TC_M0_TMIN':0.0,
|
|
'TC_M0_TMAX':0.0,
|
|
'TC_M0_TREF':0.0,
|
|
'TC_M0_X0_0':0.0,
|
|
'TC_M0_X1_0':0.0,
|
|
'TC_M0_X2_0':0.0,
|
|
'TC_M0_X3_0':0.0,
|
|
'TC_M0_X0_1':0.0,
|
|
'TC_M0_X1_1':0.0,
|
|
'TC_M0_X2_1':0.0,
|
|
'TC_M0_X3_1':0.0,
|
|
'TC_M0_X0_2':0.0,
|
|
'TC_M0_X1_2':0.0,
|
|
'TC_M0_X2_2':0.0,
|
|
'TC_M0_X3_2':0.0
|
|
}
|
|
|
|
# curve fit the data for mag 0 corrections
|
|
if num_mags >= 1:
|
|
if math.isnan(sensor_mag_0['temperature'][0]):
|
|
if not math.isnan(sensor_baro_0['temperature'][0]):
|
|
sensor_mag_0['temperature'] = sensor_baro_0['temperature']
|
|
|
|
if not math.isnan(sensor_mag_0['temperature'][0]):
|
|
|
|
mag_0_params['TC_M0_ID'] = int(np.median(sensor_mag_0['device_id']))
|
|
|
|
# find the min, max and reference temperature
|
|
mag_0_params['TC_M0_TMIN'] = np.amin(sensor_mag_0['temperature'])
|
|
mag_0_params['TC_M0_TMAX'] = np.amax(sensor_mag_0['temperature'])
|
|
mag_0_params['TC_M0_TREF'] = 0.5 * (mag_0_params['TC_M0_TMIN'] + mag_0_params['TC_M0_TMAX'])
|
|
|
|
temp_rel = sensor_mag_0['temperature'] - mag_0_params['TC_M0_TREF']
|
|
temp_rel_resample = np.linspace(mag_0_params['TC_M0_TMIN']-mag_0_params['TC_M0_TREF'], mag_0_params['TC_M0_TMAX']-mag_0_params['TC_M0_TREF'], 100)
|
|
temp_resample = temp_rel_resample + mag_0_params['TC_M0_TREF']
|
|
|
|
sensor_mag_0['x'] = median_filter(sensor_mag_0['x'])
|
|
sensor_mag_0['y'] = median_filter(sensor_mag_0['y'])
|
|
sensor_mag_0['z'] = median_filter(sensor_mag_0['z'])
|
|
|
|
# fit X axis
|
|
correction_x = sensor_mag_0['x'] - np.median(sensor_mag_0['x'])
|
|
|
|
if noResample:
|
|
coef_mag_0_x = np.polyfit(temp_rel,correction_x, 3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,correction_x)
|
|
coef_mag_0_x = np.polyfit(temp, sens, 3)
|
|
|
|
mag_0_params['TC_M0_X3_0'] = coef_mag_0_x[0]
|
|
mag_0_params['TC_M0_X2_0'] = coef_mag_0_x[1]
|
|
mag_0_params['TC_M0_X1_0'] = coef_mag_0_x[2]
|
|
mag_0_params['TC_M0_X0_0'] = coef_mag_0_x[3]
|
|
|
|
fit_coef_mag_0_x = np.poly1d(coef_mag_0_x)
|
|
correction_x_resample = fit_coef_mag_0_x(temp_rel_resample)
|
|
|
|
# fit Y axis
|
|
correction_y = sensor_mag_0['y'] - np.median(sensor_mag_0['y'])
|
|
|
|
if noResample:
|
|
coef_mag_0_y = np.polyfit(temp_rel, correction_y, 3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,correction_y)
|
|
coef_mag_0_y = np.polyfit(temp, sens, 3)
|
|
|
|
mag_0_params['TC_M0_X3_1'] = coef_mag_0_y[0]
|
|
mag_0_params['TC_M0_X2_1'] = coef_mag_0_y[1]
|
|
mag_0_params['TC_M0_X1_1'] = coef_mag_0_y[2]
|
|
mag_0_params['TC_M0_X0_1'] = coef_mag_0_y[3]
|
|
|
|
fit_coef_mag_0_y = np.poly1d(coef_mag_0_y)
|
|
correction_y_resample = fit_coef_mag_0_y(temp_rel_resample)
|
|
|
|
# fit Z axis
|
|
correction_z = sensor_mag_0['z'] - np.median(sensor_mag_0['z'])
|
|
|
|
if noResample:
|
|
coef_mag_0_z = np.polyfit(temp_rel,correction_z, 3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,correction_z)
|
|
coef_mag_0_z = np.polyfit(temp, sens, 3)
|
|
|
|
mag_0_params['TC_M0_X3_2'] = coef_mag_0_z[0]
|
|
mag_0_params['TC_M0_X2_2'] = coef_mag_0_z[1]
|
|
mag_0_params['TC_M0_X1_2'] = coef_mag_0_z[2]
|
|
mag_0_params['TC_M0_X0_2'] = coef_mag_0_z[3]
|
|
|
|
fit_coef_mag_0_z = np.poly1d(coef_mag_0_z)
|
|
correction_z_resample = fit_coef_mag_0_z(temp_rel_resample)
|
|
|
|
# mag 0 vs temperature
|
|
plt.figure(9,figsize=(20,13))
|
|
|
|
# draw plots
|
|
plt.subplot(3,1,1)
|
|
plt.plot(sensor_mag_0['temperature'],correction_x,'b')
|
|
plt.plot(temp_resample,correction_x_resample,'r')
|
|
plt.title('Mag 0 ({}) Bias vs Temperature'.format(mag_0_params['TC_M0_ID']))
|
|
plt.ylabel('X bias (Gauss)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
# draw plots
|
|
plt.subplot(3,1,2)
|
|
plt.plot(sensor_mag_0['temperature'],correction_y,'b')
|
|
plt.plot(temp_resample,correction_y_resample,'r')
|
|
plt.ylabel('Y bias (Gauss)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
# draw plots
|
|
plt.subplot(3,1,3)
|
|
plt.plot(sensor_mag_0['temperature'],correction_z,'b')
|
|
plt.plot(temp_resample,correction_z_resample,'r')
|
|
plt.ylabel('Z bias (Gauss)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
pp.savefig()
|
|
|
|
#################################################################################
|
|
|
|
#################################################################################
|
|
|
|
# define data dictionary of mag 1 thermal correction parameters
|
|
mag_1_params = {
|
|
'TC_M1_ID':0,
|
|
'TC_M1_TMIN':0.0,
|
|
'TC_M1_TMAX':0.0,
|
|
'TC_M1_TREF':0.0,
|
|
'TC_M1_X0_0':0.0,
|
|
'TC_M1_X1_0':0.0,
|
|
'TC_M1_X2_0':0.0,
|
|
'TC_M1_X3_0':0.0,
|
|
'TC_M1_X0_1':0.0,
|
|
'TC_M1_X1_1':0.0,
|
|
'TC_M1_X2_1':0.0,
|
|
'TC_M1_X3_1':0.0,
|
|
'TC_M1_X0_2':0.0,
|
|
'TC_M1_X1_2':0.0,
|
|
'TC_M1_X2_2':0.0,
|
|
'TC_M1_X3_2':0.0
|
|
}
|
|
|
|
# curve fit the data for mag 1 corrections
|
|
if num_mags >= 2:
|
|
if math.isnan(sensor_mag_1['temperature'][0]):
|
|
if not math.isnan(sensor_baro_0['temperature'][0]):
|
|
sensor_mag_1['temperature'] = sensor_baro_0['temperature']
|
|
|
|
if not math.isnan(sensor_mag_1['temperature'][0]):
|
|
|
|
mag_1_params['TC_M1_ID'] = int(np.median(sensor_mag_1['device_id']))
|
|
|
|
# find the min, max and reference temperature
|
|
mag_1_params['TC_M1_TMIN'] = np.amin(sensor_mag_1['temperature'])
|
|
mag_1_params['TC_M1_TMAX'] = np.amax(sensor_mag_1['temperature'])
|
|
mag_1_params['TC_M1_TREF'] = 0.5 * (mag_1_params['TC_M1_TMIN'] + mag_1_params['TC_M1_TMAX'])
|
|
|
|
temp_rel = sensor_mag_1['temperature'] - mag_1_params['TC_M1_TREF']
|
|
temp_rel_resample = np.linspace(mag_1_params['TC_M1_TMIN']-mag_1_params['TC_M1_TREF'], mag_1_params['TC_M1_TMAX']-mag_1_params['TC_M1_TREF'], 100)
|
|
temp_resample = temp_rel_resample + mag_1_params['TC_M1_TREF']
|
|
|
|
sensor_mag_1['x'] = median_filter(sensor_mag_1['x'])
|
|
sensor_mag_1['y'] = median_filter(sensor_mag_1['y'])
|
|
sensor_mag_1['z'] = median_filter(sensor_mag_1['z'])
|
|
|
|
# fit X axis
|
|
correction_x = sensor_mag_1['x'] - np.median(sensor_mag_1['x'])
|
|
|
|
if noResample:
|
|
coef_mag_1_x = np.polyfit(temp_rel, correction_x, 3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel, correction_x)
|
|
coef_mag_1_x = np.polyfit(temp, sens, 3)
|
|
|
|
mag_1_params['TC_M1_X3_0'] = coef_mag_1_x[0]
|
|
mag_1_params['TC_M1_X2_0'] = coef_mag_1_x[1]
|
|
mag_1_params['TC_M1_X1_0'] = coef_mag_1_x[2]
|
|
mag_1_params['TC_M1_X0_0'] = coef_mag_1_x[3]
|
|
|
|
fit_coef_mag_1_x = np.poly1d(coef_mag_1_x)
|
|
correction_x_resample = fit_coef_mag_1_x(temp_rel_resample)
|
|
|
|
# fit Y axis
|
|
correction_y = sensor_mag_1['y'] - np.median(sensor_mag_1['y'])
|
|
|
|
if noResample:
|
|
coef_mag_1_y = np.polyfit(temp_rel,correction_y,3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,correction_y)
|
|
coef_mag_1_y = np.polyfit(temp, sens ,3)
|
|
|
|
mag_1_params['TC_M1_X3_1'] = coef_mag_1_y[0]
|
|
mag_1_params['TC_M1_X2_1'] = coef_mag_1_y[1]
|
|
mag_1_params['TC_M1_X1_1'] = coef_mag_1_y[2]
|
|
mag_1_params['TC_M1_X0_1'] = coef_mag_1_y[3]
|
|
|
|
fit_coef_mag_1_y = np.poly1d(coef_mag_1_y)
|
|
correction_y_resample = fit_coef_mag_1_y(temp_rel_resample)
|
|
|
|
# fit Z axis
|
|
correction_z = sensor_mag_1['z'] - np.median(sensor_mag_1['z'])
|
|
|
|
if noResample:
|
|
coef_mag_1_z = np.polyfit(temp_rel,correction_z, 3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,correction_z)
|
|
coef_mag_1_z = np.polyfit(temp, sens, 3)
|
|
|
|
mag_1_params['TC_M1_X3_2'] = coef_mag_1_z[0]
|
|
mag_1_params['TC_M1_X2_2'] = coef_mag_1_z[1]
|
|
mag_1_params['TC_M1_X1_2'] = coef_mag_1_z[2]
|
|
mag_1_params['TC_M1_X0_2'] = coef_mag_1_z[3]
|
|
|
|
fit_coef_mag_1_z = np.poly1d(coef_mag_1_z)
|
|
correction_z_resample = fit_coef_mag_1_z(temp_rel_resample)
|
|
|
|
# mag 1 vs temperature
|
|
plt.figure(10,figsize=(20,13))
|
|
|
|
# draw plots
|
|
plt.subplot(3,1,1)
|
|
plt.plot(sensor_mag_1['temperature'],correction_x,'b')
|
|
plt.plot(temp_resample,correction_x_resample,'r')
|
|
plt.title('Mag 1 ({}) Bias vs Temperature'.format(mag_1_params['TC_M1_ID']))
|
|
plt.ylabel('X bias (Gauss)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
# draw plots
|
|
plt.subplot(3,1,2)
|
|
plt.plot(sensor_mag_1['temperature'],correction_y,'b')
|
|
plt.plot(temp_resample,correction_y_resample,'r')
|
|
plt.ylabel('Y bias (Gauss)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
# draw plots
|
|
plt.subplot(3,1,3)
|
|
plt.plot(sensor_mag_1['temperature'],correction_z,'b')
|
|
plt.plot(temp_resample,correction_z_resample,'r')
|
|
plt.ylabel('Z bias (Gauss)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
pp.savefig()
|
|
|
|
|
|
#################################################################################
|
|
|
|
#################################################################################
|
|
|
|
# define data dictionary of mag 2 thermal correction parameters
|
|
mag_2_params = {
|
|
'TC_M2_ID':0,
|
|
'TC_M2_TMIN':0.0,
|
|
'TC_M2_TMAX':0.0,
|
|
'TC_M2_TREF':0.0,
|
|
'TC_M2_X0_0':0.0,
|
|
'TC_M2_X1_0':0.0,
|
|
'TC_M2_X2_0':0.0,
|
|
'TC_M2_X3_0':0.0,
|
|
'TC_M2_X0_1':0.0,
|
|
'TC_M2_X1_1':0.0,
|
|
'TC_M2_X2_1':0.0,
|
|
'TC_M2_X3_1':0.0,
|
|
'TC_M2_X0_2':0.0,
|
|
'TC_M2_X1_2':0.0,
|
|
'TC_M2_X2_2':0.0,
|
|
'TC_M2_X3_2':0.0
|
|
}
|
|
|
|
# curve fit the data for mag 2 corrections
|
|
if num_mags >= 3:
|
|
if math.isnan(sensor_mag_2['temperature'][0]):
|
|
if not math.isnan(sensor_baro_0['temperature'][0]):
|
|
sensor_mag_2['temperature'] = sensor_baro_0['temperature']
|
|
|
|
if not math.isnan(sensor_mag_2['temperature'][0]):
|
|
|
|
mag_2_params['TC_M2_ID'] = int(np.median(sensor_mag_2['device_id']))
|
|
|
|
# find the min, max and reference temperature
|
|
mag_2_params['TC_M2_TMIN'] = np.amin(sensor_mag_2['temperature'])
|
|
mag_2_params['TC_M2_TMAX'] = np.amax(sensor_mag_2['temperature'])
|
|
mag_2_params['TC_M2_TREF'] = 0.5 * (mag_2_params['TC_M2_TMIN'] + mag_2_params['TC_M2_TMAX'])
|
|
|
|
temp_rel = sensor_mag_2['temperature'] - mag_2_params['TC_M2_TREF']
|
|
temp_rel_resample = np.linspace(mag_2_params['TC_M2_TMIN']-mag_2_params['TC_M2_TREF'], mag_2_params['TC_M2_TMAX']-mag_2_params['TC_M2_TREF'], 100)
|
|
temp_resample = temp_rel_resample + mag_2_params['TC_M2_TREF']
|
|
|
|
sensor_mag_2['x'] = median_filter(sensor_mag_2['x'])
|
|
sensor_mag_2['y'] = median_filter(sensor_mag_2['y'])
|
|
sensor_mag_2['z'] = median_filter(sensor_mag_2['z'])
|
|
|
|
# fit X axis
|
|
correction_x = sensor_mag_2['x'] - np.median(sensor_mag_2['x'])
|
|
|
|
if noResample:
|
|
coef_mag_2_x = np.polyfit(temp_rel,correction_x, 3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel, correction_x)
|
|
coef_mag_2_x = np.polyfit(temp, sens, 3)
|
|
|
|
mag_2_params['TC_M2_X3_0'] = coef_mag_2_x[0]
|
|
mag_2_params['TC_M2_X2_0'] = coef_mag_2_x[1]
|
|
mag_2_params['TC_M2_X1_0'] = coef_mag_2_x[2]
|
|
mag_2_params['TC_M2_X0_0'] = coef_mag_2_x[3]
|
|
|
|
fit_coef_mag_2_x = np.poly1d(coef_mag_2_x)
|
|
correction_x_resample = fit_coef_mag_2_x(temp_rel_resample)
|
|
|
|
# fit Y axis
|
|
correction_y = sensor_mag_2['y'] - np.median(sensor_mag_2['y'])
|
|
|
|
if noResample:
|
|
coef_mag_2_y = np.polyfit(temp_rel,correction_y,3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,correction_y)
|
|
coef_mag_2_y = np.polyfit(temp, sens ,3)
|
|
|
|
mag_2_params['TC_M2_X3_1'] = coef_mag_2_y[0]
|
|
mag_2_params['TC_M2_X2_1'] = coef_mag_2_y[1]
|
|
mag_2_params['TC_M2_X1_1'] = coef_mag_2_y[2]
|
|
mag_2_params['TC_M2_X0_1'] = coef_mag_2_y[3]
|
|
|
|
fit_coef_mag_2_y = np.poly1d(coef_mag_2_y)
|
|
correction_y_resample = fit_coef_mag_2_y(temp_rel_resample)
|
|
|
|
# fit Z axis
|
|
correction_z = sensor_mag_2['z'] - np.median(sensor_mag_2['z'])
|
|
|
|
if noResample:
|
|
coef_mag_2_z = np.polyfit(temp_rel,correction_z,3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,correction_z)
|
|
coef_mag_2_z = np.polyfit(temp, sens ,3)
|
|
|
|
mag_2_params['TC_M2_X3_2'] = coef_mag_2_z[0]
|
|
mag_2_params['TC_M2_X2_2'] = coef_mag_2_z[1]
|
|
mag_2_params['TC_M2_X1_2'] = coef_mag_2_z[2]
|
|
mag_2_params['TC_M2_X0_2'] = coef_mag_2_z[3]
|
|
|
|
fit_coef_mag_2_z = np.poly1d(coef_mag_2_z)
|
|
correction_z_resample = fit_coef_mag_2_z(temp_rel_resample)
|
|
|
|
# mag 2 vs temperature
|
|
plt.figure(11,figsize=(20,13))
|
|
|
|
# draw plots
|
|
plt.subplot(3,1,1)
|
|
plt.plot(sensor_mag_2['temperature'],correction_x,'b')
|
|
plt.plot(temp_resample,correction_x_resample,'r')
|
|
plt.title('Mag 2 ({}) Bias vs Temperature'.format(mag_2_params['TC_M2_ID']))
|
|
plt.ylabel('X bias (Gauss)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
# draw plots
|
|
plt.subplot(3,1,2)
|
|
plt.plot(sensor_mag_2['temperature'],correction_y,'b')
|
|
plt.plot(temp_resample,correction_y_resample,'r')
|
|
plt.ylabel('Y bias (Gauss)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
# draw plots
|
|
plt.subplot(3,1,3)
|
|
plt.plot(sensor_mag_2['temperature'],correction_z,'b')
|
|
plt.plot(temp_resample,correction_z_resample,'r')
|
|
plt.ylabel('Z bias (Gauss)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
pp.savefig()
|
|
|
|
#################################################################################
|
|
|
|
#################################################################################
|
|
|
|
# define data dictionary of mag 3 thermal correction parameters
|
|
mag_3_params = {
|
|
'TC_M3_ID':0,
|
|
'TC_M3_TMIN':0.0,
|
|
'TC_M3_TMAX':0.0,
|
|
'TC_M3_TREF':0.0,
|
|
'TC_M3_X0_0':0.0,
|
|
'TC_M3_X1_0':0.0,
|
|
'TC_M3_X2_0':0.0,
|
|
'TC_M3_X3_0':0.0,
|
|
'TC_M3_X0_1':0.0,
|
|
'TC_M3_X1_1':0.0,
|
|
'TC_M3_X2_1':0.0,
|
|
'TC_M3_X3_1':0.0,
|
|
'TC_M3_X0_2':0.0,
|
|
'TC_M3_X1_2':0.0,
|
|
'TC_M3_X2_2':0.0,
|
|
'TC_M3_X3_2':0.0
|
|
}
|
|
|
|
# curve fit the data for mag 3 corrections
|
|
if num_mags >= 4:
|
|
if math.isnan(sensor_mag_3['temperature'][0]):
|
|
if not math.isnan(sensor_baro_0['temperature'][0]):
|
|
sensor_mag_3['temperature'] = sensor_baro_0['temperature']
|
|
|
|
if not math.isnan(sensor_mag_3['temperature'][0]):
|
|
|
|
mag_3_params['TC_M3_ID'] = int(np.median(sensor_mag_3['device_id']))
|
|
|
|
# find the min, max and reference temperature
|
|
mag_3_params['TC_M3_TMIN'] = np.amin(sensor_mag_3['temperature'])
|
|
mag_3_params['TC_M3_TMAX'] = np.amax(sensor_mag_3['temperature'])
|
|
mag_3_params['TC_M3_TREF'] = 0.5 * (mag_3_params['TC_M3_TMIN'] + mag_3_params['TC_M3_TMAX'])
|
|
|
|
temp_rel = sensor_mag_3['temperature'] - mag_3_params['TC_M3_TREF']
|
|
temp_rel_resample = np.linspace(mag_3_params['TC_M3_TMIN']-mag_3_params['TC_M3_TREF'], mag_3_params['TC_M3_TMAX']-mag_3_params['TC_M3_TREF'], 100)
|
|
temp_resample = temp_rel_resample + mag_3_params['TC_M3_TREF']
|
|
|
|
# Delete the dataset first and last 10 seconds of data
|
|
sensor_mag_3 = np.delete(sensor_mag_3, range(0,1000), axis=['x'])
|
|
sensor_mag_3 = np.delete(sensor_mag_3, range(-1000,), axis=['x'])
|
|
|
|
sensor_mag_3['x'] = median_filter(sensor_mag_3['x'])
|
|
sensor_mag_3['y'] = median_filter(sensor_mag_3['y'])
|
|
sensor_mag_3['z'] = median_filter(sensor_mag_3['z'])
|
|
|
|
# fit X axis
|
|
correction_x = sensor_mag_3['x'] - np.median(sensor_mag_3['x'])
|
|
|
|
if noResample:
|
|
coef_mag_3_x = np.polyfit(temp_rel,correction_x, 3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,correction_x)
|
|
coef_mag_3_x = np.polyfit(temp, sens, 3)
|
|
|
|
mag_3_params['TC_M3_X3_0'] = coef_mag_3_x[0]
|
|
mag_3_params['TC_M3_X2_0'] = coef_mag_3_x[1]
|
|
mag_3_params['TC_M3_X1_0'] = coef_mag_3_x[2]
|
|
mag_3_params['TC_M3_X0_0'] = coef_mag_3_x[3]
|
|
|
|
fit_coef_mag_3_x = np.poly1d(coef_mag_3_x)
|
|
correction_x_resample = fit_coef_mag_3_x(temp_rel_resample)
|
|
|
|
# fit Y axis
|
|
correction_y = sensor_mag_3['y'] - np.median(sensor_mag_3['y'])
|
|
|
|
if noResample:
|
|
coef_mag_3_y = np.polyfit(temp_rel, correction_y, 3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,correction_y)
|
|
coef_mag_3_y = np.polyfit(temp, sens, 3)
|
|
|
|
mag_3_params['TC_M3_X3_1'] = coef_mag_3_y[0]
|
|
mag_3_params['TC_M3_X2_1'] = coef_mag_3_y[1]
|
|
mag_3_params['TC_M3_X1_1'] = coef_mag_3_y[2]
|
|
mag_3_params['TC_M3_X0_1'] = coef_mag_3_y[3]
|
|
|
|
fit_coef_mag_3_y = np.poly1d(coef_mag_3_y)
|
|
correction_y_resample = fit_coef_mag_3_y(temp_rel_resample)
|
|
|
|
# fit Z axis
|
|
correction_z = sensor_mag_3['z'] - np.median(sensor_mag_3['z'])
|
|
|
|
if noResample:
|
|
coef_mag_3_z = np.polyfit(temp_rel,correction_z, 3)
|
|
else:
|
|
temp, sens = resampleWithDeltaX(temp_rel,correction_z)
|
|
coef_mag_3_z = np.polyfit(temp, sens, 3)
|
|
|
|
mag_3_params['TC_M3_X3_2'] = coef_mag_3_z[0]
|
|
mag_3_params['TC_M3_X2_2'] = coef_mag_3_z[1]
|
|
mag_3_params['TC_M3_X1_2'] = coef_mag_3_z[2]
|
|
mag_3_params['TC_M3_X0_2'] = coef_mag_3_z[3]
|
|
|
|
fit_coef_mag_3_z = np.poly1d(coef_mag_3_z)
|
|
correction_z_resample = fit_coef_mag_3_z(temp_rel_resample)
|
|
|
|
# mag 3 vs temperature
|
|
plt.figure(12,figsize=(20,13))
|
|
|
|
# draw plots
|
|
plt.subplot(3,1,1)
|
|
plt.plot(sensor_mag_3['temperature'],correction_x,'b')
|
|
plt.plot(temp_resample,correction_x_resample,'r')
|
|
plt.title('Mag 3 ({}) Bias vs Temperature'.format(mag_3_params['TC_M3_ID']))
|
|
plt.ylabel('X bias (Gauss)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
# draw plots
|
|
plt.subplot(3,1,2)
|
|
plt.plot(sensor_mag_3['temperature'],correction_y,'b')
|
|
plt.plot(temp_resample,correction_y_resample,'r')
|
|
plt.ylabel('Y bias (Gauss)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
# draw plots
|
|
plt.subplot(3,1,3)
|
|
plt.plot(sensor_mag_3['temperature'],correction_z,'b')
|
|
plt.plot(temp_resample,correction_z_resample,'r')
|
|
plt.ylabel('Z bias (Gauss)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
pp.savefig()
|
|
|
|
#################################################################################
|
|
|
|
#################################################################################
|
|
|
|
# define data dictionary of baro 0 thermal correction parameters
|
|
baro_0_params = {
|
|
'TC_B0_ID':0,
|
|
'TC_B0_TMIN':0.0,
|
|
'TC_B0_TMAX':0.0,
|
|
'TC_B0_TREF':0.0,
|
|
'TC_B0_X0':0.0,
|
|
'TC_B0_X1':0.0,
|
|
'TC_B0_X2':0.0,
|
|
'TC_B0_X3':0.0,
|
|
'TC_B0_X4':0.0,
|
|
'TC_B0_X5':0.0
|
|
}
|
|
|
|
# curve fit the data for baro 0 corrections
|
|
baro_0_params['TC_B0_ID'] = int(np.median(sensor_baro_0['device_id']))
|
|
|
|
# find the min, max and reference temperature
|
|
baro_0_params['TC_B0_TMIN'] = np.amin(sensor_baro_0['temperature'])
|
|
baro_0_params['TC_B0_TMAX'] = np.amax(sensor_baro_0['temperature'])
|
|
baro_0_params['TC_B0_TREF'] = 0.5 * (baro_0_params['TC_B0_TMIN'] + baro_0_params['TC_B0_TMAX'])
|
|
|
|
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'])
|
|
if noResample:
|
|
coef_baro_0_x = np.polyfit(temp_rel,(sensor_baro_0['pressure']-median_pressure),5) # pressure in Pascal
|
|
else:
|
|
temperature, baro = resampleWithDeltaX(temp_rel,(sensor_baro_0['pressure']-median_pressure)) # pressure in Pascal
|
|
coef_baro_0_x = np.polyfit(temperature,baro,5)
|
|
|
|
baro_0_params['TC_B0_X5'] = coef_baro_0_x[0]
|
|
baro_0_params['TC_B0_X4'] = coef_baro_0_x[1]
|
|
baro_0_params['TC_B0_X3'] = coef_baro_0_x[2]
|
|
baro_0_params['TC_B0_X2'] = coef_baro_0_x[3]
|
|
baro_0_params['TC_B0_X1'] = coef_baro_0_x[4]
|
|
baro_0_params['TC_B0_X0'] = coef_baro_0_x[5]
|
|
|
|
fit_coef_baro_0_x = np.poly1d(coef_baro_0_x)
|
|
baro_0_x_resample = fit_coef_baro_0_x(temp_rel_resample)
|
|
|
|
# baro 0 vs temperature
|
|
plt.figure(13,figsize=(20,13))
|
|
|
|
# draw plots
|
|
plt.plot(sensor_baro_0['temperature'],sensor_baro_0['pressure']-median_pressure,'b')
|
|
plt.plot(temp_resample,baro_0_x_resample,'r')
|
|
plt.title('Baro 0 ({}) Bias vs Temperature'.format(baro_0_params['TC_B0_ID']))
|
|
plt.ylabel('Z bias (Pa)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
pp.savefig()
|
|
|
|
# define data dictionary of baro 1 thermal correction parameters
|
|
baro_1_params = {
|
|
'TC_B1_ID':0,
|
|
'TC_B1_TMIN':0.0,
|
|
'TC_B1_TMAX':0.0,
|
|
'TC_B1_TREF':0.0,
|
|
'TC_B1_X0':0.0,
|
|
'TC_B1_X1':0.0,
|
|
'TC_B1_X2':0.0,
|
|
'TC_B1_X3':0.0,
|
|
'TC_B1_X4':0.0,
|
|
'TC_B1_X5':0.0,
|
|
}
|
|
|
|
if num_baros >= 2:
|
|
|
|
# curve fit the data for baro 1 corrections
|
|
baro_1_params['TC_B1_ID'] = int(np.median(sensor_baro_1['device_id']))
|
|
|
|
# find the min, max and reference temperature
|
|
baro_1_params['TC_B1_TMIN'] = np.amin(sensor_baro_1['temperature'])
|
|
baro_1_params['TC_B1_TMAX'] = np.amax(sensor_baro_1['temperature'])
|
|
baro_1_params['TC_B1_TREF'] = 0.5 * (baro_1_params['TC_B1_TMIN'] + baro_1_params['TC_B1_TMAX'])
|
|
|
|
temp_rel = sensor_baro_1['temperature'] - baro_1_params['TC_B1_TREF']
|
|
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'])
|
|
if noResample:
|
|
coef_baro_1_x = np.polyfit(temp_rel,(sensor_baro_1['pressure']-median_pressure),5) # pressure in Pascal
|
|
else:
|
|
temperature, baro = resampleWithDeltaX(temp_rel,(sensor_baro_1['pressure']-median_pressure)) # pressure in Pascal
|
|
coef_baro_1_x = np.polyfit(temperature,baro,5)
|
|
|
|
baro_1_params['TC_B1_X5'] = coef_baro_1_x[0]
|
|
baro_1_params['TC_B1_X4'] = coef_baro_1_x[1]
|
|
baro_1_params['TC_B1_X3'] = coef_baro_1_x[2]
|
|
baro_1_params['TC_B1_X2'] = coef_baro_1_x[3]
|
|
baro_1_params['TC_B1_X1'] = coef_baro_1_x[4]
|
|
baro_1_params['TC_B1_X0'] = coef_baro_1_x[5]
|
|
|
|
fit_coef_baro_1_x = np.poly1d(coef_baro_1_x)
|
|
baro_1_x_resample = fit_coef_baro_1_x(temp_rel_resample)
|
|
|
|
# baro 2 vs temperature
|
|
plt.figure(14,figsize=(20,13))
|
|
|
|
# draw plots
|
|
plt.plot(sensor_baro_1['temperature'],sensor_baro_1['pressure']-median_pressure,'b')
|
|
plt.plot(temp_resample,baro_1_x_resample,'r')
|
|
plt.title('Baro 1 ({}) Bias vs Temperature'.format(baro_1_params['TC_B1_ID']))
|
|
plt.ylabel('Z bias (Pa)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
pp.savefig()
|
|
|
|
# define data dictionary of baro 2 thermal correction parameters
|
|
baro_2_params = {
|
|
'TC_B2_ID':0,
|
|
'TC_B2_TMIN':0.0,
|
|
'TC_B2_TMAX':0.0,
|
|
'TC_B2_TREF':0.0,
|
|
'TC_B2_X0':0.0,
|
|
'TC_B2_X1':0.0,
|
|
'TC_B2_X2':0.0,
|
|
'TC_B2_X3':0.0,
|
|
'TC_B2_X4':0.0,
|
|
'TC_B2_X5':0.0,
|
|
}
|
|
|
|
if num_baros >= 3:
|
|
|
|
# curve fit the data for baro 2 corrections
|
|
baro_2_params['TC_B2_ID'] = int(np.median(sensor_baro_2['device_id']))
|
|
|
|
# find the min, max and reference temperature
|
|
baro_2_params['TC_B2_TMIN'] = np.amin(sensor_baro_2['temperature'])
|
|
baro_2_params['TC_B2_TMAX'] = np.amax(sensor_baro_2['temperature'])
|
|
baro_2_params['TC_B2_TREF'] = 0.5 * (baro_2_params['TC_B2_TMIN'] + baro_2_params['TC_B2_TMAX'])
|
|
|
|
temp_rel = sensor_baro_2['temperature'] - baro_2_params['TC_B2_TREF']
|
|
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'])
|
|
if noResample:
|
|
coef_baro_2_x = np.polyfit(temp_rel,(sensor_baro_2['pressure']-median_pressure),5) # pressure in Pascal
|
|
else:
|
|
temperature, baro = resampleWithDeltaX(temp_rel,(sensor_baro_2['pressure']-median_pressure)) # pressure in Pascal
|
|
coef_baro_2_x = np.polyfit(temperature,baro,5)
|
|
|
|
baro_2_params['TC_B2_X5'] = coef_baro_2_x[0]
|
|
baro_2_params['TC_B2_X4'] = coef_baro_2_x[1]
|
|
baro_2_params['TC_B2_X3'] = coef_baro_2_x[2]
|
|
baro_2_params['TC_B2_X2'] = coef_baro_2_x[3]
|
|
baro_2_params['TC_B2_X1'] = coef_baro_2_x[4]
|
|
baro_2_params['TC_B2_X0'] = coef_baro_2_x[5]
|
|
|
|
fit_coef_baro_2_x = np.poly1d(coef_baro_2_x)
|
|
baro_2_x_resample = fit_coef_baro_2_x(temp_rel_resample)
|
|
|
|
# baro 2 vs temperature
|
|
plt.figure(15,figsize=(20,13))
|
|
|
|
# draw plots
|
|
plt.plot(sensor_baro_2['temperature'],sensor_baro_2['pressure']-median_pressure,'b')
|
|
plt.plot(temp_resample,baro_2_x_resample,'r')
|
|
plt.title('Baro 2 ({}) Bias vs Temperature'.format(baro_2_params['TC_B2_ID']))
|
|
plt.ylabel('Z bias (Pa)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
pp.savefig()
|
|
|
|
# define data dictionary of baro 3 thermal correction parameters
|
|
baro_3_params = {
|
|
'TC_B3_ID':0,
|
|
'TC_B3_TMIN':0.0,
|
|
'TC_B3_TMAX':0.0,
|
|
'TC_B3_TREF':0.0,
|
|
'TC_B3_X0':0.0,
|
|
'TC_B3_X1':0.0,
|
|
'TC_B3_X2':0.0,
|
|
'TC_B3_X3':0.0,
|
|
'TC_B3_X4':0.0,
|
|
'TC_B3_X5':0.0,
|
|
}
|
|
|
|
if num_baros >= 4:
|
|
|
|
# curve fit the data for baro 2 corrections
|
|
baro_3_params['TC_B3_ID'] = int(np.median(sensor_baro_3['device_id']))
|
|
|
|
# find the min, max and reference temperature
|
|
baro_3_params['TC_B3_TMIN'] = np.amin(sensor_baro_3['temperature'])
|
|
baro_3_params['TC_B3_TMAX'] = np.amax(sensor_baro_3['temperature'])
|
|
baro_3_params['TC_B3_TREF'] = 0.5 * (baro_3_params['TC_B3_TMIN'] + baro_3_params['TC_B3_TMAX'])
|
|
|
|
temp_rel = sensor_baro_3['temperature'] - baro_3_params['TC_B3_TREF']
|
|
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,(sensor_baro_3['pressure']-median_pressure),5) # pressure in Pascal
|
|
baro_3_params['TC_B3_X5'] = coef_baro_3_x[0]
|
|
baro_3_params['TC_B3_X4'] = coef_baro_3_x[1]
|
|
baro_3_params['TC_B3_X3'] = coef_baro_3_x[2]
|
|
baro_3_params['TC_B3_X2'] = coef_baro_3_x[3]
|
|
baro_3_params['TC_B3_X1'] = coef_baro_3_x[4]
|
|
baro_3_params['TC_B3_X0'] = coef_baro_3_x[5]
|
|
|
|
fit_coef_baro_3_x = np.poly1d(coef_baro_3_x)
|
|
baro_3_x_resample = fit_coef_baro_3_x(temp_rel_resample)
|
|
|
|
# baro 3 vs temperature
|
|
plt.figure(16,figsize=(20,13))
|
|
|
|
# draw plots
|
|
plt.plot(sensor_baro_3['temperature'],sensor_baro_3['pressure']-median_pressure,'b')
|
|
plt.plot(temp_resample,baro_3_x_resample,'r')
|
|
plt.title('Baro 3 ({}) Bias vs Temperature'.format(baro_3_params['TC_B3_ID']))
|
|
plt.ylabel('Z bias (Pa)')
|
|
plt.xlabel('temperature (degC)')
|
|
plt.grid()
|
|
|
|
pp.savefig()
|
|
|
|
#################################################################################
|
|
|
|
# close the pdf file
|
|
pp.close()
|
|
|
|
# close all figures
|
|
plt.close("all")
|
|
|
|
# write correction parameters to file
|
|
test_results_filename = ulog_file_name + ".params"
|
|
file = open(test_results_filename,"w")
|
|
file.write("# Sensor thermal compensation parameters\n")
|
|
file.write("#\n")
|
|
file.write("# Vehicle-Id Component-Id Name Value Type\n")
|
|
|
|
# accel 0 corrections
|
|
key_list_accel = list(accel_0_params.keys())
|
|
key_list_accel.sort
|
|
for key in key_list_accel:
|
|
if key == 'TC_A0_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(accel_0_params[key])+"\t"+type+"\n")
|
|
|
|
# accel 1 corrections
|
|
key_list_accel = list(accel_1_params.keys())
|
|
key_list_accel.sort
|
|
for key in key_list_accel:
|
|
if key == 'TC_A1_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(accel_1_params[key])+"\t"+type+"\n")
|
|
|
|
# accel 2 corrections
|
|
key_list_accel = list(accel_2_params.keys())
|
|
key_list_accel.sort
|
|
for key in key_list_accel:
|
|
if key == 'TC_A2_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(accel_2_params[key])+"\t"+type+"\n")
|
|
|
|
# accel 3 corrections
|
|
key_list_accel = list(accel_3_params.keys())
|
|
key_list_accel.sort
|
|
for key in key_list_accel:
|
|
if key == 'TC_A3_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(accel_3_params[key])+"\t"+type+"\n")
|
|
|
|
# gyro 0 corrections
|
|
key_list_gyro = list(gyro_0_params.keys())
|
|
key_list_gyro.sort()
|
|
for key in key_list_gyro:
|
|
if key == 'TC_G0_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(gyro_0_params[key])+"\t"+type+"\n")
|
|
|
|
# gyro 1 corrections
|
|
key_list_gyro = list(gyro_1_params.keys())
|
|
key_list_gyro.sort()
|
|
for key in key_list_gyro:
|
|
if key == 'TC_G1_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(gyro_1_params[key])+"\t"+type+"\n")
|
|
|
|
# gyro 2 corrections
|
|
key_list_gyro = list(gyro_2_params.keys())
|
|
key_list_gyro.sort()
|
|
for key in key_list_gyro:
|
|
if key == 'TC_G2_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(gyro_2_params[key])+"\t"+type+"\n")
|
|
|
|
# gyro 3 corrections
|
|
key_list_gyro = list(gyro_3_params.keys())
|
|
key_list_gyro.sort()
|
|
for key in key_list_gyro:
|
|
if key == 'TC_G3_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(gyro_3_params[key])+"\t"+type+"\n")
|
|
|
|
# accel 0 corrections
|
|
key_list_mag = list(mag_0_params.keys())
|
|
key_list_mag.sort
|
|
for key in key_list_mag:
|
|
if key == 'TC_M0_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(mag_0_params[key])+"\t"+type+"\n")
|
|
|
|
# mag 1 corrections
|
|
key_list_mag = list(mag_1_params.keys())
|
|
key_list_mag.sort
|
|
for key in key_list_mag:
|
|
if key == 'TC_M1_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(mag_1_params[key])+"\t"+type+"\n")
|
|
|
|
# mag 2 corrections
|
|
key_list_mag = list(mag_2_params.keys())
|
|
key_list_mag.sort
|
|
for key in key_list_mag:
|
|
if key == 'TC_M2_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(mag_2_params[key])+"\t"+type+"\n")
|
|
|
|
# mag 3 corrections
|
|
key_list_mag = list(mag_3_params.keys())
|
|
key_list_mag.sort
|
|
for key in key_list_mag:
|
|
if key == 'TC_M3_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(mag_3_params[key])+"\t"+type+"\n")
|
|
|
|
# baro 0 corrections
|
|
key_list_baro = list(baro_0_params.keys())
|
|
key_list_baro.sort
|
|
for key in key_list_baro:
|
|
if key == 'TC_B0_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_0_params[key])+"\t"+type+"\n")
|
|
|
|
# baro 1 corrections
|
|
key_list_baro = list(baro_1_params.keys())
|
|
key_list_baro.sort
|
|
for key in key_list_baro:
|
|
if key == 'TC_B1_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_1_params[key])+"\t"+type+"\n")
|
|
|
|
# baro 2 corrections
|
|
key_list_baro = list(baro_2_params.keys())
|
|
key_list_baro.sort
|
|
for key in key_list_baro:
|
|
if key == 'TC_B2_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_2_params[key])+"\t"+type+"\n")
|
|
|
|
# baro 3 corrections
|
|
key_list_baro = list(baro_3_params.keys())
|
|
key_list_baro.sort
|
|
for key in key_list_baro:
|
|
if key == 'TC_B3_ID':
|
|
type = "6"
|
|
else:
|
|
type = "9"
|
|
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_3_params[key])+"\t"+type+"\n")
|
|
|
|
file.close()
|
|
|
|
print('Correction parameters written to ' + test_results_filename)
|
|
print('Plots saved to ' + output_plot_filename)
|