diff --git a/EKF/common.h b/EKF/common.h index 88ee0ed28b..ad55d51f38 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -67,7 +67,7 @@ typedef matrix::Matrix Matrix3f; struct flow_message { uint8_t quality; // Quality of Flow data Vector2f flowdata; // Flow data received - Vector2f gyrodata; // Gyro data from flow sensor + Vector3f gyrodata; // Gyro data from flow sensor uint32_t dt; // integration time of flow samples }; @@ -120,7 +120,7 @@ struct flowSample { uint8_t quality; // quality indicator between 0 and 255 Vector2f flowRadXY; // measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive Vector2f flowRadXYcomp; // measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive - Vector2f gyroXY; // measured delta angle of the inertial frame about the X and Y body axes obtained from rate gyro measurements (rad), RH rotation is positive + Vector3f gyroXYZ; // measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive float dt; // amount of integration time (sec) uint64_t time_us; // timestamp in microseconds of the integration period mid-point }; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 5378009c6e..37e9a017b5 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -266,7 +266,7 @@ bool Ekf::update() // correct velocity for offset relative to IMU Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt); Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; - Vector3f vel_offset_body = ang_rate % pos_offset_body; + Vector3f vel_offset_body = cross_product(ang_rate,pos_offset_body); Vector3f vel_offset_earth = _R_prev.transpose() * vel_offset_body; _gps_sample_delayed.vel -= vel_offset_earth; diff --git a/EKF/ekf.h b/EKF/ekf.h index b98f9769d3..061f2c2fd0 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -173,8 +173,8 @@ private: // optical flow processing float _flow_innov[2]; // flow measurement innovation float _flow_innov_var[2]; // flow innovation variance - Vector2f _flow_gyro_bias; // bias errors in optical flow sensor rate gyro outputs - Vector2f _imu_del_ang_of; // bias corrected XY delta angle measurements accumulated across the same time frame as the optical flow rates + Vector3f _flow_gyro_bias; // bias errors in optical flow sensor rate gyro outputs + Vector3f _imu_del_ang_of; // bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates float _delta_time_of; // time in sec that _imu_del_ang_of was accumulated over float _mag_declination; // magnetic declination used by reset and fusion functions (rad) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index bd4171d97b..876b3030e9 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -271,10 +271,11 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl // 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. // copy the optical and gyro measured delta angles optflow_sample_new.flowRadXY = - flow->flowdata; - optflow_sample_new.gyroXY = - flow->gyrodata; + optflow_sample_new.gyroXYZ = - flow->gyrodata; // compensate for body motion to give a LOS rate - optflow_sample_new.flowRadXYcomp = optflow_sample_new.flowRadXY - optflow_sample_new.gyroXY; - // convert integraton interval to seconds + optflow_sample_new.flowRadXYcomp(0) = optflow_sample_new.flowRadXY(0) - optflow_sample_new.gyroXYZ(0); + optflow_sample_new.flowRadXYcomp(1) = optflow_sample_new.flowRadXY(1) - optflow_sample_new.gyroXYZ(1); + // convert integration interval to seconds optflow_sample_new.dt = 1e-6f * (float)flow->dt; _time_last_optflow = time_usec; // push to buffer diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 4d14fa7fbc..ce62339365 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -87,8 +87,17 @@ void Ekf::fuseOptFlow() matrix::Dcm earth_to_body(_state.quat_nominal); earth_to_body = earth_to_body.transpose(); - // rotate earth velocities into body frame - Vector3f vel_body = earth_to_body * _state.vel; + // calculate the sensor position relative to the IMU + Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; + + // calculate the velocity of the sensor reelative to the imu in body frame + Vector3f vel_rel_imu_body = cross_product(_flow_sample_delayed.gyroXYZ , pos_offset_body); + + // calculate the velocity of the sensor in the earth frame + Vector3f vel_rel_earth = _state.vel + _R_prev.transpose() * vel_rel_imu_body; + + // rotate into body frame + Vector3f vel_body = earth_to_body * vel_rel_earth; // calculate range from focal point to centre of image float range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view @@ -498,8 +507,7 @@ void Ekf::get_flow_innov_var(float flow_innov_var[2]) void Ekf::calcOptFlowBias() { // accumulate the bias corrected delta angles from the navigation sensor and lapsed time - _imu_del_ang_of(0) += _imu_sample_delayed.delta_ang(0); - _imu_del_ang_of(1) += _imu_sample_delayed.delta_ang(1); + _imu_del_ang_of += _imu_sample_delayed.delta_ang; _delta_time_of += _imu_sample_delayed.delta_ang_dt; // reset the accumulators if the time interval is too large @@ -514,21 +522,20 @@ void Ekf::calcOptFlowBias() if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.05f) && (_delta_time_of > 0.01f) && (_flow_sample_delayed.dt > 0.01f)) { // calculate a reference angular rate - Vector2f reference_body_rate; - reference_body_rate(0) = _imu_del_ang_of(0) / _delta_time_of; - reference_body_rate(1) = _imu_del_ang_of(1) / _delta_time_of; + Vector3f reference_body_rate; + reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of); // calculate the optical flow sensor measured body rate - Vector2f of_body_rate; - of_body_rate(0) = _flow_sample_delayed.gyroXY(0) / _flow_sample_delayed.dt; - of_body_rate(1) = _flow_sample_delayed.gyroXY(1) / _flow_sample_delayed.dt; + Vector3f of_body_rate; + of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt); // calculate the bias estimate using a combined LPF and spike filter _flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)), -0.1f, 0.1f); _flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)), -0.1f, 0.1f); - + _flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)), + -0.1f, 0.1f); } // reset the accumulators