fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 06:10:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added robust airspeed measurement

Browse Source
This commit is contained in:
Youssef Demitri
2015-10-08 11:53:37 +02:00
parent ce70803d25
commit 9c26e71ef6
2 changed files with 7 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+3 -1
View File
@@ -870,7 +870,6 @@ void AttitudePositionEstimatorEKF::publishControlState()
// XXX need to announce change of Z reference somehow elegantly
_ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset;
//_local_pos.z_stable = _ekf->states[9];
_ctrl_state.pos_variance[0] = _ekf->P[7][7];
_ctrl_state.pos_variance[1] = _ekf->P[8][8];
@@ -883,6 +882,9 @@ void AttitudePositionEstimatorEKF::publishControlState()
_ctrl_state.q[2] = _ekf->states[2];
_ctrl_state.q[3] = _ekf->states[3];
/* Airspeed (Groundspeed - Windspeed) */
_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2));
/* Attitude Rates */
_ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+4 -2
View File
@@ -49,6 +49,8 @@
#define M_PI_F static_cast<float>(M_PI)
#endif
#define MIN_AIRSPEED_MEAS 5.0f
constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f;
AttPosEKF::AttPosEKF() :
@@ -1687,7 +1689,7 @@ void AttPosEKF::FuseAirspeed()
// Calculate the predicted airspeed
VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
// Perform fusion of True Airspeed measurement
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > MIN_AIRSPEED_MEAS))
{
// Calculate observation jacobians
SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
@@ -2595,7 +2597,7 @@ void AttPosEKF::CovarianceInit()
P[13][13] = sq(0.2f*dtIMU);
//Wind velocities
P[14][14] = 0.0f;
P[14][14] = 0.01f;
P[15][15] = P[14][14];
//Earth magnetic field