mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-18 06:38:29 -04:00
Tools: added script to calculate IMU temp compensation parameters
this is run over an onboard log to calculate the INS_TCAL parameters to enable temperature compensation for gyro and accel
This commit is contained in:
Andrew Tridgell
Peter Barker
parent
580cd143b8
commit
c4664d8e32
268
Tools/scripts/tempcal_IMU.py
Executable file
268
Tools/scripts/tempcal_IMU.py
Executable file
@ -0,0 +1,268 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
'''
|
||||||
|
Create temperature calibration parameters for IMUs based on log data.
|
||||||
|
'''
|
||||||
|
|
||||||
|
from argparse import ArgumentParser
|
||||||
|
parser = ArgumentParser(description=__doc__)
|
||||||
|
parser.add_argument("--outfile", default="tcal.parm")
|
||||||
|
parser.add_argument("--no-graph", action='store_true', default=False)
|
||||||
|
parser.add_argument("log", metavar="LOG")
|
||||||
|
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
import sys
|
||||||
|
import math
|
||||||
|
import re
|
||||||
|
from pymavlink import mavutil
|
||||||
|
import numpy as np
|
||||||
|
import matplotlib.pyplot as pyplot
|
||||||
|
from scipy import signal
|
||||||
|
from pymavlink.rotmat import Vector3, Matrix3
|
||||||
|
|
||||||
|
POLY_ORDER = 3
|
||||||
|
|
||||||
|
# we scale the parameters so the values work nicely in
|
||||||
|
# parameter editors and parameter files that don't
|
||||||
|
# use exponential notation
|
||||||
|
SCALE_FACTOR = 1.0e6
|
||||||
|
|
||||||
|
def moving_average(data, w):
|
||||||
|
'''apply a moving average filter over a window of width w'''
|
||||||
|
ret = np.cumsum(data)
|
||||||
|
ret[w:] = ret[w:] - ret[:-w]
|
||||||
|
return ret[w - 1:] / w
|
||||||
|
|
||||||
|
def constrain(value, minv, maxv):
|
||||||
|
"""Constrain a value to a range."""
|
||||||
|
if value < minv:
|
||||||
|
value = minv
|
||||||
|
if value > maxv:
|
||||||
|
value = maxv
|
||||||
|
return value
|
||||||
|
|
||||||
|
def correction_parm(enable, tmin, tmax, coeff, temperature, cal_temp, axis):
|
||||||
|
'''calculate correction from temperature calibration from log data using parameters'''
|
||||||
|
if enable != 1.0:
|
||||||
|
return 0.0
|
||||||
|
tmid = 0.5 * (tmax + tmin)
|
||||||
|
if tmid <= 0:
|
||||||
|
return 0.0
|
||||||
|
if cal_temp <= -80:
|
||||||
|
return 0.0
|
||||||
|
if not axis in coeff:
|
||||||
|
return 0.0
|
||||||
|
temperature = constrain(temperature, tmin, tmax)
|
||||||
|
cal_temp = constrain(cal_temp, tmin, tmax)
|
||||||
|
poly = np.poly1d(coeff[axis])
|
||||||
|
ret = poly(cal_temp-tmid) - poly(temperature-tmid)
|
||||||
|
return ret
|
||||||
|
|
||||||
|
def IMUfit(logfile):
|
||||||
|
'''find IMU calibration parameters from a log file'''
|
||||||
|
print("Processing log %s" % logfile)
|
||||||
|
mlog = mavutil.mavlink_connection(logfile)
|
||||||
|
|
||||||
|
accel = {}
|
||||||
|
gyro = {}
|
||||||
|
acoef = {}
|
||||||
|
gcoef = {}
|
||||||
|
enable = [0]*3
|
||||||
|
tmin = [-100]*3
|
||||||
|
tmax = [-100]*3
|
||||||
|
gtcal = {}
|
||||||
|
atcal = {}
|
||||||
|
orientation = 0
|
||||||
|
axes = ['X','Y','Z']
|
||||||
|
axesT = ['X','Y','Z','T','time']
|
||||||
|
|
||||||
|
while True:
|
||||||
|
msg = mlog.recv_match(type=['IMU','PARM'])
|
||||||
|
if msg is None:
|
||||||
|
break
|
||||||
|
|
||||||
|
if msg.get_type() == 'PARM':
|
||||||
|
# build up the old coefficients so we can remove the impact of
|
||||||
|
# existing coefficients from the data
|
||||||
|
m = re.match("^INS_TCAL(\d)_([AG]..)([1-3])_([XYZ])$", msg.Name)
|
||||||
|
if m:
|
||||||
|
imu = int(m.group(1))-1
|
||||||
|
stype = m.group(2)
|
||||||
|
p = int(m.group(3))
|
||||||
|
axis = m.group(4)
|
||||||
|
if stype == 'ACC':
|
||||||
|
if not imu in acoef:
|
||||||
|
acoef[imu] = {}
|
||||||
|
if not axis in acoef[imu]:
|
||||||
|
acoef[imu][axis] = [0.0]*4
|
||||||
|
acoef[imu][axis][POLY_ORDER-p] = msg.Value/SCALE_FACTOR
|
||||||
|
if stype == 'GYR':
|
||||||
|
if not imu in gcoef:
|
||||||
|
gcoef[imu] = {}
|
||||||
|
if not axis in gcoef[imu]:
|
||||||
|
gcoef[imu][axis] = [0.0]*4
|
||||||
|
gcoef[imu][axis][POLY_ORDER-p] = msg.Value/SCALE_FACTOR
|
||||||
|
m = re.match("^INS_TCAL(\d)_ENABLE$", msg.Name)
|
||||||
|
if m:
|
||||||
|
imu = int(m.group(1))-1
|
||||||
|
if msg.Value == 1 and enable[imu] == 2:
|
||||||
|
print("TCAL[%u] enabled" % imu)
|
||||||
|
break
|
||||||
|
enable[imu] = msg.Value
|
||||||
|
m = re.match("^INS_TCAL(\d)_TMIN$", msg.Name)
|
||||||
|
if m:
|
||||||
|
imu = int(m.group(1))-1
|
||||||
|
tmin[imu] = msg.Value
|
||||||
|
m = re.match("^INS_TCAL(\d)_TMAX", msg.Name)
|
||||||
|
if m:
|
||||||
|
imu = int(m.group(1))-1
|
||||||
|
tmax[imu] = msg.Value
|
||||||
|
m = re.match("^INS_GYR_CALTEMP(\d)", msg.Name)
|
||||||
|
if m:
|
||||||
|
imu = int(m.group(1))-1
|
||||||
|
gtcal[imu] = msg.Value
|
||||||
|
m = re.match("^INS_ACC_CALTEMP(\d)", msg.Name)
|
||||||
|
if m:
|
||||||
|
imu = int(m.group(1))-1
|
||||||
|
atcal[imu] = msg.Value
|
||||||
|
if msg.Name == 'AHRS_ORIENTATION':
|
||||||
|
orientation = int(msg.Value)
|
||||||
|
print("Using orientation %d" % orientation)
|
||||||
|
|
||||||
|
if msg.get_type() != 'IMU':
|
||||||
|
continue
|
||||||
|
|
||||||
|
imu = msg.I
|
||||||
|
|
||||||
|
if imu not in accel:
|
||||||
|
accel[imu] = {}
|
||||||
|
gyro[imu] = {}
|
||||||
|
for axis in axesT:
|
||||||
|
accel[imu][axis] = np.zeros(0,dtype=float)
|
||||||
|
gyro[imu][axis] = np.zeros(0,dtype=float)
|
||||||
|
|
||||||
|
T = msg.T
|
||||||
|
accel[imu]['T'] = np.append(accel[imu]['T'], T)
|
||||||
|
gyro[imu]['T'] = np.append(gyro[imu]['T'], T)
|
||||||
|
accel[imu]['time'] = np.append(accel[imu]['time'], msg.TimeUS*1.0e-6)
|
||||||
|
gyro[imu]['time'] = np.append(gyro[imu]['time'], msg.TimeUS*1.0e-6)
|
||||||
|
|
||||||
|
acc = Vector3(msg.AccX, msg.AccY, msg.AccZ)
|
||||||
|
gyr = Vector3(msg.GyrX, msg.GyrY, msg.GyrZ)
|
||||||
|
|
||||||
|
# invert the board orientation rotation. Corrections are in sensor frame
|
||||||
|
if orientation != 0:
|
||||||
|
acc = acc.rotate_by_inverse_id(orientation)
|
||||||
|
gyr = gyr.rotate_by_inverse_id(orientation)
|
||||||
|
if acc is None or gyr is None:
|
||||||
|
print("Invalid AHRS_ORIENTATION %u" % orientation)
|
||||||
|
sys.exit(1)
|
||||||
|
|
||||||
|
for axis in axes:
|
||||||
|
value = getattr(acc, axis.lower())
|
||||||
|
if enable[imu] == 1:
|
||||||
|
value -= correction_parm(enable[imu], tmin[imu], tmax[imu], acoef[imu],
|
||||||
|
T, atcal[imu], axis)
|
||||||
|
accel[imu][axis] = np.append(accel[imu][axis], value)
|
||||||
|
|
||||||
|
value = getattr(gyr, axis.lower())
|
||||||
|
if enable[imu] == 1:
|
||||||
|
value -= correction_parm(enable[imu], tmin[imu], tmax[imu], gcoef[imu],
|
||||||
|
T, gtcal[imu], axis)
|
||||||
|
gyro[imu][axis] = np.append(gyro[imu][axis], value)
|
||||||
|
|
||||||
|
# apply moving average filter with 2s width
|
||||||
|
for imu in accel:
|
||||||
|
nseconds = accel[imu]['time'][-1] - accel[imu]['time'][0]
|
||||||
|
nsamples = len(accel[imu]['time'])
|
||||||
|
window = int(nsamples / nseconds) * 2
|
||||||
|
|
||||||
|
for axis in axesT:
|
||||||
|
accel[imu][axis] = moving_average(accel[imu][axis], window)
|
||||||
|
gyro[imu][axis] = moving_average(gyro[imu][axis], window)
|
||||||
|
|
||||||
|
trel = {}
|
||||||
|
|
||||||
|
calfile = open(args.outfile, "w")
|
||||||
|
|
||||||
|
for imu in accel:
|
||||||
|
tmin = np.amin(accel[imu]['T'])
|
||||||
|
tmax = np.amax(accel[imu]['T'])
|
||||||
|
tref = (tmin+tmax)*0.5
|
||||||
|
|
||||||
|
acoef[imu] = {}
|
||||||
|
gcoef[imu] = {}
|
||||||
|
|
||||||
|
trel[imu] = accel[imu]['T'] - tref
|
||||||
|
|
||||||
|
for axis in axes:
|
||||||
|
acoef[imu][axis] = np.polyfit(trel[imu], accel[imu][axis] - np.median(accel[imu][axis]), POLY_ORDER)
|
||||||
|
gcoef[imu][axis] = np.polyfit(trel[imu], gyro[imu][axis], POLY_ORDER)
|
||||||
|
|
||||||
|
params = ''
|
||||||
|
params += 'INS_TCAL%u_ENABLE 1\n' % (imu+1)
|
||||||
|
params += 'INS_TCAL%u_TMIN %.1f\n' % (imu+1, tmin)
|
||||||
|
params += 'INS_TCAL%u_TMAX %.1f\n' % (imu+1, tmax)
|
||||||
|
# note that we don't save the first term of the polynomial as that is a
|
||||||
|
# constant offset which is already handled by the accel/gyro constant
|
||||||
|
# offsets. We only same the temperature dependent part of the
|
||||||
|
# calibration
|
||||||
|
for p in range(POLY_ORDER):
|
||||||
|
for axis in axes:
|
||||||
|
params += 'INS_TCAL%u_ACC%u_%s %.9f\n' % (imu+1, p+1, axis, acoef[imu][axis][POLY_ORDER-(p+1)]*SCALE_FACTOR)
|
||||||
|
for p in range(POLY_ORDER):
|
||||||
|
for axis in axes:
|
||||||
|
params += 'INS_TCAL%u_GYR%u_%s %.9f\n' % (imu+1, p+1, axis, gcoef[imu][axis][POLY_ORDER-(p+1)]*SCALE_FACTOR)
|
||||||
|
print(params)
|
||||||
|
calfile.write(params)
|
||||||
|
|
||||||
|
calfile.close()
|
||||||
|
print("Calibration written to %s" % args.outfile)
|
||||||
|
|
||||||
|
if args.no_graph:
|
||||||
|
return
|
||||||
|
fig, axs = pyplot.subplots(len(gyro), 1, sharex=True)
|
||||||
|
|
||||||
|
if len(gyro) == 1:
|
||||||
|
axs = [axs]
|
||||||
|
|
||||||
|
for imu in gyro:
|
||||||
|
scale = math.degrees(1)
|
||||||
|
for axis in axes:
|
||||||
|
axs[imu].plot(gyro[imu]['time'], gyro[imu][axis]*scale, label='Uncorrected %s' % axis)
|
||||||
|
for axis in axes:
|
||||||
|
poly = np.poly1d(gcoef[imu][axis])
|
||||||
|
correction = poly(trel[imu])
|
||||||
|
axs[imu].plot(gyro[imu]['time'], (gyro[imu][axis] - correction)*scale, label='Corrected %s' % axis)
|
||||||
|
ax2 = axs[imu].twinx()
|
||||||
|
ax2.plot(gyro[imu]['time'], gyro[imu]['T'], label='Temperature(C)', color='black')
|
||||||
|
ax2.legend(loc='upper right')
|
||||||
|
axs[imu].legend(loc='upper left')
|
||||||
|
axs[imu].set_title('IMU[%u] Gyro (deg/s)' % imu)
|
||||||
|
|
||||||
|
fig, axs = pyplot.subplots(len(accel), 1, sharex=True)
|
||||||
|
if len(accel) == 1:
|
||||||
|
axs = [axs]
|
||||||
|
|
||||||
|
for imu in accel:
|
||||||
|
mean = {}
|
||||||
|
for axis in axes:
|
||||||
|
mean[axis] = np.mean(accel[imu][axis])
|
||||||
|
axs[imu].plot(accel[imu]['time'], accel[imu][axis] - mean[axis], label='Uncorrected %s' % axis)
|
||||||
|
for axis in axes:
|
||||||
|
poly = np.poly1d(acoef[imu][axis])
|
||||||
|
correction = poly(trel[imu])
|
||||||
|
axs[imu].plot(accel[imu]['time'], (accel[imu][axis]-mean[axis]) - correction, label='Corrected %s' % axis)
|
||||||
|
ax2 = axs[imu].twinx()
|
||||||
|
ax2.plot(accel[imu]['time'], accel[imu]['T'], label='Temperature(C)', color='black')
|
||||||
|
ax2.legend(loc='upper right')
|
||||||
|
axs[imu].legend(loc='upper left')
|
||||||
|
axs[imu].set_title('IMU[%u] Accel (m/s^2)' % imu)
|
||||||
|
|
||||||
|
pyplot.show()
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
IMUfit(args.log)
|
||||||
|
sys.exit(1)
|
||||||
|
|
||||||
Loading…
Reference in New Issue
Block a user