mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-26 05:13:34 +08:00
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:
committed by
Mathieu Bresciani
parent
329a2d0e98
commit
0aa4afdbce
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user