dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: pass imuSample by const reference

This commit is contained in:
Daniel Agar 2020-10-29 11:30:49 -04:00
parent d85e24d3ca
commit defb35d8f5
4 changed files with 43 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/ekf.cpp
View File

@ -132,7 +132,7 @@ bool Ekf::update()
// the output observer always runs // the output observer always runs
// Use full rate IMU data at the current time horizon // Use full rate IMU data at the current time horizon
calculateOutputStates(); calculateOutputStates(_newest_high_rate_imu_sample);
return updated; return updated;
} }
@ -312,10 +312,9 @@ void Ekf::predictState()
* “Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements” * “Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements”
* A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University * A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University
*/ */
void Ekf::calculateOutputStates() void Ekf::calculateOutputStates(const imuSample &imu)
{ {
// Use full rate IMU data at the current time horizon // Use full rate IMU data at the current time horizon
const imuSample &imu = _newest_high_rate_imu_sample;
// correct delta angles for bias offsets // correct delta angles for bias offsets
const float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg; const float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg;

2
EKF/ekf.h
View File

@ -502,7 +502,7 @@ private:
// update the real time complementary filter states. This includes the prediction // update the real time complementary filter states. This includes the prediction
// and the correction step // and the correction step
void calculateOutputStates(); void calculateOutputStates(const imuSample &imu);
void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction); void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction);
void applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction); void applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction);

51
EKF/estimator_interface.cpp
View File

@ -65,14 +65,13 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
_newest_high_rate_imu_sample = imu_sample; _newest_high_rate_imu_sample = imu_sample;
// Do not change order of computeVibrationMetric and checkIfVehicleAtRest // Do not change order of computeVibrationMetric and checkIfVehicleAtRest
computeVibrationMetric(); computeVibrationMetric(imu_sample);
_control_status.flags.vehicle_at_rest = checkIfVehicleAtRest(dt); _control_status.flags.vehicle_at_rest = checkIfVehicleAtRest(dt, imu_sample);
const bool new_downsampled_imu_sample_ready = _imu_down_sampler.update(_newest_high_rate_imu_sample); _imu_updated = _imu_down_sampler.update(imu_sample);
_imu_updated = new_downsampled_imu_sample_ready;
// accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available // accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
if (new_downsampled_imu_sample_ready) { if (_imu_updated) {
_imu_buffer.push(_imu_down_sampler.getDownSampledImuAndTriggerReset()); _imu_buffer.push(_imu_down_sampler.getDownSampledImuAndTriggerReset());
@ -81,44 +80,44 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
// calculate the minimum interval between observations required to guarantee no loss of data // calculate the minimum interval between observations required to guarantee no loss of data
// this will occur if data is overwritten before its time stamp falls behind the fusion time horizon // this will occur if data is overwritten before its time stamp falls behind the fusion time horizon
_min_obs_interval_us = (_newest_high_rate_imu_sample.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1); _min_obs_interval_us = (imu_sample.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1);
setDragData(); setDragData(imu_sample);
} }
} }
void EstimatorInterface::computeVibrationMetric() void EstimatorInterface::computeVibrationMetric(const imuSample &imu)
{ {
// calculate a metric which indicates the amount of coning vibration // calculate a metric which indicates the amount of coning vibration
Vector3f temp = _newest_high_rate_imu_sample.delta_ang % _delta_ang_prev; Vector3f temp = imu.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 // calculate a metric which indicates the amount of high frequency gyro vibration
temp = _newest_high_rate_imu_sample.delta_ang - _delta_ang_prev; temp = imu.delta_ang - _delta_ang_prev;
_delta_ang_prev = _newest_high_rate_imu_sample.delta_ang; _delta_ang_prev = imu.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 // calculate a metric which indicates the amount of high frequency accelerometer vibration
temp = _newest_high_rate_imu_sample.delta_vel - _delta_vel_prev; temp = imu.delta_vel - _delta_vel_prev;
_delta_vel_prev = _newest_high_rate_imu_sample.delta_vel; _delta_vel_prev = imu.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) bool EstimatorInterface::checkIfVehicleAtRest(float dt, const imuSample &imu)
{ {
// detect if the vehicle is not moving when on ground // detect if the vehicle is not moving when on ground
if (!_control_status.flags.in_air) { if (!_control_status.flags.in_air) {
if ((_vibe_metrics(1) * 4.0E4f > _params.is_moving_scaler) if ((_vibe_metrics(1) * 4.0E4f > _params.is_moving_scaler)
|| (_vibe_metrics(2) * 2.1E2f > _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)) { || ((imu.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) {
_time_last_move_detect_us = _newest_high_rate_imu_sample.time_us; _time_last_move_detect_us = imu.time_us;
} }
return ((_newest_high_rate_imu_sample.time_us - _time_last_move_detect_us) > (uint64_t)1E6); return ((imu.time_us - _time_last_move_detect_us) > (uint64_t)1E6);
} else { } else {
_time_last_move_detect_us = _newest_high_rate_imu_sample.time_us; _time_last_move_detect_us = imu.time_us;
return false; return false;
} }
} }
@ -207,8 +206,10 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
gps_sample_new.hgt = (float)gps.alt * 1e-3f; gps_sample_new.hgt = (float)gps.alt * 1e-3f;
gps_sample_new.yaw = gps.yaw; gps_sample_new.yaw = gps.yaw;
if (ISFINITE(gps.yaw_offset)) { if (ISFINITE(gps.yaw_offset)) {
_gps_yaw_offset = gps.yaw_offset; _gps_yaw_offset = gps.yaw_offset;
} else { } else {
_gps_yaw_offset = 0.0f; _gps_yaw_offset = 0.0f;
} }
@ -380,6 +381,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample& flow)
// Check data validity and write to buffers // Check data validity and write to buffers
// Invalid flow data is allowed when on ground and is handled as a special case in controlOpticalFlowFusion() // Invalid flow data is allowed when on ground and is handled as a special case in controlOpticalFlowFusion()
bool use_flow_data_to_navigate = delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow); bool use_flow_data_to_navigate = delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow);
if (use_flow_data_to_navigate || (!_control_status.flags.in_air && relying_on_flow)) { if (use_flow_data_to_navigate || (!_control_status.flags.in_air && relying_on_flow)) {
_time_last_optflow = flow.time_us; _time_last_optflow = flow.time_us;
@ -457,7 +459,7 @@ void EstimatorInterface::setAuxVelData(const auxVelSample& auxvel_sample)
} }
} }
void EstimatorInterface::setDragData() void EstimatorInterface::setDragData(const imuSample &imu)
{ {
// down-sample the drag specific force data by accumulating and calculating the mean when // down-sample the drag specific force data by accumulating and calculating the mean when
// sufficient samples have been collected // sufficient samples have been collected
@ -476,10 +478,10 @@ void EstimatorInterface::setDragData()
_drag_sample_count ++; _drag_sample_count ++;
// note acceleration is accumulated as a delta velocity // note acceleration is accumulated as a delta velocity
_drag_down_sampled.accelXY(0) += _newest_high_rate_imu_sample.delta_vel(0); _drag_down_sampled.accelXY(0) += imu.delta_vel(0);
_drag_down_sampled.accelXY(1) += _newest_high_rate_imu_sample.delta_vel(1); _drag_down_sampled.accelXY(1) += imu.delta_vel(1);
_drag_down_sampled.time_us += _newest_high_rate_imu_sample.time_us; _drag_down_sampled.time_us += imu.time_us;
_drag_sample_time_dt += _newest_high_rate_imu_sample.delta_vel_dt; _drag_sample_time_dt += imu.delta_vel_dt;
// calculate the downsample ratio for drag specific force data // calculate the downsample ratio for drag specific force data
uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length); uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length);
@ -600,8 +602,7 @@ bool EstimatorInterface::isHorizontalAidingActive() const
void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name) void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
{ {
if(buffer_name) if (buffer_name) {
{
ECL_ERR("%s buffer allocation failed", buffer_name); ECL_ERR("%s buffer allocation failed", buffer_name);
} }
} }

9
EKF/estimator_interface.h
View File

@ -378,7 +378,8 @@ public:
// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned. // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
virtual void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, float &hagl, float &beta) = 0; virtual void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
float &hagl, float &beta) = 0;
// return a bitmask integer that describes which state estimates can be used for flight control // return a bitmask integer that describes which state estimates can be used for flight control
virtual void get_ekf_soln_status(uint16_t *status) = 0; virtual void get_ekf_soln_status(uint16_t *status) = 0;
@ -586,10 +587,10 @@ protected:
// this is the previous status of the filter control modes - used to detect mode transitions // this is the previous status of the filter control modes - used to detect mode transitions
filter_control_status_u _control_status_prev{}; filter_control_status_u _control_status_prev{};
inline void setDragData(); inline void setDragData(const imuSample &imu);
inline void computeVibrationMetric(); inline void computeVibrationMetric(const imuSample &imu);
inline bool checkIfVehicleAtRest(float dt); inline bool checkIfVehicleAtRest(float dt, const imuSample &imu);
virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) = 0; virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) = 0;