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

EKF: down-sample drag specific force data

This commit is contained in:
Paul Riseborough 2017-04-11 20:59:34 +10:00 committed by Lorenz Meier
parent 9f48c0505b
commit 3758c5a09d
5 changed files with 60 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/common.h
View File

@ -140,6 +140,11 @@ struct extVisionSample {
uint64_t time_us; // timestamp of the measurement in microseconds
};
struct dragSample {
Vector2f accelXY; // measured specific force along the X and Y body axes (m/s**2)
uint64_t time_us; // timestamp in microseconds of the measurement
};
// Integer definitions for vdist_sensor_type
#define VDIST_SENSOR_BARO 0 // Use baro height
#define VDIST_SENSOR_GPS 1 // Use GPS height

4
EKF/control.cpp
View File

@ -865,12 +865,14 @@ void Ekf::controlDragFusion()
{
if (_params.fusion_mode & MASK_USE_DRAG && _control_status.flags.in_air) {
if (!_control_status.flags.wind) {
// reset the wind states and covariances when starting drag accel fusion
_control_status.flags.wind = true;
resetWindStates();
resetWindCovariance();
} else {
} else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) {
fuseDrag();
}
} else {
_control_status.flags.wind = false;

13
EKF/drag_fusion.cpp
View File

@ -75,11 +75,6 @@ void Ekf::fuseDrag()
float vwn = _state.wind_vel(0);
float vwe = _state.wind_vel(1);
// calculate measured XY specific forces if enough data
if (_imu_sample_delayed.delta_vel_dt < 1e-3f) {
return;
}
// predicted specific forces
// calculate relative wind velocity in earth frame and rotte into body frame
Vector3f rel_wind;
@ -95,11 +90,11 @@ void Ekf::fuseDrag()
// calculate observation jacobiam and Kalman gain vectors
if (axis_index == 0) {
// Estimate the airspeed from the measured drag force and ballistic coefficient
float mea_acc = (_imu_sample_delayed.delta_vel(axis_index) - _state.accel_bias(axis_index))/_imu_sample_delayed.delta_vel_dt;
float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.accel_bias(axis_index) / _dt_ekf_avg;
float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_x * rho));
// Estimate the derivative of specific force wrt airspeed along the X axis
// Limit lower value to prevent artithmetic exceptions
// Limit lower value to prevent arithmetic exceptions
float Kacc = fmax(1e-1f,rho * BC_inv_x * airSpd);
SH_ACC[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
@ -165,11 +160,11 @@ void Ekf::fuseDrag()
} else if (axis_index == 1) {
// Estimate the airspeed from the measured drag force and ballistic coefficient
float mea_acc = (_imu_sample_delayed.delta_vel(axis_index) - _state.accel_bias(axis_index))/_imu_sample_delayed.delta_vel_dt;
float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.accel_bias(axis_index) / _dt_ekf_avg;
float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_y * rho));
// Estimate the derivative of specific force wrt airspeed along the X axis
// Limit lower value to prevent artithmetic exceptions
// Limit lower value to prevent arithmetic exceptions
float Kacc = fmax(1e-1f,rho * BC_inv_y * airSpd);
SH_ACC[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);

43
EKF/estimator_interface.cpp
View File

@ -59,6 +59,10 @@ EstimatorInterface::EstimatorInterface():
_airspeed_sample_delayed{},
_flow_sample_delayed{},
_ev_sample_delayed{},
_drag_sample_delayed{},
_drag_down_sampled{},
_drag_sample_count(0),
_drag_sample_time_dt(0.0f),
_imu_ticks(0),
_imu_updated(false),
_initialised(false),
@ -143,6 +147,42 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
_imu_ticks = 0;
_imu_updated = true;
// down-sample the drag specific force data by accumulating and calculating the mean when
// sufficient samples have been collected
if (_params.fusion_mode & MASK_USE_DRAG) {
_drag_sample_count ++;
// note acceleration is accumulated as a delta velocity
_drag_down_sampled.accelXY(0) += imu_sample_new.delta_vel(0);
_drag_down_sampled.accelXY(1) += imu_sample_new.delta_vel(1);
_drag_down_sampled.time_us += imu_sample_new.time_us;
_drag_sample_time_dt += imu_sample_new.delta_vel_dt;
// calculate the downsample ratio for drag specific force data
uint8_t min_sample_ratio = (uint8_t) ceil((float)_imu_buffer_length / _obs_buffer_length);
if (min_sample_ratio < 5) {
min_sample_ratio = 5;
}
// calculate and store means from accumulated values
if (_drag_sample_count >= min_sample_ratio) {
// note conversion from accumulated delta velocity to acceleration
_drag_down_sampled.accelXY(0) /= _drag_sample_time_dt;
_drag_down_sampled.accelXY(1) /= _drag_sample_time_dt;
_drag_down_sampled.time_us /= _drag_sample_count;
// write to buffer
_drag_buffer.push(_drag_down_sampled);
// reset accumulators
_drag_sample_count = 0;
_drag_down_sampled.accelXY.zero();
_drag_down_sampled.time_us = 0;
_drag_sample_time_dt = 0.0f;
}
}
// get the oldest data from the buffer
_imu_sample_delayed = _imu_buffer.get_oldest();
@ -387,6 +427,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
_airspeed_buffer.allocate(_obs_buffer_length) &&
_flow_buffer.allocate(_obs_buffer_length) &&
_ext_vision_buffer.allocate(_obs_buffer_length) &&
_drag_buffer.allocate(_obs_buffer_length) &&
_output_buffer.allocate(_imu_buffer_length))) {
ECL_ERR("EKF buffer allocation failed!");
unallocate_buffers();
@ -409,6 +450,8 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
_flow_buffer.push(flow_sample_init);
extVisionSample ext_vision_sample_init = {};
_ext_vision_buffer.push(ext_vision_sample_init);
dragSample drag_sample_init = {};
_drag_buffer.push(drag_sample_init);
}
// zero the data in the imu data and output observer state buffers

5
EKF/estimator_interface.h
View File

@ -319,6 +319,10 @@ protected:
airspeedSample _airspeed_sample_delayed;
flowSample _flow_sample_delayed;
extVisionSample _ev_sample_delayed;
dragSample _drag_sample_delayed;
dragSample _drag_down_sampled; // down sampled drag specific force data (filter prediction rate -> observation rate)
uint8_t _drag_sample_count; // number of drag specific force samples assumulated at the filter prediction rate
float _drag_sample_time_dt; // time integral across all samples used to form _drag_down_sampled (sec)
outputSample _output_sample_delayed; // filter output on the delayed time horizon
outputSample _output_new; // filter output on the non-delayed time horizon
@ -368,6 +372,7 @@ protected:
RingBuffer<flowSample> _flow_buffer;
RingBuffer<extVisionSample> _ext_vision_buffer;
RingBuffer<outputSample> _output_buffer;
RingBuffer<dragSample> _drag_buffer;
uint64_t _time_last_imu; // timestamp of last imu sample in microseconds
uint64_t _time_last_gps; // timestamp of last gps measurement in microseconds