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
// Use full rate IMU data at the current time horizon
calculateOutputStates();
calculateOutputStates(_newest_high_rate_imu_sample);
return updated;
}
@ -312,10 +312,9 @@ void Ekf::predictState()
* “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
*/
void Ekf::calculateOutputStates()
void Ekf::calculateOutputStates(const imuSample &imu)
{
// Use full rate IMU data at the current time horizon
const imuSample &imu = _newest_high_rate_imu_sample;
// correct delta angles for bias offsets
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
// and the correction step
void calculateOutputStates();
void calculateOutputStates(const imuSample &imu);
void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction);
void applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction);

69
EKF/estimator_interface.cpp
View File

@ -65,14 +65,13 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
_newest_high_rate_imu_sample = imu_sample;
// Do not change order of computeVibrationMetric and checkIfVehicleAtRest
computeVibrationMetric();
_control_status.flags.vehicle_at_rest = checkIfVehicleAtRest(dt);
computeVibrationMetric(imu_sample);
_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 = new_downsampled_imu_sample_ready;
_imu_updated = _imu_down_sampler.update(imu_sample);
// 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());
@ -81,44 +80,44 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
// 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
_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
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();
// 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;
temp = imu.delta_ang - _delta_ang_prev;
_delta_ang_prev = imu.delta_ang;
_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;
temp = imu.delta_vel - _delta_vel_prev;
_delta_vel_prev = imu.delta_vel;
_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
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)
|| ((_newest_high_rate_imu_sample.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) {
|| (_vibe_metrics(2) * 2.1E2f > _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 {
_time_last_move_detect_us = _newest_high_rate_imu_sample.time_us;
_time_last_move_detect_us = imu.time_us;
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.yaw = gps.yaw;
if (ISFINITE(gps.yaw_offset)) {
_gps_yaw_offset = gps.yaw_offset;
} else {
_gps_yaw_offset = 0.0f;
}
@ -305,7 +306,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
}
}
void EstimatorInterface::setRangeData(const rangeSample& range_sample)
void EstimatorInterface::setRangeData(const rangeSample &range_sample)
{
if (!_initialised || _range_buffer_fail) {
return;
@ -334,7 +335,7 @@ void EstimatorInterface::setRangeData(const rangeSample& range_sample)
}
}
void EstimatorInterface::setOpticalFlowData(const flowSample& flow)
void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
{
if (!_initialised || _flow_buffer_fail) {
return;
@ -380,6 +381,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample& flow)
// Check data validity and write to buffers
// 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);
if (use_flow_data_to_navigate || (!_control_status.flags.in_air && relying_on_flow)) {
_time_last_optflow = flow.time_us;
@ -397,7 +399,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample& flow)
}
// set attitude and position data derived from an external vision system
void EstimatorInterface::setExtVisionData(const extVisionSample& evdata)
void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
{
if (!_initialised || _ev_buffer_fail) {
return;
@ -427,7 +429,7 @@ void EstimatorInterface::setExtVisionData(const extVisionSample& evdata)
}
}
void EstimatorInterface::setAuxVelData(const auxVelSample& auxvel_sample)
void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
{
if (!_initialised || _auxvel_buffer_fail) {
return;
@ -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
// sufficient samples have been collected
@ -476,10 +478,10 @@ void EstimatorInterface::setDragData()
_drag_sample_count ++;
// 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(1) += _newest_high_rate_imu_sample.delta_vel(1);
_drag_down_sampled.time_us += _newest_high_rate_imu_sample.time_us;
_drag_sample_time_dt += _newest_high_rate_imu_sample.delta_vel_dt;
_drag_down_sampled.accelXY(0) += imu.delta_vel(0);
_drag_down_sampled.accelXY(1) += imu.delta_vel(1);
_drag_down_sampled.time_us += imu.time_us;
_drag_sample_time_dt += imu.delta_vel_dt;
// calculate the downsample ratio for drag specific force data
uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length);
@ -588,9 +590,9 @@ bool EstimatorInterface::isOtherSourceOfHorizontalAidingThan(const bool aiding_f
int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const
{
return int(_control_status.flags.gps)
+ int(_control_status.flags.opt_flow)
+ int(_control_status.flags.ev_pos)
+ int(_control_status.flags.ev_vel);
+ int(_control_status.flags.opt_flow)
+ int(_control_status.flags.ev_pos)
+ int(_control_status.flags.ev_vel);
}
bool EstimatorInterface::isHorizontalAidingActive() const
@ -598,10 +600,9 @@ bool EstimatorInterface::isHorizontalAidingActive() const
return getNumberOfActiveHorizontalAidingSources() > 0;
}
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);
}
}

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.
// 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.
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
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
filter_control_status_u _control_status_prev{};
inline void setDragData();
inline void setDragData(const imuSample &imu);
inline void computeVibrationMetric();
inline bool checkIfVehicleAtRest(float dt);
inline void computeVibrationMetric(const imuSample &imu);
inline bool checkIfVehicleAtRest(float dt, const imuSample &imu);
virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) = 0;