fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 19:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensor_simulator: update EKF at IMU rate

Browse Source 
This is how it is also done in ekf2_main. Otherwise, this leads to
multiple integration of the same IMU data due to asynchronous sensor
updates triggering a prediction step between IMU updates.

Fix unit tests that broke because of this fix
This commit is contained in:
bresch
2020-06-03 11:09:18 +02:00
committed by Mathieu Bresciani
parent 59183f70ba
commit 6126c190b2
8 changed files with 60 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/test_EKF_initialization.cpp
+13 -5
View File
@@ -65,7 +65,12 @@ class EkfInitializationTest : public ::testing::Test {
void initializedOrienationIsMatchingGroundTruth(Quatf true_quaternion)
{
const Quatf quat_est = _ekf->getQuaternion();
EXPECT_TRUE(matrix::isEqual(quat_est, true_quaternion));
const float precision = 0.0002f; // TODO: this is only required for the pitch90 test to pass
EXPECT_TRUE(matrix::isEqual(quat_est, true_quaternion, precision))
<< "quat est = " << quat_est(0) << ", " << quat_est(1) << ", "
<< quat_est(2) << ", " << quat_est(3)
<< "\nquat true = " << true_quaternion(0) << ", " << true_quaternion(1) << ", "
<< true_quaternion(2) << ", " << true_quaternion(3);
}
void validStateAfterOrientationInitialization()
@@ -89,8 +94,10 @@ class EkfInitializationTest : public ::testing::Test {
const Vector3f pos = _ekf->getPosition();
const Vector3f vel = _ekf->getVelocity();
EXPECT_TRUE(matrix::isEqual(pos, Vector3f{}, 0.001f));
EXPECT_TRUE(matrix::isEqual(vel, Vector3f{}, 0.001f));
EXPECT_TRUE(matrix::isEqual(pos, Vector3f{}, 0.001f))
<< "pos = " << pos(0) << ", " << pos(1) << ", " << pos(2);
EXPECT_TRUE(matrix::isEqual(vel, Vector3f{}, 0.002f))
<< "vel = " << vel(0) << ", " << vel(1) << ", " << vel(2);
}
void velocityAndPositionVarianceBigEnoughAfterOrientationInitialization()
@@ -118,7 +125,7 @@ class EkfInitializationTest : public ::testing::Test {
for (int i = 0; i < 3; i++){
if (fabsf(R_to_earth(2, i)) > 0.8f) {
// Highly observable, the variance decreases
EXPECT_LT(dvel_bias_var(i), 2.0e-6f) << "axis " << i;
EXPECT_LT(dvel_bias_var(i), 4.0e-6f) << "axis " << i;
} else {
// Poorly observable, the variance is set to 0
@@ -192,7 +199,8 @@ TEST_F(EkfInitializationTest, initializeWithPitch90)
_sensor_simulator.runSeconds(_init_tilt_period);
initializedOrienationIsMatchingGroundTruth(quat_sim);
// TODO: Quaternion Variance is smaller in this case than in the other cases
// TODO: Quaternion Variance is smaller and vel x is larger
// in this case than in the other cases
validStateAfterOrientationInitialization();
_sensor_simulator.runSeconds(1.f);