Browse Source
this is run over an onboard log to calculate the INS_TCAL parameters to enable temperature compensation for gyro and accelc415-sdk
Andrew Tridgell
4 years ago
committed by
Peter Barker
1 changed files with 268 additions and 0 deletions
@ -0,0 +1,268 @@
@@ -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