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.
This commit is contained in:
bresch
2023-08-30 09:56:19 +02:00
committed by Mathieu Bresciani
parent 329a2d0e98
commit 0aa4afdbce
5 changed files with 13 additions and 0 deletions
+5
View File
@@ -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)