Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-29 03:36:07 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

baro static pressure compensation tuning: remove dependency to baro bias

Browse Source 
`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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py
+12 -16
View File
@@ -44,7 +44,7 @@ from pyulog.px4 import PX4ULog
import numpy as np import numpy as np
import quaternion import quaternion
from scipy import optimize from scipy import optimize
from scipy.signal import detrend from scipy.signal import sosfilt, butter
def getAllData(logfile): def getAllData(logfile):
log = ULog(logfile) log = ULog(logfile)
@@ -59,9 +59,6 @@ def getAllData(logfile):
baro = getData(log, 'vehicle_air_data', 'baro_alt_meter') baro = getData(log, 'vehicle_air_data', 'baro_alt_meter')
t_baro = ms2s(getData(log, 'vehicle_air_data', 'timestamp')) 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]'), q = np.matrix([getData(log, 'vehicle_attitude', 'q[0]'),
getData(log, 'vehicle_attitude', 'q[1]'), getData(log, 'vehicle_attitude', 'q[1]'),
getData(log, 'vehicle_attitude', 'q[2]'), getData(log, 'vehicle_attitude', 'q[2]'),
@@ -71,18 +68,17 @@ def getAllData(logfile):
gnss_h = getData(log, 'vehicle_gps_position', 'altitude_msl_m') gnss_h = getData(log, 'vehicle_gps_position', 'altitude_msl_m')
t_gnss = ms2s(getData(log, 'vehicle_gps_position', 'timestamp')) 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] 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? #TODO: use resample?
len_q = len(t_q) len_q = len(t_q)
len_l = len(t_local) len_l = len(t_local)
len_g = len(t_gnss) len_g = len(t_gnss)
len_bb = len(t_baro_bias)
i_q = 0 i_q = 0
i_l = 0 i_l = 0
i_g = 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 = [] baro_aligned = []
gnss_h_aligned = [] gnss_h_aligned = []
v_local_z_aligned = [] v_local_z_aligned = []
baro_bias_aligned = []
t_aligned = [] t_aligned = []
for i_b in range(len(t_baro)): 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 i_q += 1
while t_gnss[i_g] < t and i_g < len_g-1: while t_gnss[i_g] < t and i_g < len_g-1:
i_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 # Only use in air data
if dist_bottom[i_l] < 1.0: 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]) baro_aligned = np.append(baro_aligned, baro[i_b])
v_local_z_aligned = np.append(v_local_z_aligned, v_local[2, i_l]) 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]) 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) 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): def getData(log, topic_name, variable_name, instance=0):
variable_data = np.array([]) variable_data = np.array([])
@@ -159,15 +151,19 @@ def baroCorrection(x, v_body):
return correction return correction
def run(logfile): 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[0]: pcoef_xn / g
# x[1]: pcoef_xp / g # x[1]: pcoef_xp / g
# x[2]: pcoef_yn / g # x[2]: pcoef_yn / g
# x[3]: pcoef_yp / g # x[3]: pcoef_yp / g
# x[4]: pcoef_z / g # x[4]: pcoef_z / g
baro -= baro_bias baro_error = (gnss_h - baro)
baro_error = detrend(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 J = lambda x: np.sum(np.power(baro_error - baroCorrection(x, v_body), 2.0)) # cost function