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

msg/vehicle_odometry.msg: simplify covariance handling and update all usage (#19966)

Browse Source 
- replace float32[21] URT covariances with smaller dedicated position/velocity/orientation variances (the crossterms are unused, awkward, and relatively costly)
 - these are easier to casually inspect and more representative of what's actually being used currently and reduces the size of vehicle_odometry_s quite a bit
 - ekf2: add new helper to get roll/pitch/yaw covariances
 - mavlink: receiver ODOMETRY handle more frame types for both pose (MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FRD, MAV_FRAME_LOCAL_FLU) and velocity (MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FRD, MAV_FRAME_LOCAL_FLU, MAV_FRAME_BODY_FRD)
 - mavlink: delete unused ATT_POS_MOCAP stream (this is just a passthrough)

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
2022-08-04 12:55:21 -04:00
committed by GitHub
parent 61f390b0dd
commit dfdfbbfa9c
21 changed files with 838 additions and 646 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/sensor_simulator/vio.cpp
+2 -7
View File
@@ -26,12 +26,7 @@ void Vio::setData(const extVisionSample &vio_data)
void Vio::setVelocityVariance(const Vector3f &velVar)
{
setVelocityCovariance(matrix::diag(velVar));
}
void Vio::setVelocityCovariance(const Matrix3f &velCov)
{
_vio_data.velCov = velCov;
_vio_data.velVar = velVar;
}
void Vio::setPositionVariance(const Vector3f &posVar)
@@ -76,7 +71,7 @@ extVisionSample Vio::dataAtRest()
vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};;
vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f};
vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
vio_data.velCov = matrix::eye<float, 3>() * 0.1f;
vio_data.velVar = Vector3f{0.1f, 0.1f, 0.1f};
vio_data.angVar = 0.05f;
vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
return vio_data;
src/modules/ekf2/test/sensor_simulator/vio.h
-1
View File
@@ -53,7 +53,6 @@ public:
void setData(const extVisionSample &vio_data);
void setVelocityVariance(const Vector3f &velVar);
void setVelocityCovariance(const Matrix3f &velCov);
void setPositionVariance(const Vector3f &posVar);
void setAngularVariance(float angVar);
void setVelocity(const Vector3f &vel);
src/modules/ekf2/test/test_EKF_externalVision.cpp
+6 -12
View File
@@ -276,13 +276,10 @@ TEST_F(EkfExternalVisionTest, velocityFrameBody)
// WHEN: measurement is given in BODY-FRAME and
// x variance is bigger than y variance
_sensor_simulator._vio.setVelocityFrameToBody();
float vel_cov_data [9] = {2.0f, 0.0f, 0.0f,
0.0f, 0.01f, 0.0f,
0.0f, 0.0f, 0.01f
};
const Matrix3f vel_cov_body(vel_cov_data);
const Vector3f vel_cov_body(2.0f, 0.01f, 0.01f);
const Vector3f vel_body(1.0f, 0.0f, 0.0f);
_sensor_simulator._vio.setVelocityCovariance(vel_cov_body);
_sensor_simulator._vio.setVelocityVariance(vel_cov_body);
_sensor_simulator._vio.setVelocity(vel_body);
_ekf_wrapper.enableExternalVisionVelocityFusion();
_sensor_simulator.startExternalVision();
@@ -312,13 +309,10 @@ TEST_F(EkfExternalVisionTest, velocityFrameLocal)
// WHEN: measurement is given in LOCAL-FRAME and
// x variance is bigger than y variance
_sensor_simulator._vio.setVelocityFrameToLocal();
float vel_cov_data [9] = {2.0f, 0.0f, 0.0f,
0.0f, 0.01f, 0.0f,
0.0f, 0.0f, 0.01f
};
const Matrix3f vel_cov_earth(vel_cov_data);
const Vector3f vel_cov_earth{2.f, 0.01f, 0.01f};
const Vector3f vel_earth(1.0f, 0.0f, 0.0f);
_sensor_simulator._vio.setVelocityCovariance(vel_cov_earth);
_sensor_simulator._vio.setVelocityVariance(vel_cov_earth);
_sensor_simulator._vio.setVelocity(vel_earth);
_ekf_wrapper.enableExternalVisionVelocityFusion();
_sensor_simulator.startExternalVision();