From 0aa4afdbce47dd99c629610e7831af5ca5c05de2 Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Fri, 18 Aug 2023 16:31:43 +0200 Subject: [PATCH] ekf2: add unaided_yaw for more resilient yaw control This estimate doesn't converge to the true yaw but can be used as a more consistent but drifting heading source. It can be used by a setpoint generator to adjust its heading setpoint while the true yaw estimate is converging in order to keep a constant course over ground. --- msg/VehicleLocalPosition.msg | 1 + src/modules/ekf2/EKF/estimator_interface.h | 1 + src/modules/ekf2/EKF/output_predictor.cpp | 5 +++++ src/modules/ekf2/EKF/output_predictor.h | 5 +++++ src/modules/ekf2/EKF2.cpp | 1 + 5 files changed, 13 insertions(+) diff --git a/msg/VehicleLocalPosition.msg b/msg/VehicleLocalPosition.msg index 4f9238123c..5f17cde407 100644 --- a/msg/VehicleLocalPosition.msg +++ b/msg/VehicleLocalPosition.msg @@ -39,6 +39,7 @@ float32 ay # East velocity derivative in NED earth-fixed frame, (metres/s float32 az # Down velocity derivative in NED earth-fixed frame, (metres/sec^2) float32 heading # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians) +float32 unaided_heading # Same as heading but generated by integrating corrected gyro data only float32 delta_heading uint8 heading_reset_counter bool heading_good_for_control diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index e4b386f141..d784e442e0 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -224,6 +224,7 @@ public: int getNumberOfActiveVerticalVelocityAidingSources() const; const matrix::Quatf &getQuaternion() const { return _output_predictor.getQuaternion(); } + float getUnaidedYaw() const { return _output_predictor.getUnaidedYaw(); } Vector3f getVelocity() const { return _output_predictor.getVelocity(); } const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); } float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); } diff --git a/src/modules/ekf2/EKF/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor.cpp index 123ec83fa9..32bf4fe8ad 100644 --- a/src/modules/ekf2/EKF/output_predictor.cpp +++ b/src/modules/ekf2/EKF/output_predictor.cpp @@ -237,6 +237,11 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector // rotate the relative velocity into earth frame _vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body; } + + // update auxiliary yaw estimate + const Vector3f unbiased_delta_angle = delta_angle - delta_angle_bias_scaled; + const float spin_del_ang_D = unbiased_delta_angle.dot(Vector3f(_R_to_earth_now.row(2))); + _unaided_yaw = matrix::wrap_pi(_unaided_yaw + spin_del_ang_D); } void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, diff --git a/src/modules/ekf2/EKF/output_predictor.h b/src/modules/ekf2/EKF/output_predictor.h index 4acded9c34..ee5cc0f710 100644 --- a/src/modules/ekf2/EKF/output_predictor.h +++ b/src/modules/ekf2/EKF/output_predictor.h @@ -94,6 +94,9 @@ public: const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; } + // get a yaw value solely based on bias-removed gyro integration + float getUnaidedYaw() const { return _unaided_yaw; } + // get the velocity of the body frame origin in local NED earth frame matrix::Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; } @@ -184,6 +187,8 @@ private: matrix::Vector3f _imu_pos_body{}; ///< xyz position of IMU in body frame (m) + float _unaided_yaw{}; + // output complementary filter tuning float _vel_tau{0.25f}; ///< velocity state correction time constant (1/sec) float _pos_tau{0.25f}; ///< position state correction time constant (1/sec) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index ef33773998..d48a46a58d 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1490,6 +1490,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) _ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter); lpos.heading = Eulerf(_ekf.getQuaternion()).psi(); + lpos.unaided_heading = _ekf.getUnaidedYaw(); lpos.delta_heading = Eulerf(delta_q_reset).psi(); lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();