Browse Source

Remove unnecessary comments

master
kamilritz 5 years ago committed by Mathieu Bresciani
parent
commit
3daf25763e
  1. 3
      EKF/covariance.cpp
  2. 1
      EKF/ekf.cpp
  3. 1
      EKF/ekf_helper.cpp
  4. 4
      EKF/estimator_interface.cpp
  5. 1
      EKF/gps_yaw_fusion.cpp

3
EKF/covariance.cpp

@ -59,7 +59,6 @@ void Ekf::initialiseCovariance() @@ -59,7 +59,6 @@ void Ekf::initialiseCovariance()
Vector3f rot_vec_var;
rot_vec_var.setAll(sq(_params.initial_tilt_err));
// update the quaternion state covariances
initialiseQuatCovariances(rot_vec_var);
// velocity
@ -96,8 +95,6 @@ void Ekf::initialiseCovariance() @@ -96,8 +95,6 @@ void Ekf::initialiseCovariance()
// record IMU bias state covariance reset time - used to prevent resets being performed too often
_last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us;
// variances for optional states
// earth frame and body frame magnetic field
// set to observation variance
for (uint8_t index = 16; index <= 21; index ++) {

1
EKF/ekf.cpp

@ -266,7 +266,6 @@ void Ekf::predictState() @@ -266,7 +266,6 @@ void Ekf::predictState()
// save the previous value of velocity so we can use trapzoidal integration
const Vector3f vel_last = _state.vel;
// update transformation matrix from body to world frame
_R_to_earth = Dcmf(_state.quat_nominal);
// Calculate an earth frame delta velocity

1
EKF/ekf_helper.cpp

@ -1684,7 +1684,6 @@ void Ekf::resetExtVisRotMat() @@ -1684,7 +1684,6 @@ void Ekf::resetExtVisRotMat()
_ev_rot_vec_filt.zero();
}
// reset the rotation matrix
_ev_rot_mat = Dcmf(q_error); // rotation from EV reference to EKF reference
}

4
EKF/estimator_interface.cpp

@ -397,10 +397,8 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl @@ -397,10 +397,8 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
// calculate the system time-stamp for the trailing edge of the flow data integration period
optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000;
// copy the quality metric returned by the PX4Flow sensor
optflow_sample_new.quality = flow->quality;
// copy the optical and gyro measured delta angles
// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
// is produced by a RH rotation of the image about the sensor axis.
optflow_sample_new.gyroXYZ = - flow->gyrodata;
@ -438,7 +436,6 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message @@ -438,7 +436,6 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
// calculate the system time-stamp for the mid point of the integration period
ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000;
// copy required data
ev_sample_new.angErr = evdata->angErr;
ev_sample_new.posErr = evdata->posErr;
ev_sample_new.velErr = evdata->velErr;
@ -447,7 +444,6 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message @@ -447,7 +444,6 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
ev_sample_new.pos = evdata->pos;
ev_sample_new.vel = evdata->vel;
// record time for comparison next measurement
_time_last_ext_vision = time_usec;
_ext_vision_buffer.push(ev_sample_new);

1
EKF/gps_yaw_fusion.cpp

@ -325,7 +325,6 @@ bool Ekf::resetGpsAntYaw() @@ -325,7 +325,6 @@ bool Ekf::resetGpsAntYaw()
euler_init(2) += yaw_delta;
euler_init(2) = wrap_pi(euler_init(2));
// update the quaternions
quat_after_reset = Quatf(euler_init);
} else {

Loading…
Cancel
Save