fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 15:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: split horizontal and vertical origin reset

Browse Source 
Allow partial resets (only lat/lon or only altitude)
This commit is contained in:
bresch
2024-08-28 11:02:26 +02:00
committed by Mathieu Bresciani
parent f3d58cdf10
commit 9169a7c5fc
2 changed files with 89 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+5
View File
@@ -261,6 +261,8 @@ public:
// get the ekf WGS-84 origin position and height and the system time it was last set
// return true if the origin is valid
bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
bool checkLatLonValidity(double latitude, double longitude, float eph = 0.f);
bool checkAltitudeValidity(float altitude, float epv = 0.f);
bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f);
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
@@ -765,6 +767,9 @@ private:
P.slice<S.dof, S.dof>(S.idx, S.idx) = cov;
}
bool setLatLonOrigin(double latitude, double longitude, float eph = NAN);
bool setAltOrigin(float altitude, float epv = NAN);
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const;