diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index d6072e1e5b..aaf25c9d7e 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -313,7 +313,6 @@ bool Ekf::update() // correct position and height for offset relative to IMU Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - warnx("pos_offset_earth %lf %lf %lf", (double)pos_offset_earth(0)*100, (double)pos_offset_earth(1)*100, (double)pos_offset_earth(2)*100); _ev_sample_delayed.posNED(0) -= pos_offset_earth(0); _ev_sample_delayed.posNED(1) -= pos_offset_earth(1); _ev_sample_delayed.posNED(2) -= pos_offset_earth(2); @@ -407,8 +406,6 @@ bool Ekf::initialiseFilter(void) } } - // warnx("_params.fusion_mode & MASK_USE_EVPOS %d", (int)(_params.fusion_mode & MASK_USE_EVPOS)); - // warnx("_primary_hgt_source == VDIST_SENSOR_EV %d", (int)(_primary_hgt_source == VDIST_SENSOR_EV)); // set the default height source from the adjustable parameter if (_hgt_counter == 0) { if (_params.fusion_mode & MASK_USE_EVPOS) { diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 607a9c901e..c71e014b7d 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -105,7 +105,6 @@ void Ekf::fuseVelPosHeight() _vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1); } else if (_control_status.flags.ev_pos) { - // warnx("Fusing EV pos"); R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f); _vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0); _vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1); @@ -154,7 +153,6 @@ void Ekf::fuseVelPosHeight() // innovation gate size gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f); } else if (_control_status.flags.ev_pos) { - // warnx("fusing ext_visn_hight"); fuse_map[5] = true; // calculate the innovation assuming the external vision observaton is in local NED frame _vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);