From 4cf51e58332d4a7cbad0fbafa95425ecec222ead Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 22 Apr 2024 14:07:17 +0200 Subject: [PATCH] baro comp: set hpf optional --- .../baro_static_pressure_compensation_tuning.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py b/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py index 01fe5e758ca9..2629700228c3 100644 --- a/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py +++ b/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py @@ -150,7 +150,7 @@ def baroCorrection(x, v_body): return correction -def run(logfile): +def run(logfile, w_hpf): (t, v_body, baro, v_local_z, gnss_h) = getAllData(logfile) # x[0]: pcoef_xn / g @@ -162,8 +162,10 @@ def run(logfile): # 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) + + if (w_hpf > 0): + sos = butter(4, w_hpf, '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 @@ -224,8 +226,10 @@ def run(logfile): # Provide parameter file path and name parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str) + parser.add_argument('--hpf', help='Cuttoff frequency of high-pass filter on baro error (Hz)', type=float) args = parser.parse_args() logfile = os.path.abspath(args.logfile) # Convert to absolute path + w_hpf = 2 * np.pi * args.hpf - run(logfile) + run(logfile, w_hpf)