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

Ekf: add more useful methods to interface with the covariances (#543)

Browse Source
This commit is contained in:
Nuno Marques
2019-03-09 11:57:35 -05:00
committed by Daniel Agar
parent a85d3a43ed
commit 4e0e68e905
3 changed files with 14 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.h
+14 -2
View File
@@ -49,7 +49,7 @@ class Ekf : public EstimatorInterface
public:
Ekf() = default;
~Ekf() = default;
virtual ~Ekf() = default;
// initialise variables to sane values (also interface class)
bool init(uint64_t timestamp);
@@ -122,8 +122,20 @@ public:
// get the true airspeed in m/s
void get_true_airspeed(float *tas);
// get the full covariance matrix
matrix::SquareMatrix<float, 24> covariances() const { return matrix::SquareMatrix<float, _k_num_states>(P); }
// get the diagonal elements of the covariance matrix
void get_covariances(float *covariances);
matrix::Vector<float, 24> covariances_diagonal() const { return covariances().diag(); }
// get the orientation (quaterion) covariances
matrix::SquareMatrix<float, 4> orientation_covariances() const { return covariances().slice<4, 4>(0, 0); }
// get the linear velocity covariances
matrix::SquareMatrix<float, 3> velocity_covariances() const { return covariances().slice<3, 3>(4, 4); }
// get the position covariances
matrix::SquareMatrix<float, 3> position_covariances() const { return covariances().slice<3, 3>(7, 7); }
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
bool collect_gps(const gps_message &gps);