diff --git a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 7cff56de42..115db064e6 100644 --- a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -909,6 +909,11 @@ void AttitudePositionEstimatorEKF::publishControlState() _ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; _ctrl_state.yaw_rate = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt; + /* Gyro bias estimates */ + _ctrl_state.roll_rate_bias = _ekf->states[10] / _ekf->dtIMUfilt; + _ctrl_state.pitch_rate_bias = _ekf->states[11] / _ekf->dtIMUfilt; + _ctrl_state.yaw_rate_bias = _ekf->states[12] / _ekf->dtIMUfilt; + /* Guard from bad data */ if (!PX4_ISFINITE(_ctrl_state.x_vel) || !PX4_ISFINITE(_ctrl_state.y_vel) ||