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 a0d777a11a..c027dbeb2a 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 @@ -948,6 +948,9 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.vy = _ekf->states[5]; _local_pos.vz = _ekf->states[6]; + // this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity + _local_pos.z_deriv = _ekf->states[6]; + _local_pos.xy_valid = _gps_initialized && _gpsIsGood; _local_pos.z_valid = true; _local_pos.v_xy_valid = _gps_initialized && _gpsIsGood; @@ -1016,11 +1019,13 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; - } else { _global_pos.vel_d = 0.0f; } + // this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity + _global_pos.pos_d_deriv = _global_pos.vel_d; + /* terrain altitude */ if (_terrain_estimator->is_valid()) { _global_pos.terrain_alt = _global_pos.alt - _terrain_estimator->get_distance_to_ground();