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

EKF: Add specific drag fusion tuning parameters

This commit is contained in:
Paul Riseborough 2017-04-10 14:59:44 +10:00 committed by Lorenz Meier
parent 465b145929
commit 5cf31e439d
2 changed files with 17 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
EKF/common.h
View File

@ -286,6 +286,11 @@ struct parameters {
unsigned no_aid_timeout_max; // maximum lapsed time from last fusion of measurements that constrain drift before
// the EKF will report that it is dead-reckoning (usec)
// multi-rotor drag specific force fusion
float drag_noise; // observation noise for drag specific force measurements (m/sec**2)
float bcoef_x; // ballistic coefficient along the X-axis (kg/m**2)
float bcoef_y; // ballistic coefficient along the Y-axis (kg/m**2)
// Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility.
parameters()
{
@ -397,6 +402,11 @@ struct parameters {
no_gps_timeout_max = 7e6;
no_aid_timeout_max = 1e6;
// multi-rotor drag specific force fusion
drag_noise = 2.5f;
bcoef_x = 25.0f;
bcoef_y = 25.0f;
}
};

19
EKF/drag_fusion.cpp
View File

@ -48,22 +48,17 @@ void Ekf::fuseDrag()
float H_ACC[24] = {}; // Observation Jacobian
float SK_ACC[9] = {}; // Variable used to optimise calculations of the Kalman gain vector
float Kfusion[24] = {}; // Kalman gain vector
float R_ACC = 2.5f; // observation noise in specific force drag (m/sec**2) TODO add tuning parameter
float R_ACC = _params.drag_noise; // observation noise in specific force drag (m/sec**2)
float eas2tas = 1.0f; // conversion factor from EAS to TAS - TODO calculate from barometric pressure
float rho = 1.225f / sqrtf(eas2tas); // air density (kg/m**3)
// measured steady state EAS at specified tilt angle in X and Y directions
// TODO make the speeds tuning parameters
const float max_spd_x = 15.0f;
const float max_spd_y = 15.0f;
const float tilt_ref_angle = math::radians(25.0f);
// calculate inverse of ballistic coefficient from max speed and tilt angle
// used to predict the acceleration for a given predicted state vector
float numerator = 2.0f*_gravity_mss*tanf(tilt_ref_angle);
float BC_inv_x = numerator / (rho * sq(max_spd_x));
float BC_inv_y = numerator / (rho * sq(max_spd_y));
// calculate inverse of ballistic coefficient
if (_params.bcoef_x < 1.0f || _params.bcoef_y < 1.0f) {
return;
}
float BC_inv_x = 1.0f / _params.bcoef_x;
float BC_inv_y = 1.0f / _params.bcoef_y;
// get latest estimated orientation
float q0 = _state.quat_nominal(0);