From 66815df7ec4855c075f7d2e08f26f9ad637727be Mon Sep 17 00:00:00 2001 From: Lovro Markovic Date: Wed, 8 Sep 2021 13:15:45 +0200 Subject: [PATCH] [filter.cpp] Q error state Jacobian changed to local --- src/filter.cpp | 11 ++++++++++- src/sensor.h | 1 - 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/filter.cpp b/src/filter.cpp index 2b56464..393e5fc 100644 --- a/src/filter.cpp +++ b/src/filter.cpp @@ -86,6 +86,11 @@ void EsEkf2::prediction(const Matrix& imu_f, // 2. Propagate uncertainty // Components on pg. 60 (262-265) // Definition on pg. 70 (312) + // TODO(lmark): var_imu_f, var_imu_w, m_acc_bias_variance, m_gyro_bias_variance should be + // renamed to velocity varniance, orientation variance, acceleration bias variance, + // angular velocity bias variance + + // TODO(lmark): We should probably add sensor position and orientation drift variance Matrix q_cov = Matrix::Identity(); q_cov.block<3, 3>(0, 0) = var_imu_f * pow(delta_t, 2); q_cov.block<3, 3>(3, 3) = var_imu_w * pow(delta_t, 2); @@ -209,11 +214,15 @@ void EsEkf2::angleMeasurementUpdateDrift(const Matrix3d& R_cov, // Scola equation:(280) H_dx.block<6, 6>(POSITION_IDX, POSITION_IDX) = Matrix::Identity(6, 6); H_dx.block<4, 3>(ANGLE_AXIS_IDX, ANGLE_AXIS_IDX) = - 0.5 * sf::firstOrderApprox(m_est_quaternion); + 0.5 * sf::firstOrderApproxLocal(m_est_quaternion); H_dx.block<12, 12>(ACC_BIAS_IDX + 1, ACC_BIAS_IDX) = MatrixXd::Identity(12, 12); H_dx.block<4, 3>(DRIFT_ROT_IDX + 1, DRIFT_ROT_IDX) = 0.5 * sf::firstOrderApproxLocal(est_quaternion_drift); + // TODO(lmark): This covariance "switching" based on sensor is probably not right. + // What about prediction ? Which sensor drift covariance are we estimating there? + // Solution: Reserve a variable amount of space for sensor drift. + // Set drift covariance, and save previously stored covariance auto ph_pos = m_p_covariance.block<3, 3>(DRIFT_TRANS_IDX, DRIFT_TRANS_IDX); auto ph_rot = m_p_covariance.block<3, 3>(DRIFT_ROT_IDX, DRIFT_ROT_IDX); diff --git a/src/sensor.h b/src/sensor.h index 9c853a2..7e2f6f3 100644 --- a/src/sensor.h +++ b/src/sensor.h @@ -19,7 +19,6 @@ using SensorPtr = std::shared_ptr; * update the es-ekf state. */ -// TODO add sensor_state publishing class Sensor { private: