dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

baro static pressure compensation tuning: remove dependency to baro bias 

`estimator_baro_bias` requires to have GNSS and baro hgt active and GNSS as the reference. This is quite restrictive. Instead, we can simply use a high-passed version of the baro error.
This commit is contained in:
bresch 2024-02-21 16:11:38 +01:00 committed by Mathieu Bresciani
parent b405d75553
commit 37a40d3fc2
1 changed files with 12 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py
View File

@ -44,7 +44,7 @@ from pyulog.px4 import PX4ULog
import numpy as np
import quaternion
from scipy import optimize
from scipy.signal import detrend
from scipy.signal import sosfilt, butter
def getAllData(logfile):
log = ULog(logfile)
@ -59,9 +59,6 @@ def getAllData(logfile):
baro = getData(log, 'vehicle_air_data', 'baro_alt_meter')
t_baro = ms2s(getData(log, 'vehicle_air_data', 'timestamp'))
baro_bias = getData(log, 'estimator_baro_bias', 'bias')
t_baro_bias = ms2s(getData(log, 'estimator_baro_bias', 'timestamp'))
q = np.matrix([getData(log, 'vehicle_attitude', 'q[0]'),
getData(log, 'vehicle_attitude', 'q[1]'),
getData(log, 'vehicle_attitude', 'q[2]'),
@ -71,18 +68,17 @@ def getAllData(logfile):
gnss_h = getData(log, 'vehicle_gps_position', 'altitude_msl_m')
t_gnss = ms2s(getData(log, 'vehicle_gps_position', 'timestamp'))
(t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned, baro_bias_aligned) = alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_h, t_baro_bias, baro_bias)
(t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned) = alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_h)
t_aligned -= t_aligned[0]
return (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned, baro_bias_aligned)
return (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned)
def alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_h, t_baro_bias, baro_bias):
def alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_h):
#TODO: use resample?
len_q = len(t_q)
len_l = len(t_local)
len_g = len(t_gnss)
len_bb = len(t_baro_bias)
i_q = 0
i_l = 0
i_g = 0
@ -91,7 +87,6 @@ def alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_
baro_aligned = []
gnss_h_aligned = []
v_local_z_aligned = []
baro_bias_aligned = []
t_aligned = []
for i_b in range(len(t_baro)):
@ -102,8 +97,6 @@ def alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_
i_q += 1
while t_gnss[i_g] < t and i_g < len_g-1:
i_g += 1
while t_baro_bias[i_bb] < t and i_bb < len_bb-1:
i_bb += 1
# Only use in air data
if dist_bottom[i_l] < 1.0:
@ -118,10 +111,9 @@ def alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_
baro_aligned = np.append(baro_aligned, baro[i_b])
v_local_z_aligned = np.append(v_local_z_aligned, v_local[2, i_l])
gnss_h_aligned = np.append(gnss_h_aligned, gnss_h[i_g])
baro_bias_aligned = np.append(baro_bias_aligned, baro_bias[i_bb])
t_aligned.append(t)
return (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned, baro_bias_aligned)
return (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned)
def getData(log, topic_name, variable_name, instance=0):
variable_data = np.array([])
@ -159,15 +151,19 @@ def baroCorrection(x, v_body):
return correction
def run(logfile):
(t, v_body, baro, v_local_z, gnss_h, baro_bias) = getAllData(logfile)
(t, v_body, baro, v_local_z, gnss_h) = getAllData(logfile)
# x[0]: pcoef_xn / g
# x[1]: pcoef_xp / g
# x[2]: pcoef_yn / g
# x[3]: pcoef_yp / g
# x[4]: pcoef_z / g
baro -= baro_bias
baro_error = detrend(gnss_h - baro)
baro_error = (gnss_h - baro)
# Remove low ferquency part of the signal as we're only interested in the short-term errors
baro_error -= baro_error[0]
sos = butter(4, 0.01, 'hp', fs=1/(t[1]-t[0]), output='sos')
baro_error = sosfilt(sos, baro_error)
J = lambda x: np.sum(np.power(baro_error - baroCorrection(x, v_body), 2.0)) # cost function