diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 94319d531a..588944c7bc 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -225,16 +225,14 @@ void Ekf::fuseAirspeed() } } -void Ekf::get_wind_velocity(float *wind) +Vector2f Ekf::getWindVelocity() const { - wind[0] = _state.wind_vel(0); - wind[1] = _state.wind_vel(1); + return _state.wind_vel; } -void Ekf::get_wind_velocity_var(float *wind_var) +Vector2f Ekf::getWindVelocityVariance() const { - wind_var[0] = P(22,22); - wind_var[1] = P(23,23); + return P.slice<2, 2>(22,22).diag(); } void Ekf::get_true_airspeed(float *tas) @@ -248,8 +246,7 @@ void Ekf::get_true_airspeed(float *tas) */ void Ekf::resetWindStates() { - // get euler yaw angle - Eulerf euler321(_state.quat_nominal); + const Eulerf euler321(_state.quat_nominal); const float euler_yaw = euler321(2); if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) { diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 483ad4ef7d..414d4d3013 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -101,15 +101,14 @@ void Ekf::initialiseCovariance() } -void Ekf::get_pos_var(Vector3f &pos_var) +Vector3f Ekf::getPositionVariance() const { - pos_var = P.slice<3,3>(7,7).diag(); + return P.slice<3,3>(7,7).diag(); } -void Ekf::get_vel_var(Vector3f &vel_var) +Vector3f Ekf::getVelocityVariance() const { - vel_var = P.slice<3,3>(4,4).diag(); - + return P.slice<3,3>(4,4).diag(); } void Ekf::predictCovariance() diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 144a51b432..8443d242c3 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -350,17 +350,17 @@ void Ekf::calculateOutputStates() _R_to_earth_now = Dcmf(_output_new.quat_nominal); // correct delta velocity for bias offsets - const Vector3f delta_vel{imu.delta_vel - _state.delta_vel_bias * dt_scale_correction}; + const Vector3f delta_vel_body{imu.delta_vel - _state.delta_vel_bias * dt_scale_correction}; // rotate the delta velocity to earth frame - Vector3f delta_vel_NED{_R_to_earth_now * delta_vel}; + Vector3f delta_vel_earth{_R_to_earth_now * delta_vel_body}; // correct for measured acceleration due to gravity - delta_vel_NED(2) += CONSTANTS_ONE_G * imu.delta_vel_dt; + delta_vel_earth(2) += CONSTANTS_ONE_G * imu.delta_vel_dt; // calculate the earth frame velocity derivatives if (imu.delta_vel_dt > 1e-4f) { - _vel_deriv_ned = delta_vel_NED * (1.0f / imu.delta_vel_dt); + _vel_deriv = delta_vel_earth * (1.0f / imu.delta_vel_dt); } // save the previous velocity so we can use trapezoidal integration @@ -368,8 +368,8 @@ void Ekf::calculateOutputStates() // increment the INS velocity states by the measurement plus corrections // do the same for vertical state used by alternative correction algorithm - _output_new.vel += delta_vel_NED; - _output_vert_new.vel_d += delta_vel_NED(2); + _output_new.vel += delta_vel_earth; + _output_vert_new.vel_d += delta_vel_earth(2); // use trapezoidal integration to calculate the INS position states // do the same for vertical state used by alternative correction algorithm @@ -427,13 +427,13 @@ void Ekf::calculateOutputStates() _delta_angle_corr = delta_ang_error * att_gain; // calculate velocity and position tracking errors - const Vector3f vel_err{_state.vel - _output_sample_delayed.vel}; - const Vector3f pos_err{_state.pos - _output_sample_delayed.pos}; + const Vector3f vel_err(_state.vel - _output_sample_delayed.vel); + const Vector3f pos_err(_state.pos - _output_sample_delayed.pos); // collect magnitude tracking error for diagnostics - _output_tracking_error[0] = delta_ang_error.norm(); - _output_tracking_error[1] = vel_err.norm(); - _output_tracking_error[2] = pos_err.norm(); + _output_tracking_error(0) = delta_ang_error.norm(); + _output_tracking_error(1) = vel_err.norm(); + _output_tracking_error(2) = pos_err.norm(); /* * Loop through the output filter state history and apply the corrections to the velocity and position states. diff --git a/EKF/ekf.h b/EKF/ekf.h index e110972daa..e45bb8a846 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -135,13 +135,13 @@ public: void getHaglInnovRatio(float &hagl_innov_ratio) const override; // get the state vector at the delayed time horizon - void get_state_delayed(float *state) override; + matrix::Vector getStateAtFusionHorizonAsVector() const override; // get the wind velocity in m/s - void get_wind_velocity(float *wind) override; + Vector2f getWindVelocity() const override; // get the wind velocity var - void get_wind_velocity_var(float *wind_var) override; + Vector2f getWindVelocityVariance() const override; // get the true airspeed in m/s void get_true_airspeed(float *tas) override; @@ -187,13 +187,13 @@ public: */ bool reset_imu_bias() override; - void get_vel_var(Vector3f &vel_var) override; + Vector3f getVelocityVariance() const override; - void get_pos_var(Vector3f &pos_var) override; + Vector3f getPositionVariance() const override; // return an array containing the output predictor angular, velocity and position tracking // error magnitudes (rad), (m/sec), (m) - void get_output_tracking_error(float error[3]) override; + Vector3f getOutputTrackingError() const override; /* Returns following IMU vibration metrics in the following array locations @@ -201,7 +201,7 @@ public: 1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) 2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) */ - void get_imu_vibe_metrics(float vibe[3]) override; + Vector3f getImuVibrationMetrics() const override; /* First argument returns GPS drift metrics in the following array locations @@ -224,16 +224,16 @@ public: void updateTerrainValidity(); // get the estimated terrain vertical position relative to the NED origin - void getTerrainVertPos(float *ret) override; + float getTerrainVertPos() const override; // get the terrain variance float get_terrain_var() const { return _terrain_var; } - // get the accelerometer bias in m/s/s - void get_accel_bias(float bias[3]) override; + // get the accelerometer bias in m/s**2 + Vector3f getAccelBias() const override; // get the gyroscope bias in rad/s - void get_gyro_bias(float bias[3]) override; + Vector3f getGyroBias() const override; // get GPS check status void get_gps_check_status(uint16_t *val) override; @@ -276,7 +276,7 @@ public: void get_ekf_soln_status(uint16_t *status) override; // return the quaternion defining the rotation from the External Vision to the EKF reference frame - void get_ev2ekf_quaternion(float *quat) override; + matrix::Quatf getVisionAlignmentQuaternion() const override; // use the latest IMU data at the current time horizon. Quatf calculate_quaternion() const; @@ -441,7 +441,7 @@ private: Vector3f _delta_angle_corr; ///< delta angle correction vector (rad) Vector3f _vel_err_integ; ///< integral of velocity tracking error (m) Vector3f _pos_err_integ; ///< integral of position tracking error (m.s) - float _output_tracking_error[3] {}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m) + Vector3f _output_tracking_error; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m) // variables used for the GPS quality checks Vector3f _gps_pos_deriv_filt; ///< GPS NED position derivative (m/sec) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 6cd3bd4d14..b8d367735d 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -876,59 +876,51 @@ void Ekf::get_gps_check_status(uint16_t *val) } // get the state vector at the delayed time horizon -void Ekf::get_state_delayed(float *state) +matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const { + matrix::Vector state; for (int i = 0; i < 4; i++) { - state[i] = _state.quat_nominal(i); + state(i) = _state.quat_nominal(i); } for (int i = 0; i < 3; i++) { - state[i + 4] = _state.vel(i); + state(i + 4) = _state.vel(i); } for (int i = 0; i < 3; i++) { - state[i + 7] = _state.pos(i); + state(i + 7) = _state.pos(i); } for (int i = 0; i < 3; i++) { - state[i + 10] = _state.delta_ang_bias(i); + state(i + 10) = _state.delta_ang_bias(i); } for (int i = 0; i < 3; i++) { - state[i + 13] = _state.delta_vel_bias(i); + state(i + 13) = _state.delta_vel_bias(i); } for (int i = 0; i < 3; i++) { - state[i + 16] = _state.mag_I(i); + state(i + 16) = _state.mag_I(i); } for (int i = 0; i < 3; i++) { - state[i + 19] = _state.mag_B(i); + state(i + 19) = _state.mag_B(i); } for (int i = 0; i < 2; i++) { - state[i + 22] = _state.wind_vel(i); + state(i + 22) = _state.wind_vel(i); } + return state; } -// get the accelerometer bias -void Ekf::get_accel_bias(float bias[3]) +Vector3f Ekf::getAccelBias() const { - float temp[3]; - temp[0] = _state.delta_vel_bias(0) / _dt_ekf_avg; - temp[1] = _state.delta_vel_bias(1) / _dt_ekf_avg; - temp[2] = _state.delta_vel_bias(2) / _dt_ekf_avg; - memcpy(bias, temp, 3 * sizeof(float)); + return _state.delta_vel_bias / _dt_ekf_avg; } -// get the gyroscope bias in rad/s -void Ekf::get_gyro_bias(float bias[3]) +Vector3f Ekf::getGyroBias() const { - float temp[3]; - temp[0] = _state.delta_ang_bias(0) / _dt_ekf_avg; - temp[1] = _state.delta_ang_bias(1) / _dt_ekf_avg; - temp[2] = _state.delta_ang_bias(2) / _dt_ekf_avg; - memcpy(bias, temp, 3 * sizeof(float)); + return _state.delta_ang_bias / _dt_ekf_avg; } // get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set @@ -941,11 +933,11 @@ bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *orig return _NED_origin_initialised; } -// return an array containing the output predictor angular, velocity and position tracking +// return a vector containing the output predictor angular, velocity and position tracking // error magnitudes (rad), (m/s), (m) -void Ekf::get_output_tracking_error(float error[3]) +Vector3f Ekf::getOutputTrackingError() const { - memcpy(error, _output_tracking_error, 3 * sizeof(float)); + return _output_tracking_error; } /* @@ -954,9 +946,9 @@ Returns following IMU vibration metrics in the following array locations 1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) 2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) */ -void Ekf::get_imu_vibe_metrics(float vibe[3]) +Vector3f Ekf::getImuVibrationMetrics() const { - memcpy(vibe, _vibe_metrics, 3 * sizeof(float)); + return _vibe_metrics; } /* @@ -1503,13 +1495,9 @@ void Ekf::calcExtVisRotMat() } // return the quaternions for the rotation from External Vision system reference frame to the EKF reference frame -void Ekf::get_ev2ekf_quaternion(float *quat) +matrix::Quatf Ekf::getVisionAlignmentQuaternion() const { - const Quatf quat_ev2ekf(_R_ev_to_ekf); - - for (unsigned i = 0; i < 4; i++) { - quat[i] = quat_ev2ekf(i); - } + return Quatf(_R_ev_to_ekf); } // Increase the yaw error variance of the quaternions diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index f16672c0de..d0306ad978 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -91,25 +91,25 @@ void EstimatorInterface::computeVibrationMetric() { // calculate a metric which indicates the amount of coning vibration Vector3f temp = _newest_high_rate_imu_sample.delta_ang % _delta_ang_prev; - _vibe_metrics[0] = 0.99f * _vibe_metrics[0] + 0.01f * temp.norm(); + _vibe_metrics(0) = 0.99f * _vibe_metrics(0) + 0.01f * temp.norm(); // calculate a metric which indicates the amount of high frequency gyro vibration temp = _newest_high_rate_imu_sample.delta_ang - _delta_ang_prev; _delta_ang_prev = _newest_high_rate_imu_sample.delta_ang; - _vibe_metrics[1] = 0.99f * _vibe_metrics[1] + 0.01f * temp.norm(); + _vibe_metrics(1) = 0.99f * _vibe_metrics(1) + 0.01f * temp.norm(); // calculate a metric which indicates the amount of high frequency accelerometer vibration temp = _newest_high_rate_imu_sample.delta_vel - _delta_vel_prev; _delta_vel_prev = _newest_high_rate_imu_sample.delta_vel; - _vibe_metrics[2] = 0.99f * _vibe_metrics[2] + 0.01f * temp.norm(); + _vibe_metrics(2) = 0.99f * _vibe_metrics(2) + 0.01f * temp.norm(); } bool EstimatorInterface::checkIfVehicleAtRest(float dt) { // detect if the vehicle is not moving when on ground if (!_control_status.flags.in_air) { - if ((_vibe_metrics[1] * 4.0E4f > _params.is_moving_scaler) - || (_vibe_metrics[2] * 2.1E2f > _params.is_moving_scaler) + if ((_vibe_metrics(1) * 4.0E4f > _params.is_moving_scaler) + || (_vibe_metrics(2) * 2.1E2f > _params.is_moving_scaler) || ((_newest_high_rate_imu_sample.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) { _time_last_move_detect_us = _newest_high_rate_imu_sample.time_us; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index ada016d581..c9f524fcf6 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -116,24 +116,17 @@ public: virtual void getHaglInnovVar(float &hagl_innov_var) const = 0; virtual void getHaglInnovRatio(float &hagl_innov_ratio) const = 0; - virtual void get_state_delayed(float *state) = 0; + virtual matrix::Vector getStateAtFusionHorizonAsVector() const = 0; - virtual void get_wind_velocity(float *wind) = 0; + virtual Vector2f getWindVelocity() const = 0; - virtual void get_wind_velocity_var(float *wind_var) = 0; + virtual Vector2f getWindVelocityVariance() const = 0; virtual void get_true_airspeed(float *tas) = 0; - // gets the variances for the NED velocity states - virtual void get_vel_var(Vector3f &vel_var) = 0; - - // gets the variances for the NED position states - virtual void get_pos_var(Vector3f &pos_var) = 0; - - // return an array containing the output predictor angular, velocity and position tracking // error magnitudes (rad), (m/s), (m) - virtual void get_output_tracking_error(float error[3]) = 0; + virtual Vector3f getOutputTrackingError() const = 0; /* Returns following IMU vibration metrics in the following array locations @@ -141,7 +134,7 @@ public: 1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) 2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) */ - virtual void get_imu_vibe_metrics(float vibe[3]) = 0; + virtual Vector3f getImuVibrationMetrics() const = 0; /* First argument returns GPS drift metrics in the following array locations @@ -281,59 +274,48 @@ public: bool get_terrain_valid() { return isTerrainEstimateValid(); } // get the estimated terrain vertical position relative to the NED origin - virtual void getTerrainVertPos(float *ret) = 0; - //[[deprecated("Replaced by getTerrainVertPos")]] - void get_terrain_vert_pos(float *ret) { getTerrainVertPos(ret); } + virtual float getTerrainVertPos() const = 0; // return true if the local position estimate is valid bool local_position_is_valid(); - const matrix::Quatf &get_quaternion() const { return _output_new.quat_nominal; } + const matrix::Quatf getQuaternion() const { return _output_new.quat_nominal; } // return the quaternion defining the rotation from the EKF to the External Vision reference frame - virtual void get_ev2ekf_quaternion(float *quat) = 0; + virtual matrix::Quatf getVisionAlignmentQuaternion() const = 0; // get the velocity of the body frame origin in local NED earth frame - void get_velocity(float *vel) + Vector3f getVelocity() const { - Vector3f vel_earth = _output_new.vel - _vel_imu_rel_body_ned; - - for (unsigned i = 0; i < 3; i++) { - vel[i] = vel_earth(i); - } + const Vector3f vel_earth = _output_new.vel - _vel_imu_rel_body_ned; + return vel_earth; } - // get the NED velocity derivative in earth frame - void get_vel_deriv_ned(float *vel_deriv) + virtual Vector3f getVelocityVariance() const = 0; + + // get the velocity derivative in earth frame + Vector3f getVelocityDerivative() const { - for (unsigned i = 0; i < 3; i++) { - vel_deriv[i] = _vel_deriv_ned(i); - } + return _vel_deriv; } // get the derivative of the vertical position of the body frame origin in local NED earth frame - void get_pos_d_deriv(float *pos_d_deriv) + float getVerticalPositionDerivative() const { - float var = _output_vert_new.vel_d - _vel_imu_rel_body_ned(2); - *pos_d_deriv = var; + return _output_vert_new.vel_d - _vel_imu_rel_body_ned(2); } - // get the position of the body frame origin in local NED earth frame - void get_position(float *pos) + // get the position of the body frame origin in local earth frame + Vector3f getPosition() const { // rotate the position of the IMU relative to the boy origin into earth frame - Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body; - + const Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body; // subtract from the EKF position (which is at the IMU) to get position at the body origin - for (unsigned i = 0; i < 3; i++) { - pos[i] = _output_new.pos(i) - pos_offset_earth(i); - } - } - void copy_timestamp(uint64_t *time_us) - { - *time_us = _time_last_imu; + return _output_new.pos - pos_offset_earth; } + virtual Vector3f getPositionVariance() const = 0; + // Get the value of magnetic declination in degrees to be saved for use at the next startup // Returns true when the declination can be saved // At the next startup, set param.mag_declination_deg to the value saved @@ -349,8 +331,8 @@ public: } } - virtual void get_accel_bias(float bias[3]) = 0; - virtual void get_gyro_bias(float bias[3]) = 0; + virtual Vector3f getAccelBias() const = 0; + virtual Vector3f getGyroBias() const = 0; // get EKF mode status void get_control_mode(uint32_t *val) @@ -482,7 +464,7 @@ protected: imuSample _newest_high_rate_imu_sample{}; // imu sample capturing the newest imu data Matrix3f _R_to_earth_now; // rotation matrix from body to earth frame at current time Vector3f _vel_imu_rel_body_ned; // velocity of IMU relative to body origin in NED earth frame - Vector3f _vel_deriv_ned; // velocity derivative at the IMU in NED earth frame (m/s/s) + Vector3f _vel_deriv; // velocity derivative at the IMU in NED earth frame (m/s/s) bool _imu_updated{false}; // true if the ekf should update (completed downsampling process) bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized @@ -520,7 +502,7 @@ protected: // IMU vibration and movement monitoring Vector3f _delta_ang_prev; // delta angle from the previous IMU measurement Vector3f _delta_vel_prev; // delta velocity from the previous IMU measurement - float _vibe_metrics[3] {}; // IMU vibration metrics + Vector3f _vibe_metrics; // IMU vibration metrics // [0] Level of coning vibration in the IMU delta angles (rad^2) // [1] high frequency vibration level in the IMU delta angle data (rad) // [2] high frequency vibration level in the IMU delta velocity data (m/s) diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index d0c3494be3..9465c86489 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -307,7 +307,7 @@ void Ekf::updateTerrainValidity() } // get the estimated vertical position of the terrain relative to the NED origin -void Ekf::getTerrainVertPos(float *ret) +float Ekf::getTerrainVertPos() const { - memcpy(ret, &_terrain_vpos, sizeof(float)); + return _terrain_vpos; } diff --git a/test/sensor_simulator/ekf_logger.cpp b/test/sensor_simulator/ekf_logger.cpp index 66e8a57b57..4ff9f7beca 100644 --- a/test/sensor_simulator/ekf_logger.cpp +++ b/test/sensor_simulator/ekf_logger.cpp @@ -58,7 +58,7 @@ void EkfLogger::writeState() _file << time; if(_state_logging_enabled) { - matrix::Vector state = _ekf_wrapper.getState(); + matrix::Vector state = _ekf->getStateAtFusionHorizonAsVector(); for(int i = 0; i < 24; i++) { _file << "," << state(i); diff --git a/test/sensor_simulator/ekf_wrapper.cpp b/test/sensor_simulator/ekf_wrapper.cpp index d4a1a28760..56f57ca314 100644 --- a/test/sensor_simulator/ekf_wrapper.cpp +++ b/test/sensor_simulator/ekf_wrapper.cpp @@ -160,74 +160,12 @@ bool EkfWrapper::isWindVelocityEstimated() const return control_status.flags.wind; } -Vector3f EkfWrapper::getPosition() const -{ - float temp[3]; - _ekf->get_position(temp); - return Vector3f(temp); -} -Vector3f EkfWrapper::getVelocity() const -{ - float temp[3]; - _ekf->get_velocity(temp); - return Vector3f(temp); -} -Vector3f EkfWrapper::getAccelBias() const -{ - float temp[3]; - _ekf->get_accel_bias(temp); - return Vector3f(temp); -} - -Vector3f EkfWrapper::getGyroBias() const -{ - float temp[3]; - _ekf->get_gyro_bias(temp); - return Vector3f(temp); -} - -Quatf EkfWrapper::getQuaternion() const -{ - return _ekf->get_quaternion(); -} - Eulerf EkfWrapper::getEulerAngles() const { - return Eulerf(getQuaternion()); -} - -matrix::Vector EkfWrapper::getState() const -{ - float state[24]; - _ekf->get_state_delayed(state); - return matrix::Vector{state}; + return Eulerf(_ekf->getQuaternion()); } matrix::Vector EkfWrapper::getQuaternionVariance() const { return matrix::Vector(_ekf->orientation_covariances().diag()); } - -Vector3f EkfWrapper::getPositionVariance() const -{ - return Vector3f(_ekf->position_covariances().diag()); -} - -Vector3f EkfWrapper::getVelocityVariance() const -{ - return Vector3f(_ekf->velocity_covariances().diag()); -} - -Vector2f EkfWrapper::getWindVelocity() const -{ - float temp[2]; - _ekf->get_wind_velocity(temp); - return Vector2f(temp); -} - -Quatf EkfWrapper::getVisionAlignmentQuaternion() const -{ - float temp[4]; - _ekf->get_ev2ekf_quaternion(temp); - return Quatf(temp); -} diff --git a/test/sensor_simulator/ekf_wrapper.h b/test/sensor_simulator/ekf_wrapper.h index 764a00c8aa..09805fa291 100644 --- a/test/sensor_simulator/ekf_wrapper.h +++ b/test/sensor_simulator/ekf_wrapper.h @@ -85,19 +85,8 @@ public: bool isWindVelocityEstimated() const; - Vector3f getPosition() const; - Vector3f getVelocity() const; - Vector3f getAccelBias() const; - Vector3f getGyroBias() const; - Quatf getQuaternion() const; Eulerf getEulerAngles() const; - matrix::Vector getState() const; matrix::Vector getQuaternionVariance() const; - Vector3f getPositionVariance() const; - Vector3f getVelocityVariance() const; - Vector2f getWindVelocity() const; - - Quatf getVisionAlignmentQuaternion() const; private: std::shared_ptr _ekf; diff --git a/test/test_EKF_airspeed.cpp b/test/test_EKF_airspeed.cpp index 66dd96d93a..dd399107b7 100644 --- a/test/test_EKF_airspeed.cpp +++ b/test/test_EKF_airspeed.cpp @@ -88,9 +88,9 @@ TEST_F(EkfAirspeedTest, testWindVelocityEstimation) EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated()); - const Vector3f vel = _ekf_wrapper.getVelocity(); - const Vector2f vel_wind_earth = _ekf_wrapper.getWindVelocity(); - const float height_before_pressure_correction = _ekf_wrapper.getPosition()(2); + const Vector3f vel = _ekf->getVelocity(); + const Vector2f vel_wind_earth = _ekf->getWindVelocity(); + const float height_before_pressure_correction = _ekf->getPosition()(2); const Dcmf R_to_earth_sim(_quat_sim); EXPECT_TRUE(matrix::isEqual(vel, simulated_velocity_earth)); @@ -108,7 +108,7 @@ TEST_F(EkfAirspeedTest, testWindVelocityEstimation) _sensor_simulator.runSeconds(20); - const float height_after_pressure_correction = _ekf_wrapper.getPosition()(2); + const float height_after_pressure_correction = _ekf->getPosition()(2); // height increase means that state z decrease due to z axis pointing down const float expected_height_after_pressure_correction = height_before_pressure_correction - expected_height_difference; diff --git a/test/test_EKF_basics.cpp b/test/test_EKF_basics.cpp index a1d989959e..edcdc4c16a 100644 --- a/test/test_EKF_basics.cpp +++ b/test/test_EKF_basics.cpp @@ -113,11 +113,11 @@ TEST_F(EkfBasicsTest, convergesToZero) // GIVEN: initialized EKF with default IMU, baro and mag input _sensor_simulator.runSeconds(4); - Vector3f pos = _ekf_wrapper.getPosition(); - Vector3f vel = _ekf_wrapper.getVelocity(); - Vector3f accel_bias = _ekf_wrapper.getAccelBias(); - Vector3f gyro_bias = _ekf_wrapper.getGyroBias(); - Vector3f ref{0.0f, 0.0f, 0.0f}; + const Vector3f pos = _ekf->getPosition(); + const Vector3f vel = _ekf->getVelocity(); + const Vector3f accel_bias = _ekf->getAccelBias(); + const Vector3f gyro_bias = _ekf->getGyroBias(); + const Vector3f ref{0.0f, 0.0f, 0.0f}; // THEN: EKF should stay or converge to zero EXPECT_TRUE(matrix::isEqual(pos, ref, 0.001f)); @@ -168,17 +168,17 @@ TEST_F(EkfBasicsTest, accleBiasEstimation) { // GIVEN: initialized EKF with default IMU, baro and mag input for 3s // WHEN: Added more sensor measurements with accel bias and gps measurements - Vector3f accel_bias_sim = {0.0f,0.0f,0.1f}; + const Vector3f accel_bias_sim = {0.0f,0.0f,0.1f}; _sensor_simulator.startGps(); _sensor_simulator.setImuBias(accel_bias_sim, Vector3f{0.0f,0.0f,0.0f}); _sensor_simulator.runSeconds(10); - Vector3f pos = _ekf_wrapper.getPosition(); - Vector3f vel = _ekf_wrapper.getVelocity(); - Vector3f accel_bias = _ekf_wrapper.getAccelBias(); - Vector3f gyro_bias = _ekf_wrapper.getGyroBias(); - Vector3f zero{0.0f, 0.0f, 0.0f}; + const Vector3f pos = _ekf->getPosition(); + const Vector3f vel = _ekf->getVelocity(); + const Vector3f accel_bias = _ekf->getAccelBias(); + const Vector3f gyro_bias = _ekf->getGyroBias(); + const Vector3f zero{0.0f, 0.0f, 0.0f}; // THEN: EKF should stay or converge to zero EXPECT_TRUE(matrix::isEqual(pos, zero, 0.01f)); diff --git a/test/test_EKF_externalVision.cpp b/test/test_EKF_externalVision.cpp index de2ebf6b0f..e520b2de22 100644 --- a/test/test_EKF_externalVision.cpp +++ b/test/test_EKF_externalVision.cpp @@ -103,7 +103,7 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic) TEST_F(EkfExternalVisionTest, visionVarianceCheck) { - const Vector3f velVar_init = _ekf_wrapper.getVelocityVariance(); + const Vector3f velVar_init = _ekf->getVelocityVariance(); EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001); _sensor_simulator._vio.setVelocityVariance(Vector3f{2.0f,0.01f,0.01f}); @@ -111,7 +111,7 @@ TEST_F(EkfExternalVisionTest, visionVarianceCheck) _sensor_simulator.startExternalVision(); _sensor_simulator.runSeconds(4); - const Vector3f velVar_new = _ekf_wrapper.getVelocityVariance(); + const Vector3f velVar_new = _ekf->getVelocityVariance(); EXPECT_TRUE(velVar_new(0) > velVar_new(1)); } @@ -131,17 +131,17 @@ TEST_F(EkfExternalVisionTest, visionAlignment) _ekf_wrapper.enableExternalVisionVelocityFusion(); _sensor_simulator.startExternalVision(); - const Vector3f velVar_init = _ekf_wrapper.getVelocityVariance(); + const Vector3f velVar_init = _ekf->getVelocityVariance(); EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001); _sensor_simulator.runSeconds(4); // THEN: velocity uncertainty in y should be bigger - const Vector3f velVar_new = _ekf_wrapper.getVelocityVariance(); + const Vector3f velVar_new = _ekf->getVelocityVariance(); EXPECT_TRUE(velVar_new(1) > velVar_new(0)); // THEN: the frame offset should be estimated correctly - Quatf estimatedExternalVisionFrameOffset = _ekf_wrapper.getVisionAlignmentQuaternion(); + Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion(); EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(), estimatedExternalVisionFrameOffset.canonical())); } diff --git a/test/test_EKF_fusionLogic.cpp b/test/test_EKF_fusionLogic.cpp index f9c0554364..28764d1ee4 100644 --- a/test/test_EKF_fusionLogic.cpp +++ b/test/test_EKF_fusionLogic.cpp @@ -136,25 +136,25 @@ TEST_F(EkfFusionLogicTest, rejectGpsSignalJump) EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); // WHEN: Having a big horizontal position Gps jump coming from the Gps Receiver - Vector3f pos_old = _ekf_wrapper.getPosition(); - Vector3f vel_old = _ekf_wrapper.getVelocity(); - Vector3f accel_bias_old = _ekf_wrapper.getAccelBias(); + const Vector3f pos_old = _ekf->getPosition(); + const Vector3f vel_old = _ekf->getVelocity(); + const Vector3f accel_bias_old = _ekf->getAccelBias(); _sensor_simulator._gps.stepHorizontalPositionByMeters(Vector2f{10.0f, 0.0f}); _sensor_simulator.runSeconds(2); // THEN: The estimate should not change much in the short run // and GPS fusion should be stopped after a while. - Vector3f pos_new = _ekf_wrapper.getPosition(); - Vector3f vel_new = _ekf_wrapper.getVelocity(); - Vector3f accel_bias_new = _ekf_wrapper.getAccelBias(); + Vector3f pos_new = _ekf->getPosition(); + Vector3f vel_new = _ekf->getVelocity(); + Vector3f accel_bias_new = _ekf->getAccelBias(); EXPECT_TRUE(matrix::isEqual(pos_new, pos_old, 0.01f)); EXPECT_TRUE(matrix::isEqual(vel_new, vel_old, 0.01f)); EXPECT_TRUE(matrix::isEqual(accel_bias_new, accel_bias_old, 0.01f)); _sensor_simulator.runSeconds(10); - pos_new = _ekf_wrapper.getPosition(); - vel_new = _ekf_wrapper.getVelocity(); - accel_bias_new = _ekf_wrapper.getAccelBias(); + pos_new = _ekf->getPosition(); + vel_new = _ekf->getVelocity(); + accel_bias_new = _ekf->getAccelBias(); EXPECT_TRUE(matrix::isEqual(pos_new, pos_old, 0.01f)); EXPECT_TRUE(matrix::isEqual(vel_new, vel_old, 0.01f)); EXPECT_TRUE(matrix::isEqual(accel_bias_new, accel_bias_old, 0.01f)); diff --git a/test/test_EKF_initialization.cpp b/test/test_EKF_initialization.cpp index f53bf2d8ae..6bd781a4cc 100644 --- a/test/test_EKF_initialization.cpp +++ b/test/test_EKF_initialization.cpp @@ -64,7 +64,7 @@ class EkfInitializationTest : public ::testing::Test { void initializedOrienationIsMatchingGroundTruth(Quatf true_quaternion) { - Quatf quat_est = _ekf_wrapper.getQuaternion(); + const Quatf quat_est = _ekf->getQuaternion(); EXPECT_TRUE(matrix::isEqual(quat_est, true_quaternion)); } @@ -86,8 +86,8 @@ class EkfInitializationTest : public ::testing::Test { void velocityAndPositionCloseToZero() { - Vector3f pos = _ekf_wrapper.getPosition(); - Vector3f vel = _ekf_wrapper.getVelocity(); + 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)); @@ -95,8 +95,8 @@ class EkfInitializationTest : public ::testing::Test { void velocityAndPositionVarianceBigEnoughAfterOrientationInitialization() { - Vector3f pos_var = _ekf_wrapper.getPositionVariance(); - Vector3f vel_var = _ekf_wrapper.getVelocityVariance(); + const Vector3f pos_var = _ekf->getPositionVariance(); + const Vector3f vel_var = _ekf->getVelocityVariance(); const float pos_variance_limit = 0.2f; EXPECT_TRUE(pos_var(0) > pos_variance_limit) << "pos_var(1)" << pos_var(0);