diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 11a2deb051..ca34817370 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -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() // 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 ++) { diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 52647624c1..5fd2f28303 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -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 diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 513469d169..d8c485fb9d 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -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 } diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 692817a0e7..01f6acc9a3 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -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 // 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 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); diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 17ec1e02de..c0f429fdc7 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -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 {