Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Nav velocity Z estimation improvements #10243

Merged
merged 8 commits into from
Oct 3, 2024
Merged

Nav velocity Z estimation improvements #10243

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
f5e209c
nav_vel_z_improvement
breadoven Jul 17, 2024
569bd14
add default altitude source setting
breadoven Jul 23, 2024
3a4c348
Merge remote-tracking branch 'upstream/master' into abo_nav_vel_z_imp…
breadoven Jul 23, 2024
cc59175
remove gravity calibration changes
breadoven Aug 13, 2024
ef2d9c1
add baro v setting, clean up config calls
breadoven Sep 9, 2024
d0b5dbe
add new settings
breadoven Sep 10, 2024
4a8440a
Merge branch 'master' into abo_nav_vel_z_improvements
breadoven Sep 26, 2024
1ed4875
Merge branch 'master' into abo_nav_vel_z_improvements
breadoven Sep 28, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 20 additions & 0 deletions docs/Settings.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2002,6 +2002,16 @@ Uncertainty value for barometric sensor [cm]

---

### inav_default_alt_sensor

Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors unless there is an altitude error between the sensors that exceeds a set limit. In this case only the selected sensor will be used while the altitude error limit is exceeded. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use.

| Default | Min | Max |
| --- | --- | --- |
| GPS | | |

---

### inav_gravity_cal_tolerance

Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value.
Expand Down Expand Up @@ -2122,6 +2132,16 @@ Weight of barometer measurements in estimated altitude and climb rate. Setting i

---

### inav_w_z_baro_v

Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors.

| Default | Min | Max |
| --- | --- | --- |
| 0.1 | 0 | 10 |

---

### inav_w_z_gps_p

Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors.
Expand Down
14 changes: 14 additions & 0 deletions src/main/fc/settings.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -201,6 +201,9 @@ tables:
- name: mavlink_radio_type
values: ["GENERIC", "ELRS", "SIK"]
enum: mavlinkRadio_e
- name: default_altitude_source
values: ["GPS", "BARO", "GPS ONLY", "BARO ONLY"]
enum: navDefaultAltitudeSensor_e

constants:
RPYL_PID_MIN: 0
Expand Down Expand Up @@ -2410,6 +2413,12 @@ groups:
min: 0
max: 10
default_value: 0.35
- name: inav_w_z_baro_v
description: "Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors."
field: w_z_baro_v
min: 0
max: 10
default_value: 0.1
- name: inav_w_z_gps_p
description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors."
field: w_z_gps_p
Expand Down Expand Up @@ -2464,6 +2473,11 @@ groups:
field: baro_epv
min: 0
max: 9999
- name: inav_default_alt_sensor
description: "Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors unless there is an altitude error between the sensors that exceeds a set limit. In this case only the selected sensor will be used while the altitude error limit is exceeded. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use."
default_value: "GPS"
field: default_alt_sensor
table: default_altitude_source

- name: PG_NAV_CONFIG
type: navConfig_t
Expand Down
30 changes: 16 additions & 14 deletions src/main/navigation/navigation.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -231,35 +231,37 @@ typedef enum {

typedef struct positionEstimationConfig_s {
uint8_t automatic_mag_declination;
uint8_t reset_altitude_type; // from nav_reset_type_e
uint8_t reset_home_type; // nav_reset_type_e
uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s)
uint8_t reset_altitude_type; // from nav_reset_type_e
uint8_t reset_home_type; // nav_reset_type_e
uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s)
uint8_t allow_dead_reckoning;

uint16_t max_surface_altitude;

float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements
float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements
float w_z_baro_v; // Weight (cutoff frequency) for barometer climb rate measurements

float w_z_surface_p; // Weight (cutoff frequency) for surface altitude measurements
float w_z_surface_v; // Weight (cutoff frequency) for surface velocity measurements
float w_z_surface_p; // Weight (cutoff frequency) for surface altitude measurements
float w_z_surface_v; // Weight (cutoff frequency) for surface velocity measurements

float w_z_gps_p; // GPS altitude data is very noisy and should be used only on airplanes
float w_z_gps_v; // Weight (cutoff frequency) for GPS climb rate measurements
float w_z_gps_p; // GPS altitude data is very noisy and should be used only on airplanes
float w_z_gps_v; // Weight (cutoff frequency) for GPS climb rate measurements

float w_xy_gps_p; // Weight (cutoff frequency) for GPS position measurements
float w_xy_gps_v; // Weight (cutoff frequency) for GPS velocity measurements
float w_xy_gps_p; // Weight (cutoff frequency) for GPS position measurements
float w_xy_gps_v; // Weight (cutoff frequency) for GPS velocity measurements

float w_xy_flow_p;
float w_xy_flow_v;

float w_z_res_v; // When velocity sources lost slowly decrease estimated velocity with this weight
float w_z_res_v; // When velocity sources lost slowly decrease estimated velocity with this weight
float w_xy_res_v;

float w_acc_bias; // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
float w_acc_bias; // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.

float max_eph_epv; // Max estimated position error acceptable for estimation (cm)
float baro_epv; // Baro position error
float max_eph_epv; // Max estimated position error acceptable for estimation (cm)
float baro_epv; // Baro position error

uint8_t default_alt_sensor; // default altitude sensor source
#ifdef USE_GPS_FIX_ESTIMATION
uint8_t allow_gps_fix_estimation;
#endif
Expand Down
107 changes: 65 additions & 42 deletions src/main/navigation/navigation_pos_estimator.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@
navigationPosEstimator_t posEstimator;
static float initialBaroAltitudeOffset = 0.0f;

PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 7);
PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 8);

PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
// Inertial position estimator parameters
Expand All @@ -69,6 +69,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
.max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,

.w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT,
.w_z_baro_v = SETTING_INAV_W_Z_BARO_V_DEFAULT,

.w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT,
.w_z_surface_v = SETTING_INAV_W_Z_SURFACE_V_DEFAULT,
Expand All @@ -89,6 +90,8 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,

.max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT,
.baro_epv = SETTING_INAV_BARO_EPV_DEFAULT,

.default_alt_sensor = SETTING_INAV_DEFAULT_ALT_SENSOR_DEFAULT,
#ifdef USE_GPS_FIX_ESTIMATION
.allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT
#endif
Expand Down Expand Up @@ -342,6 +345,7 @@ static bool gravityCalibrationComplete(void)

return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
}

#define ACC_VIB_FACTOR_S 1.0f
#define ACC_VIB_FACTOR_E 3.0f
static void updateIMUEstimationWeight(const float dt)
Expand Down Expand Up @@ -423,7 +427,6 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
posEstimator.imu.calibratedGravityCMSS = gyroConfig()->gravity_cmss_cal;
}

/* If calibration is incomplete - report zero acceleration */
if (gravityCalibrationComplete()) {
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) {
Expand All @@ -432,7 +435,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
#endif
posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS;
}
else {
else { // If calibration is incomplete - report zero acceleration
posEstimator.imu.accelNEU.x = 0.0f;
posEstimator.imu.accelNEU.y = 0.0f;
posEstimator.imu.accelNEU.z = 0.0f;
Expand Down Expand Up @@ -476,14 +479,16 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
/* Figure out if we have valid position data from our data sources */
uint32_t newFlags = 0;

const float max_eph_epv = positionEstimationConfig()->max_eph_epv;

if ((sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && posControl.gpsOrigin.valid &&
((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
(posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) {
if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) {
(posEstimator.gps.eph < max_eph_epv)) {
if (posEstimator.gps.epv < max_eph_epv) {
newFlags |= EST_GPS_XY_VALID | EST_GPS_Z_VALID;
}
else {
Expand All @@ -503,11 +508,11 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
newFlags |= EST_FLOW_VALID;
}

if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
if (posEstimator.est.eph < max_eph_epv) {
newFlags |= EST_XY_VALID;
}

if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
if (posEstimator.est.epv < max_eph_epv) {
newFlags |= EST_Z_VALID;
}

Expand All @@ -521,7 +526,9 @@ static void estimationPredict(estimationContext_t * ctx)
if ((ctx->newFlags & EST_Z_VALID)) {
posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
if (ARMING_FLAG(WAS_EVER_ARMED)) { // Hold at zero until first armed
posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
}
}

/* Prediction step: XY-axis */
Expand Down Expand Up @@ -552,20 +559,28 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count

bool correctOK = false;
const uint8_t defaultAltitudeSource = positionEstimationConfig()->default_alt_sensor;
float wGps = defaultAltitudeSource == ALTITUDE_SOURCE_BARO_ONLY && ctx->newFlags & EST_BARO_VALID ? 0.0f : 1.0f;
float wBaro = defaultAltitudeSource == ALTITUDE_SOURCE_GPS_ONLY && ctx->newFlags & EST_GPS_Z_VALID ? 0.0f : 1.0f;

float wBaro = 0.0f;
if (ctx->newFlags & EST_BARO_VALID) {
// Ignore baro if difference is too big, baro is probably wrong
const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f;
if (wBaro && ctx->newFlags & EST_BARO_VALID && wGps && ctx->newFlags & EST_GPS_Z_VALID) {
const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt);

// Fade out the baro to prevent sudden jump
const float start_epv = positionEstimationConfig()->max_eph_epv;
const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f;
// Fade out the non default sensor to prevent sudden jump
uint16_t residualErrorEpvLimit = defaultAltitudeSource == ALTITUDE_SOURCE_BARO ? 2 * positionEstimationConfig()->baro_epv : positionEstimationConfig()->max_eph_epv;
const float start_epv = residualErrorEpvLimit;
const float end_epv = residualErrorEpvLimit * 2.0f;

// Calculate residual gps/baro sensor weighting based on assumed default altitude source = GPS
wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f);

if (defaultAltitudeSource == ALTITUDE_SOURCE_BARO) { // flip residual sensor weighting if default = BARO
wGps = wBaro;
wBaro = 1.0f;
}
}

// Always use Baro if no GPS otherwise only use if accuracy OK compared to GPS
if (wBaro > 0.01f) {
if (ctx->newFlags & EST_BARO_VALID && wBaro) {
timeUs_t currentTimeUs = micros();

if (!ARMING_FLAG(ARMED)) {
Expand All @@ -588,21 +603,25 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));

// Altitude
const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
ctx->estPosCorr.z += wBaro * baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
ctx->estVelCorr.z += wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
const float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z);
const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z);
const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p;

ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
ctx->estPosCorr.z += baroAltResidual * w_z_baro_p * ctx->dt;
ctx->estVelCorr.z += baroAltResidual * sq(w_z_baro_p) * ctx->dt;
ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_v * ctx->dt;

ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, w_z_baro_p);

// Accelerometer bias
if (!isAirCushionEffectDetected) {
ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
ctx->accBiasCorr.z -= baroAltResidual * sq(w_z_baro_p);
}

correctOK = true;
correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed
}

if (ctx->newFlags & EST_GPS_Z_VALID) {
if (ctx->newFlags & EST_GPS_Z_VALID && wGps) {
// Reset current estimate to GPS altitude if estimate not valid
if (!(ctx->newFlags & EST_Z_VALID)) {
ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
Expand All @@ -611,19 +630,20 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
}
else {
// Altitude
const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z;
const float gpsVelZResudual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z);
const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z);
const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p;

ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
ctx->estVelCorr.z += gpsVelZResudual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResudual)), positionEstimationConfig()->w_z_gps_p);
ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * ctx->dt;
ctx->estVelCorr.z += gpsAltResidual * sq(w_z_gps_p) * ctx->dt;
ctx->estVelCorr.z += gpsVelZResidual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p);

// Accelerometer bias
ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
ctx->accBiasCorr.z -= gpsAltResidual * sq(w_z_gps_p);
}

correctOK = true;
correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed
}

return correctOK;
Expand Down Expand Up @@ -700,21 +720,23 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
{
estimationContext_t ctx;

const float max_eph_epv = positionEstimationConfig()->max_eph_epv;

/* Calculate dT */
ctx.dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime);
posEstimator.est.lastUpdateTime = currentTimeUs;

/* If IMU is not ready we can't estimate anything */
if (!isImuReady()) {
posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f;
posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f;
posEstimator.est.eph = max_eph_epv + 0.001f;
posEstimator.est.epv = max_eph_epv + 0.001f;
posEstimator.flags = 0;
return;
}

/* Calculate new EPH and EPV for the case we didn't update postion */
ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs);
vectorZero(&ctx.estPosCorr);
vectorZero(&ctx.estVelCorr);
Expand All @@ -737,12 +759,12 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
estimationCalculateCorrection_XY_FLOW(&ctx);

// If we can't apply correction or accuracy is off the charts - decay velocity to zero
if (!estXYCorrectOk || ctx.newEPH > positionEstimationConfig()->max_eph_epv) {
if (!estXYCorrectOk || ctx.newEPH > max_eph_epv) {
ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
}

if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) {
if (!estZCorrectOk || ctx.newEPV > max_eph_epv) {
ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
}
// Boost the corrections based on accWeight
Expand All @@ -754,16 +776,17 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);

/* Correct accelerometer bias */
if (positionEstimationConfig()->w_acc_bias > 0.0f) {
const float w_acc_bias = positionEstimationConfig()->w_acc_bias;
if (w_acc_bias > 0.0f) {
const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z);
if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) {
/* transform error vector from NEU frame to body frame */
imuTransformVectorEarthToBody(&ctx.accBiasCorr);

/* Correct accel bias */
posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * positionEstimationConfig()->w_acc_bias * ctx.dt;
posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * positionEstimationConfig()->w_acc_bias * ctx.dt;
posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * positionEstimationConfig()->w_acc_bias * ctx.dt;
posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt;
posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt;
posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt;
}
}

Expand Down
Loading
Loading