diff --git a/EKF/ekf.h b/EKF/ekf.h index 5c64e5bd28..c345e2f24b 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -105,6 +105,12 @@ public: void getFlowInnovRatio(float &flow_innov_ratio) const override; + Vector2f getFlowVelBody() const override; + Vector2f getFlowVelNE() const override; + Vector2f getFlowCompensated() const override; + Vector2f getFlowUncompensated() const override; + Vector3f getFlowGyro() const override; + void getHeadingInnov(float &heading_innov) const override; void getHeadingInnovVar(float &heading_innov_var) const override; @@ -437,6 +443,8 @@ private: Vector2f _flow_innov; ///< flow measurement innovation (rad/sec) Vector2f _flow_innov_var; ///< flow innovation variance ((rad/sec)**2) Vector3f _flow_gyro_bias; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) + Vector2f _flow_vel_body; ///< velocity from corrected flow measurement (body frame)(m/s) + Vector2f _flow_vel_ne; ///< velocity from corrected flow measurement (local frame) (m/s) Vector3f _imu_del_ang_of; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad) float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec) uint64_t _time_bad_motion_us{0}; ///< last system time that on-ground motion exceeded limits (uSec) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index f75a2627d2..6547dc8bce 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -708,6 +708,31 @@ void Ekf::getFlowInnovRatio(float &flow_innov_ratio) const flow_innov_ratio = _optflow_test_ratio; } +Vector2f Ekf::getFlowVelBody() const +{ + return _flow_vel_body; +} + +Vector2f Ekf::getFlowVelNE() const +{ + return _flow_vel_ne; +} + +Vector2f Ekf::getFlowCompensated() const +{ + return _flow_compensated_XY_rad; +} + +Vector2f Ekf::getFlowUncompensated() const +{ + return _flow_sample_delayed.flow_xy_rad; +} + +Vector3f Ekf::getFlowGyro() const +{ + return _flow_sample_delayed.gyro_xyz; +} + void Ekf::getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index daa6dfb649..26fb670db9 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -91,6 +91,11 @@ public: virtual void getFlowInnov(float flow_innov[2]) const = 0; virtual void getFlowInnovVar(float flow_innov_var[2]) const = 0; virtual void getFlowInnovRatio(float &flow_innov_ratio) const = 0; + virtual Vector2f getFlowVelBody() const = 0; + virtual Vector2f getFlowVelNE() const = 0; + virtual Vector2f getFlowCompensated() const = 0; + virtual Vector2f getFlowUncompensated() const = 0; + virtual Vector3f getFlowGyro() const = 0; virtual void getHeadingInnov(float &heading_innov) const = 0; virtual void getHeadingInnovVar(float &heading_innov_var) const = 0; diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index dcbe90a759..b09f0950c2 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -102,6 +102,12 @@ void Ekf::fuseOptFlow() // Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis. const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias); + // compute the velocities in body and local frames from corrected optical flow measurement + // for logging only + _flow_vel_body(0) = -opt_flow_rate(1) * range; + _flow_vel_body(1) = opt_flow_rate(0) * range; + _flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f)); + if (opt_flow_rate.norm() < _flow_max_rate) { _flow_innov(0) = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis _flow_innov(1) = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis