Merge pull request #1919 from PX4/ekf_gyro

EKF gyro offsetfix

9 files changed, 112 insertions, 87 deletions

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index d70e05000..526b135d8 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -164,11 +164,6 @@ int do_accel_calibration(int mavlink_fd)
 
 	mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 
-	mavlink_and_console_log_info(mavlink_fd, "You need to put the system on all six sides");
-	sleep(3);
-	mavlink_and_console_log_info(mavlink_fd, "Follow the instructions on the screen");
-	sleep(5);
-
 	struct accel_scale accel_scale = {
 		0.0f,
 		1.0f,
@@ -352,7 +347,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se
 				 (!data_collected[4]) ? "up " : "");
 
 		/* allow user enough time to read the message */
-		sleep(3);
+		sleep(1);
 
 		int orient = detect_orientation(mavlink_fd, subs);
 
@@ -365,7 +360,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se
 		/* inform user about already handled side */
 		if (data_collected[orient]) {
 			mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
-			sleep(3);
+			sleep(1);
 			continue;
 		}
 
@@ -374,7 +369,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se
 		read_accelerometer_avg(subs, accel_ref, orient, samples_num);
 		mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
 		usleep(100000);
-		mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient],
+		mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %8.4f %8.4f %8.4f ]", orientation_strs[orient],
 				 (double)accel_ref[0][orient][0],
 				 (double)accel_ref[0][orient][1],
 				 (double)accel_ref[0][orient][2]);
@@ -399,13 +394,6 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se
 		for (unsigned i = 0; i < (*active_sensors); i++) {
 			res = calculate_calibration_values(i, accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
 
-			/* verbose output on the console */
-			printf("accel_T transformation matrix:\n");
-			for (unsigned j = 0; j < 3; j++) {
-				printf("  %8.4f %8.4f %8.4f\n", (double)accel_T[i][j][0], (double)accel_T[i][j][1], (double)accel_T[i][j][2]);
-			}
-			printf("\n");
-
 			if (res != OK) {
 				mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error");
 				break;
@@ -635,7 +623,6 @@ int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6
 	for (unsigned s = 0; s < max_sens; s++) {
 		for (unsigned i = 0; i < 3; i++) {
 			accel_avg[s][orient][i] = accel_sum[s][i] / counts[s];
-			warnx("input: s:%u, axis: %u, orient: %u cnt: %u -> %8.4f", s, i, orient, counts[s], (double)accel_avg[s][orient][i]);
 		}
 	}
 
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 4e119a110..b8638c6a1 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -278,7 +278,7 @@ int commander_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("commander",
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_MAX - 40,
-					     3200,
+					     3400,
 					     commander_thread_main,
 					     (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 
@@ -967,19 +967,6 @@ int commander_thread_main(int argc, char *argv[])
 
 	int ret;
 
-	pthread_attr_t commander_low_prio_attr;
-	pthread_attr_init(&commander_low_prio_attr);
-	pthread_attr_setstacksize(&commander_low_prio_attr, 2000);
-
-	struct sched_param param;
-	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
-
-	/* low priority */
-	param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
-	(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
-	pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
-	pthread_attr_destroy(&commander_low_prio_attr);
-
 	/* Start monitoring loop */
 	unsigned counter = 0;
 	unsigned stick_off_counter = 0;
@@ -1144,6 +1131,20 @@ int commander_thread_main(int argc, char *argv[])
 	bool main_state_changed = false;
 	bool failsafe_old = false;
 
+	/* initialize low priority thread */
+	pthread_attr_t commander_low_prio_attr;
+	pthread_attr_init(&commander_low_prio_attr);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2100);
+
+	struct sched_param param;
+	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
+
+	/* low priority */
+	param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
+	(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
+	pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
+	pthread_attr_destroy(&commander_low_prio_attr);
+
 	while (!thread_should_exit) {
 
 		if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index b5ec6c4e6..a2e827a15 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -231,7 +231,7 @@ int led_init()
 	/* then try RGB LEDs, this can fail on FMUv1*/
 	rgbleds = open(RGBLED0_DEVICE_PATH, 0);
 
-	if (rgbleds == -1) {
+	if (rgbleds < 0) {
 		warnx("No RGB LED found at " RGBLED0_DEVICE_PATH);
 	}
 
@@ -240,50 +240,64 @@ int led_init()
 
 void led_deinit()
 {
-	close(leds);
+	if (leds >= 0) {
+		close(leds);
+	}
 
-	if (rgbleds != -1) {
+	if (rgbleds >= 0) {
 		close(rgbleds);
 	}
 }
 
 int led_toggle(int led)
 {
+	if (leds < 0) {
+		return leds;
+	}
 	return ioctl(leds, LED_TOGGLE, led);
 }
 
 int led_on(int led)
 {
+	if (leds < 0) {
+		return leds;
+	}
 	return ioctl(leds, LED_ON, led);
 }
 
 int led_off(int led)
 {
+	if (leds < 0) {
+		return leds;
+	}
 	return ioctl(leds, LED_OFF, led);
 }
 
 void rgbled_set_color(rgbled_color_t color)
 {
 
-	if (rgbleds != -1) {
-		ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
+	if (rgbleds < 0) {
+		return;
 	}
+	ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
 }
 
 void rgbled_set_mode(rgbled_mode_t mode)
 {
 
-	if (rgbleds != -1) {
-		ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
+	if (rgbleds < 0) {
+		return;
 	}
+	ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
 }
 
 void rgbled_set_pattern(rgbled_pattern_t *pattern)
 {
 
-	if (rgbleds != -1) {
-		ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
+	if (rgbleds < 0) {
+		return;
 	}
+	ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
 }
 
 float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized)
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index dfd1657c5..8a70cf66e 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -73,16 +73,17 @@ int do_gyro_calibration(int mavlink_fd)
 	/* wait for the user to respond */
 	sleep(2);
 
-	struct gyro_scale gyro_scale[max_gyros] = { {
+	struct gyro_scale gyro_scale_zero = {
 		0.0f,
 		1.0f,
 		0.0f,
 		1.0f,
 		0.0f,
 		1.0f,
-	}
 	};
 
+	struct gyro_scale gyro_scale[max_gyros];
+
 	int res = OK;
 
 	/* store board ID */
@@ -97,7 +98,7 @@ int do_gyro_calibration(int mavlink_fd)
 	for (unsigned s = 0; s < max_gyros; s++) {
 
 		/* ensure all scale fields are initialized tha same as the first struct */
-		(void)memcpy(&gyro_scale[s], &gyro_scale[0], sizeof(gyro_scale[0]));
+		(void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0]));
 
 		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 		/* reset all offsets to zero and all scales to one */
@@ -109,7 +110,7 @@ int do_gyro_calibration(int mavlink_fd)
 
 		device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
 
-		res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
+		res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
 		close(fd);
 
 		if (res != OK) {
@@ -120,6 +121,8 @@ int do_gyro_calibration(int mavlink_fd)
 	unsigned calibration_counter[max_gyros] = { 0 };
 	const unsigned calibration_count = 5000;
 
+	struct gyro_report gyro_report_0 = {};
+
 	if (res == OK) {
 		/* determine gyro mean values */
 		unsigned poll_errcount = 0;
@@ -140,7 +143,7 @@ int do_gyro_calibration(int mavlink_fd)
 		while (calibration_counter[0] < calibration_count) {
 			/* wait blocking for new data */
 
-			int poll_ret = poll(fds, 1, 1000);
+			int poll_ret = poll(&fds[0], max_gyros, 1000);
 
 			if (poll_ret > 0) {
 
@@ -150,6 +153,11 @@ int do_gyro_calibration(int mavlink_fd)
 
 					if (changed) {
 						orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report);
+
+						if (s == 0) {
+							orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0);
+						}
+
 						gyro_scale[s].x_offset += gyro_report.x;
 						gyro_scale[s].y_offset += gyro_report.y;
 						gyro_scale[s].z_offset += gyro_report.z;
@@ -183,8 +191,20 @@ int do_gyro_calibration(int mavlink_fd)
 
 	if (res == OK) {
 		/* check offsets */
-		if (!isfinite(gyro_scale[0].x_offset) || !isfinite(gyro_scale[0].y_offset) || !isfinite(gyro_scale[0].z_offset)) {
-			mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN");
+		float xdiff = gyro_report_0.x - gyro_scale[0].x_offset;
+		float ydiff = gyro_report_0.y - gyro_scale[0].y_offset;
+		float zdiff = gyro_report_0.z - gyro_scale[0].z_offset;
+
+		/* maximum allowable calibration error in radians */
+		const float maxoff = 0.01f;
+
+		if (!isfinite(gyro_scale[0].x_offset) ||
+		    !isfinite(gyro_scale[0].y_offset) ||
+		    !isfinite(gyro_scale[0].z_offset) ||
+		    fabsf(xdiff) > maxoff ||
+		    fabsf(ydiff) > maxoff ||
+		    fabsf(zdiff) > maxoff) {
+			mavlink_log_critical(mavlink_fd, "ERROR: Calibration failed");
 			res = ERROR;
 		}
 	}
@@ -214,6 +234,7 @@ int do_gyro_calibration(int mavlink_fd)
 			int fd = open(str, 0);
 
 			if (fd < 0) {
+				failed = true;
 				continue;
 			}
 
@@ -226,7 +247,7 @@ int do_gyro_calibration(int mavlink_fd)
 		}
 		
 		if (failed) {
-			mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
+			mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params");
 			res = ERROR;
 		}
 	}
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index e0786db79..a0aadab39 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -164,9 +164,9 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
 	const unsigned int calibration_maxcount = 240;
 	unsigned int calibration_counter;
 
-	float *x = NULL;
-	float *y = NULL;
-	float *z = NULL;
+	float *x = new float[calibration_maxcount];
+	float *y = new float[calibration_maxcount];
+	float *z = new float[calibration_maxcount];
 
 	char str[30];
 	int res = OK;
@@ -174,24 +174,20 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
 	/* allocate memory */
 	mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
 
-	x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
-	y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
-	z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
-
-	if (x == NULL || y == NULL || z == NULL) {
+	if (x == nullptr || y == nullptr || z == nullptr) {
 		mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory");
 
 		/* clean up */
-		if (x != NULL) {
-			free(x);
+		if (x != nullptr) {
+			delete x;
 		}
 
-		if (y != NULL) {
-			free(y);
+		if (y != nullptr) {
+			delete y;
 		}
 
-		if (z != NULL) {
-			free(z);
+		if (z != nullptr) {
+			delete z;
 		}
 
 		res = ERROR;
@@ -274,16 +270,16 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
 		}
 	}
 
-	if (x != NULL) {
-		free(x);
+	if (x != nullptr) {
+		delete x;
 	}
 
-	if (y != NULL) {
-		free(y);
+	if (y != nullptr) {
+		delete y;
 	}
 
-	if (z != NULL) {
-		free(z);
+	if (z != nullptr) {
+		delete z;
 	}
 
 	if (res == OK) {
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index 4a1369aba..4ee8732fc 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -52,5 +52,5 @@ MODULE_STACKSIZE = 5000
 
 MAXOPTIMIZATION	 = -Os
 
-EXTRACXXFLAGS = -Wframe-larger-than=2000
+EXTRACXXFLAGS = -Wframe-larger-than=2200
 
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 6ca10e56a..c16e72ea0 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -784,14 +784,14 @@ void AttitudePositionEstimatorEKF::publishAttitude()
 	_att.pitch = euler(1);
 	_att.yaw = euler(2);
 
-	_att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMU;
-	_att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMU;
-	_att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMU;
+	_att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMUfilt;
+	_att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMUfilt;
+	_att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMUfilt;
 
 	// gyro offsets
-	_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMU;
-	_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMU;
-	_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMU;
+	_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
+	_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMUfilt;
+	_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMUfilt;
 
 	/* lazily publish the attitude only once available */
 	if (_att_pub > 0) {
@@ -1069,7 +1069,7 @@ void AttitudePositionEstimatorEKF::print_status()
 	// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
 	// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
 
-	printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
+	printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec);
 	printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f));
 	printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset,
 	       (double)_baro_gps_offset);
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
index 8c82dd6c1..f8cca6c0d 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
@@ -218,7 +218,7 @@ PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f);
  * @max 0.00001
  * @group Position Estimator
  */
-PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f);
+PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-06f);
 
 /**
  * Accelerometer bias estimate process noise
@@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f);
  * @max 0.001
  * @group Position Estimator
  */
-PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.00005f);
+PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0002f);
 
 /**
  * Magnetometer earth frame offsets process noise
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index c313e83af..817590bab 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -210,10 +210,10 @@ void AttPosEKF::InitialiseParameters()
 
     yawVarScale = 1.0f;
     windVelSigma = 0.1f;
-    dAngBiasSigma = 5.0e-7f;
-    dVelBiasSigma = 1e-4f;
-    magEarthSigma = 3.0e-4f;
-    magBodySigma  = 3.0e-4f;
+    dAngBiasSigma = 1.0e-6;
+    dVelBiasSigma = 0.0002f;
+    magEarthSigma = 0.0003f;
+    magBodySigma  = 0.0003f;
 
     vneSigma = 0.2f;
     vdSigma = 0.3f;
@@ -414,9 +414,10 @@ void AttPosEKF::CovariancePrediction(float dt)
     // calculate covariance prediction process noise
     for (uint8_t i= 0; i<4;  i++) processNoise[i] = 1.0e-9f;
     for (uint8_t i= 4; i<10;  i++) processNoise[i] = 1.0e-9f;
-    for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
     // scale gyro bias noise when on ground to allow for faster bias estimation
-    for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
+    float gyroBiasScale = (_onGround) ? 2.0f : 1.0f;
+
+    for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma * gyroBiasScale;
     processNoise[13] = dVelBiasSigma;
     if (!inhibitWindStates) {
         for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma;
@@ -2706,6 +2707,11 @@ void AttPosEKF::ConstrainStates()
     // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
     // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
 
+    // Constrain dtIMUfilt
+    if (!isfinite(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) {
+        dtIMUfilt = dtIMU;
+    }
+
     // Constrain quaternion
     for (size_t i = 0; i <= 3; i++) {
         states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
@@ -2727,11 +2733,11 @@ void AttPosEKF::ConstrainStates()
 
     // Angle bias limit - set to 8 degrees / sec
     for (size_t i = 10; i <= 12; i++) {
-        states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU);
+        states[i] = ConstrainFloat(states[i], -0.16f * dtIMUfilt, 0.16f * dtIMUfilt);
     }
 
     // Constrain delta velocity bias
-    states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU);
+    states[13] = ConstrainFloat(states[13], -1.0f * dtIMUfilt, 1.0f * dtIMUfilt);
 
     // Wind velocity limits - assume 120 m/s max velocity
     for (size_t i = 14; i <= 15; i++) {
@@ -2795,8 +2801,8 @@ bool AttPosEKF::GyroOffsetsDiverged()
 
     // Protect against division by zero
     if (delta_len > 0.0f) {
-        float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f);
-        delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMU;
+        float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-2f);
+        delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMUfilt;
     }
 
     bool diverged = (delta_len_scaled > 1.0f);