fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +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 16:57:35 +00:00 committed by Daniel Agar
parent a85d3a43ed
commit 4e0e68e905
3 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
EKF/ekf.h
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);

8
EKF/ekf_helper.cpp
View File

@ -982,14 +982,6 @@ void Ekf::get_gyro_bias(float bias[3])
memcpy(bias, temp, 3 * sizeof(float));
}
// get the diagonal elements of the covariance matrix
void Ekf::get_covariances(float *covariances)
{
for (unsigned i = 0; i < _k_num_states; i++) {
covariances[i] = P[i][i];
}
}
// get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set
// return true if the origin is valid
bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt)

2
EKF/estimator_interface.h
View File

@ -104,8 +104,6 @@ public:
virtual void get_true_airspeed(float *tas) = 0;
virtual void get_covariances(float *covariances) = 0;
// gets the variances for the NED velocity states
virtual void get_vel_var(Vector3f &vel_var) = 0;