Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: rework parameter handling 

and fixed indentation
This commit is contained in:
Andrew Tridgell 2020-11-23 19:14:28 +11:00
parent 1c1c067dee
commit be4d10a95e
4 changed files with 244 additions and 251 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -188,25 +188,9 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Units: m // @Units: m
AP_GROUPINFO("GLITCH_RAD", 7, NavEKF3, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT), AP_GROUPINFO("GLITCH_RAD", 7, NavEKF3, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),
// bluff body drag fusion // 8 previously used for EKF3_GPS_DELAY parameter that has been deprecated.
// The EKF now takes its GPS delay form the GPS library with the default delays
// @Param: BCOEF_X // specified by the GPS_DELAY and GPS_DELAY2 parameters.
// @DisplayName: Ballistic coefficient measured in X direction
// @Description: Ratio of mass to drag coefficient measured along the X body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the frontal area. The predicted drag from the rotors is specified separately by the EK3_MCOEF parameter.
// @Range: 0.0 1000.0
// @User: Advanced
// @Param: BCOEF_Y
// @DisplayName: Ballistic coefficient measured in Y direction
// @Description: Ratio of mass to drag coefficient measured along the Y body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the side area. The predicted drag from the rotors is specified separately by the EK3_MCOEF parameter.
// @Range: 50.0 1000.0
// @User: Advanced
// @Param: BCOEF_Z
// @DisplayName: unused
// @Description: unused
// @User: Advanced
AP_GROUPINFO("BCOEF", 8, NavEKF3, _ballisticCoef, 0.0f),
// Height measurement parameters // Height measurement parameters
@ -372,14 +356,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Units: rad/s/s // @Units: rad/s/s
AP_GROUPINFO("GBIAS_P_NSE", 26, NavEKF3, _gyroBiasProcessNoise, GBIAS_P_NSE_DEFAULT), AP_GROUPINFO("GBIAS_P_NSE", 26, NavEKF3, _gyroBiasProcessNoise, GBIAS_P_NSE_DEFAULT),
// @Param: DRAG_M_NSE // 27 previously used for EK2_GSCL_P_NSE parameter that has been removed
// @DisplayName: Observation noise for drag acceleration
// @Description: This sets the amount of noise used when fusing X and Y acceleration as an observation that enables esitmation of wind velocity for multi-rotor vehicles. This feature is enabled by the EK3_BCOEF_X and EK3_BCOEF_Y parameters
// @Range: 0.1 2.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s/s
AP_GROUPINFO("DRAG_M_NSE", 27, NavEKF3, _dragObsNoise, 0.5f),
// @Param: ABIAS_P_NSE // @Param: ABIAS_P_NSE
// @DisplayName: Accelerometer bias stability (m/s^3) // @DisplayName: Accelerometer bias stability (m/s^3)
@ -389,13 +366,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Units: m/s/s/s // @Units: m/s/s/s
AP_GROUPINFO("ABIAS_P_NSE", 28, NavEKF3, _accelBiasProcessNoise, ABIAS_P_NSE_DEFAULT), AP_GROUPINFO("ABIAS_P_NSE", 28, NavEKF3, _accelBiasProcessNoise, ABIAS_P_NSE_DEFAULT),
// @Param: MCOEF // 29 previously used for EK2_MAG_P_NSE parameter that has been replaced with EK3_MAGE_P_NSE and EK3_MAGB_P_NSE
// @DisplayName: Momentum drag coefficient
// @Description: This parameter is used to predict the drag produced by the rotors when flying a multi-copter, enabling estimation of wind drift. It represents the ratio of rotor drag induced acceleration to airspeed. The drag produced by this effect scales with speed not speed squared and is produced because some of the air velocity normal to the rotors axis of rotation is lost when passing through the rotor disc which changes the momentum of the airflow causing drag. For unducted rotors the effect is roughly proportional to the area of the propeller blades when viewed side on an will change with different propellers. For example if flying at 15 m/s at sea level conditions produces a rotor induced drag acceleration of 1.5 m/s/s, then EK3_MCOEF would be set to 0.1 = (1.5/15.0). Set EK3_MCOEFto a postive value to enable wind estimation using this drag effect. To account for the drag produced by the body which scales with speed squared, see documentation for the EK3_BCOEF_X and EK3_BCOEF_Y parameters.
// @Range: 0.0 1.0
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("MCOEF", 29, NavEKF3, _momentumDragCoef, 0.0f),
// @Param: WIND_P_NSE // @Param: WIND_P_NSE
// @DisplayName: Wind velocity process noise (m/s^2) // @DisplayName: Wind velocity process noise (m/s^2)
@ -685,6 +656,37 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = {
// @Path: ../AP_NavEKF/AP_NavEKF_Source.cpp // @Path: ../AP_NavEKF/AP_NavEKF_Source.cpp
AP_SUBGROUPINFO(sources, "SRC", 1, NavEKF3, AP_NavEKF_Source), AP_SUBGROUPINFO(sources, "SRC", 1, NavEKF3, AP_NavEKF_Source),
// @Param: BCOEF_X
// @DisplayName: Ballistic coefficient measured in X direction
// @Description: Ratio of mass to drag coefficient measured along the X body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the frontal area. The predicted drag from the rotors is specified separately by the EK3_MCOEF parameter.
// @Range: 0.0 1000.0
// @User: Advanced
AP_GROUPINFO("BCOEF_X", 2, NavEKF3, _ballisticCoef_x, 0.0f),
// @Param: BCOEF_Y
// @DisplayName: Ballistic coefficient measured in Y direction
// @Description: Ratio of mass to drag coefficient measured along the Y body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the side area. The predicted drag from the rotors is specified separately by the EK3_MCOEF parameter.
// @Range: 50.0 1000.0
// @User: Advanced
AP_GROUPINFO("BCOEF_Y", 3, NavEKF3, _ballisticCoef_y, 0.0f),
// @Param: DRAG_M_NSE
// @DisplayName: Observation noise for drag acceleration
// @Description: This sets the amount of noise used when fusing X and Y acceleration as an observation that enables esitmation of wind velocity for multi-rotor vehicles. This feature is enabled by the EK3_BCOEF_X and EK3_BCOEF_Y parameters
// @Range: 0.1 2.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s/s
AP_GROUPINFO("DRAG_M_NSE", 4, NavEKF3, _dragObsNoise, 0.5f),
// @Param: MCOEF
// @DisplayName: Momentum drag coefficient
// @Description: This parameter is used to predict the drag produced by the rotors when flying a multi-copter, enabling estimation of wind drift. It represents the ratio of rotor drag induced acceleration to airspeed. The drag produced by this effect scales with speed not speed squared and is produced because some of the air velocity normal to the rotors axis of rotation is lost when passing through the rotor disc which changes the momentum of the airflow causing drag. For unducted rotors the effect is roughly proportional to the area of the propeller blades when viewed side on an will change with different propellers. For example if flying at 15 m/s at sea level conditions produces a rotor induced drag acceleration of 1.5 m/s/s, then EK3_MCOEF would be set to 0.1 = (1.5/15.0). Set EK3_MCOEFto a postive value to enable wind estimation using this drag effect. To account for the drag produced by the body which scales with speed squared, see documentation for the EK3_BCOEF_X and EK3_BCOEF_Y parameters.
// @Range: 0.0 1.0
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("MCOEF", 5, NavEKF3, _momentumDragCoef, 0.0f),
AP_GROUPEND AP_GROUPEND
}; };
@ -1408,15 +1410,6 @@ void NavEKF3::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posI
} }
} }
// publish output observer angular, velocity and position tracking error
void NavEKF3::getOutputTrackingError(int8_t instance, Vector3f &error) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
core[instance].getOutputTrackingError(error);
}
}
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{ {

6
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -181,9 +181,6 @@ public:
// An out of range instance (eg -1) returns data for the primary instance // An out of range instance (eg -1) returns data for the primary instance
void getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const; void getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
// publish output observer angular, velocity and position tracking error
void getOutputTrackingError(int8_t instance, Vector3f &error) const;
// return the innovation consistency test ratios for the specified instance // return the innovation consistency test ratios for the specified instance
// An out of range instance (eg -1) returns data for the primary instance // An out of range instance (eg -1) returns data for the primary instance
void getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const; void getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
@ -462,7 +459,8 @@ private:
AP_Float _err_thresh; // lanes have to be consistently better than the primary by at least this threshold to reduce their overall relativeCoreError AP_Float _err_thresh; // lanes have to be consistently better than the primary by at least this threshold to reduce their overall relativeCoreError
AP_Int32 _affinity; // bitmask of sensor affinity options AP_Int32 _affinity; // bitmask of sensor affinity options
AP_Float _dragObsNoise; // drag specific force observatoin noise (m/s/s)**2 AP_Float _dragObsNoise; // drag specific force observatoin noise (m/s/s)**2
AP_Vector3f _ballisticCoef; // ballistic coefficient measured for flow in X and Y body frame directions (Z is unused) AP_Float _ballisticCoef_x; // ballistic coefficient measured for flow in X body frame directions
AP_Float _ballisticCoef_y; // ballistic coefficient measured for flow in Y body frame directions
AP_Float _momentumDragCoef; // lift rotor momentum drag coefficient AP_Float _momentumDragCoef; // lift rotor momentum drag coefficient
// Possible values for _flowUse // Possible values for _flowUse

31
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -457,10 +457,11 @@ void NavEKF3_core::FuseSideslip()
void NavEKF3_core::FuseDragForces() void NavEKF3_core::FuseDragForces()
{ {
// drag model parameters // drag model parameters
const Vector3f bcoef = frontend->_ballisticCoef.get(); const float bcoef_x = frontend->_ballisticCoef_x;
const float bcoef_y = frontend->_ballisticCoef_x;
const float mcoef = frontend->_momentumDragCoef.get(); const float mcoef = frontend->_momentumDragCoef.get();
const bool using_bcoef_x = bcoef.x > 1.0f; const bool using_bcoef_x = bcoef_x > 1.0f;
const bool using_bcoef_y = bcoef.y > 1.0f; const bool using_bcoef_y = bcoef_y > 1.0f;
const bool using_mcoef = mcoef > 0.001f; const bool using_mcoef = mcoef > 0.001f;
memset (&Kfusion, 0, sizeof(Kfusion)); memset (&Kfusion, 0, sizeof(Kfusion));
@ -507,18 +508,18 @@ void NavEKF3_core::FuseDragForces()
float Kacc; // Derivative of specific force wrt airspeed float Kacc; // Derivative of specific force wrt airspeed
if (using_mcoef && using_bcoef_x) { if (using_mcoef && using_bcoef_x) {
// mixed bluff body and propeller momentum drag // mixed bluff body and propeller momentum drag
const float airSpd = (bcoef.x / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef.x) * fabsf(mea_acc))); const float airSpd = (bcoef_x / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef_x) * fabsf(mea_acc)));
Kacc = fmaxf(1e-1f, (rho / bcoef.x) * airSpd + mcoef * density_ratio); Kacc = fmaxf(1e-1f, (rho / bcoef_x) * airSpd + mcoef * density_ratio);
predAccel = (0.5f / bcoef[0]) * rho * sq(rel_wind_body[0]) * dragForceSign - rel_wind_body[0] * mcoef * density_ratio; predAccel = (0.5f / bcoef_x) * rho * sq(rel_wind_body[0]) * dragForceSign - rel_wind_body[0] * mcoef * density_ratio;
} else if (using_mcoef) { } else if (using_mcoef) {
// propeller momentum drag only // propeller momentum drag only
Kacc = fmaxf(1e-1f, mcoef * density_ratio); Kacc = fmaxf(1e-1f, mcoef * density_ratio);
predAccel = - rel_wind_body[0] * mcoef * density_ratio; predAccel = - rel_wind_body[0] * mcoef * density_ratio;
} else if (using_bcoef_x) { } else if (using_bcoef_x) {
// bluff body drag only // bluff body drag only
const float airSpd = sqrtf((2.0f * bcoef.x * fabsf(mea_acc)) / rho); const float airSpd = sqrtf((2.0f * bcoef_x * fabsf(mea_acc)) / rho);
Kacc = fmaxf(1e-1f, (rho / bcoef.x) * airSpd); Kacc = fmaxf(1e-1f, (rho / bcoef_x) * airSpd);
predAccel = (0.5f / bcoef[0]) * rho * sq(rel_wind_body[0]) * dragForceSign; predAccel = (0.5f / bcoef_x) * rho * sq(rel_wind_body[0]) * dragForceSign;
} else { } else {
// skip this axis // skip this axis
continue; continue;
@ -611,18 +612,18 @@ void NavEKF3_core::FuseDragForces()
float Kacc; // Derivative of specific force wrt airspeed float Kacc; // Derivative of specific force wrt airspeed
if (using_mcoef && using_bcoef_y) { if (using_mcoef && using_bcoef_y) {
// mixed bluff body and propeller momentum drag // mixed bluff body and propeller momentum drag
const float airSpd = (bcoef.y / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef.y) * fabsf(mea_acc))); const float airSpd = (bcoef_y / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef_y) * fabsf(mea_acc)));
Kacc = fmaxf(1e-1f, (rho / bcoef.y) * airSpd + mcoef * density_ratio); Kacc = fmaxf(1e-1f, (rho / bcoef_y) * airSpd + mcoef * density_ratio);
predAccel = (0.5f / bcoef[1]) * rho * sq(rel_wind_body[1]) * dragForceSign - rel_wind_body[1] * mcoef * density_ratio; predAccel = (0.5f / bcoef_y) * rho * sq(rel_wind_body[1]) * dragForceSign - rel_wind_body[1] * mcoef * density_ratio;
} else if (using_mcoef) { } else if (using_mcoef) {
// propeller momentum drag only // propeller momentum drag only
Kacc = fmaxf(1e-1f, mcoef * density_ratio); Kacc = fmaxf(1e-1f, mcoef * density_ratio);
predAccel = - rel_wind_body[1] * mcoef * density_ratio; predAccel = - rel_wind_body[1] * mcoef * density_ratio;
} else if (using_bcoef_y) { } else if (using_bcoef_y) {
// bluff body drag only // bluff body drag only
const float airSpd = sqrtf((2.0f * bcoef.y * fabsf(mea_acc)) / rho); const float airSpd = sqrtf((2.0f * bcoef_y * fabsf(mea_acc)) / rho);
Kacc = fmaxf(1e-1f, (rho / bcoef.y) * airSpd); Kacc = fmaxf(1e-1f, (rho / bcoef_y) * airSpd);
predAccel = (0.5f / bcoef[1]) * rho * sq(rel_wind_body[1]) * dragForceSign; predAccel = (0.5f / bcoef_y) * rho * sq(rel_wind_body[1]) * dragForceSign;
} else { } else {
// nothing more to do // nothing more to do
return; return;

7
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -1362,10 +1362,11 @@ void NavEKF3_core::updateMovementCheck(void)
void NavEKF3_core::SampleDragData(const imu_elements &imu) void NavEKF3_core::SampleDragData(const imu_elements &imu)
{ {
// Average and down sample to 5Hz // Average and down sample to 5Hz
const Vector3f bcoef = frontend->_ballisticCoef.get(); const float bcoef_x = frontend->_ballisticCoef_x;
const float bcoef_y = frontend->_ballisticCoef_y;
const float mcoef = frontend->_momentumDragCoef.get(); const float mcoef = frontend->_momentumDragCoef.get();
const bool using_bcoef_x = bcoef.x > 1.0f; const bool using_bcoef_x = bcoef_x > 1.0f;
const bool using_bcoef_y = bcoef.y > 1.0f; const bool using_bcoef_y = bcoef_y > 1.0f;
const bool using_mcoef = mcoef > 0.001f; const bool using_mcoef = mcoef > 0.001f;
if (!using_bcoef_x && !using_bcoef_y && !using_mcoef) { if (!using_bcoef_x && !using_bcoef_y && !using_mcoef) {
// nothing to do // nothing to do