From cc73f214d18b92c07fba4e579c59d1705b868a19 Mon Sep 17 00:00:00 2001
From: TSC21 <n.marques21@hotmail.com>
Date: Fri, 14 Sep 2018 11:43:48 +0100
Subject: [PATCH] add covariance matrices index aliases
---
msg/vehicle_odometry.msg | 20 +++++++++++--
.../attitude_estimator_q_main.cpp | 14 +++++----
src/modules/ekf2/ekf2_main.cpp | 15 ++++++----
.../sensors/mocap.cpp | 30 +++++++++++++------
.../sensors/vision.cpp | 30 +++++++++++++------
.../position_estimator_inav_main.cpp | 24 +++++++++------
src/modules/simulator/simulator_mavlink.cpp | 8 +++--
7 files changed, 98 insertions(+), 43 deletions(-)
diff --git a/msg/vehicle_odometry.msg b/msg/vehicle_odometry.msg
index 98bac50dc4..138f34b8a2 100644
--- a/msg/vehicle_odometry.msg
+++ b/msg/vehicle_odometry.msg
@@ -1,6 +1,20 @@
# Vehicle odometry data. Fits ROS REP 147 for aerial vehicles
uint64 timestamp # time since system start (microseconds)
+# Covariance matrix index constants
+uint8 COVARIANCE_MATRIX_X_VARIANCE=0
+uint8 COVARIANCE_MATRIX_Y_VARIANCE=6
+uint8 COVARIANCE_MATRIX_Z_VARIANCE=11
+uint8 COVARIANCE_MATRIX_ROLL_VARIANCE=15
+uint8 COVARIANCE_MATRIX_PITCH_VARIANCE=18
+uint8 COVARIANCE_MATRIX_YAW_VARIANCE=20
+uint8 COVARIANCE_MATRIX_VX_VARIANCE=0
+uint8 COVARIANCE_MATRIX_VY_VARIANCE=6
+uint8 COVARIANCE_MATRIX_VZ_VARIANCE=11
+uint8 COVARIANCE_MATRIX_ROLLRATE_VARIANCE=15
+uint8 COVARIANCE_MATRIX_PITCHRATE_VARIANCE=18
+uint8 COVARIANCE_MATRIX_YAWRATE_VARIANCE=20
+
# Position in NED earth-fixed frame (meters). NaN if invalid/unknown
float32 x # North position
float32 y # East position
@@ -12,7 +26,8 @@ float32[4] q # Quaternion rotation from NED earth-fixed frame to XYZ body fram
# Row-major representation of 6x6 pose cross-covariance matrix URT.
# NED earth-fixed frame.
# Order: x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis
-# If invalid/unknown, first cell is NaN
+# If position covariance invalid/unknown, first cell is NaN
+# If orientation covariance invalid/unknown, 16th cell is NaN
float32[21] pose_covariance
# Velocity in NED earth-fixed frame (meters/sec). NaN if invalid/unknown
@@ -28,7 +43,8 @@ float32 yawspeed # Angular velocity about Z body axis
# Row-major representation of 6x6 velocity cross-covariance matrix URT.
# Linear velocity in NED earth-fixed frame. Angular velocity in body-fixed frame.
# Order: vx, vy, vz, rotation rate about X axis, rotation rate about Y axis, rotation rate about Z axis
-# If invalid/unknown, first cell is NaN
+# If linear velocity covariance invalid/unknown, first cell is NaN
+# If angular velocity covariance invalid/unknown, 16th cell is NaN
float32[21] velocity_covariance
# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index aa806698a2..2c9aa01c06 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -350,9 +350,10 @@ void AttitudeEstimatorQ::task_main()
if (orb_copy(ORB_ID(vehicle_visual_odometry), _vision_odom_sub, &vision) == PX4_OK) {
// validation check for vision attitude data
bool vision_att_valid = PX4_ISFINITE(vision.q[0])
- && (PX4_ISFINITE(vision.pose_covariance[0]) ? fabsf(sqrtf(fmaxf(vision.pose_covariance[15],
- fmaxf(vision.pose_covariance[18],
- vision.pose_covariance[20]))) - _eo_max_std_dev) < FLT_EPSILON : true);
+ && (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? fabsf(sqrtf(fmaxf(
+ vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE],
+ fmaxf(vision.pose_covariance[vision.COVARIANCE_MATRIX_PITCH_VARIANCE],
+ vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) - _eo_max_std_dev) < FLT_EPSILON : true);
if (vision_att_valid) {
Dcmf Rvis = Quatf(vision.q);
@@ -381,9 +382,10 @@ void AttitudeEstimatorQ::task_main()
if (orb_copy(ORB_ID(vehicle_mocap_odometry), _mocap_odom_sub, &mocap) == PX4_OK) {
// validation check for mocap attitude data
bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
- && (PX4_ISFINITE(mocap.pose_covariance[0]) ? fabsf(sqrtf(fmaxf(mocap.pose_covariance[15],
- fmaxf(mocap.pose_covariance[18],
- mocap.pose_covariance[20]))) - _eo_max_std_dev) < FLT_EPSILON : true);
+ && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? fabsf(sqrtf(fmaxf(
+ mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE],
+ fmaxf(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_PITCH_VARIANCE],
+ mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) - _eo_max_std_dev) < FLT_EPSILON : true);
if (mocap_att_valid) {
Dcmf Rmoc = Quatf(mocap.q);
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index a8ee9a2e36..d3c142cabf 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -1172,9 +1172,10 @@ void Ekf2::run()
ev_data.posNED(2) = ev_odom.z;
// position measurement error from parameters
- if (PX4_ISFINITE(ev_odom.pose_covariance[0])) {
- ev_data.posErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[0], ev_odom.pose_covariance[6])));
- ev_data.hgtErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[11]));
+ if (PX4_ISFINITE(ev_odom.COVARIANCE_MATRIX_X_VARIANCE)) {
+ ev_data.posErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE],
+ ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])));
+ ev_data.hgtErr = fmaxf(_ev_pos_noise.get(), sqrtf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE])));
} else {
ev_data.posErr = _ev_pos_noise.get();
ev_data.hgtErr = _ev_pos_noise.get();
@@ -1186,9 +1187,11 @@ void Ekf2::run()
ev_data.quat = matrix::Quatf(ev_odom.q);
// orientation measurement error from parameters
- if (PX4_ISFINITE(ev_odom.pose_covariance[15])) {
- ev_data.angErr = fmaxf(_ev_ang_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[15], fmaxf(ev_odom.pose_covariance[18],
- ev_odom.pose_covariance[20]))));
+ if (PX4_ISFINITE(ev_odom.COVARIANCE_MATRIX_ROLL_VARIANCE)) {
+ ev_data.angErr = fmaxf(_ev_ang_noise.get(),
+ sqrtf(fmaxf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_ROLL_VARIANCE],
+ fmaxf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_PITCH_VARIANCE],
+ ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]))));
} else {
ev_data.angErr = _ev_ang_noise.get();
diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp
index ee0c028784..2ad5983bfb 100644
--- a/src/modules/local_position_estimator/sensors/mocap.cpp
+++ b/src/modules/local_position_estimator/sensors/mocap.cpp
@@ -60,10 +60,15 @@ void BlockLocalPositionEstimator::mocapInit()
int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
{
- if (PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[0])) {
+ uint8_t x_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_X_VARIANCE;
+ uint8_t y_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_Y_VARIANCE;
+ uint8_t z_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_Z_VARIANCE;
+
+ if (PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[x_variance])) {
// check if the mocap data is valid based on the covariances
- _mocap_eph = sqrtf(fmaxf(_sub_mocap_odom.get().pose_covariance[0], _sub_mocap_odom.get().pose_covariance[6]));
- _mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[11]);
+ _mocap_eph = sqrtf(fmaxf(_sub_mocap_odom.get().pose_covariance[x_variance],
+ _sub_mocap_odom.get().pose_covariance[y_variance]));
+ _mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[z_variance]);
_mocap_xy_valid = _mocap_eph <= EP_MAX_STD_DEV;
_mocap_z_valid = _mocap_epv <= EP_MAX_STD_DEV;
@@ -78,13 +83,20 @@ int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
return -1;
} else {
- y.setZero();
- y(Y_mocap_x) = _sub_mocap_odom.get().x;
- y(Y_mocap_y) = _sub_mocap_odom.get().y;
- y(Y_mocap_z) = _sub_mocap_odom.get().z;
- _mocapStats.update(y);
_time_last_mocap = _sub_mocap_odom.get().timestamp;
- return OK;
+
+ if (PX4_ISFINITE(_sub_mocap_odom.get().x)) {
+ y.setZero();
+ y(Y_mocap_x) = _sub_mocap_odom.get().x;
+ y(Y_mocap_y) = _sub_mocap_odom.get().y;
+ y(Y_mocap_z) = _sub_mocap_odom.get().z;
+ _mocapStats.update(y);
+
+ return OK;
+
+ } else {
+ return -1;
+ }
}
}
diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp
index ad20d3d3b9..71dbd528d7 100644
--- a/src/modules/local_position_estimator/sensors/vision.cpp
+++ b/src/modules/local_position_estimator/sensors/vision.cpp
@@ -65,10 +65,15 @@ void BlockLocalPositionEstimator::visionInit()
int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
{
- if (PX4_ISFINITE(_sub_visual_odom.get().pose_covariance[0])) {
+ uint8_t x_variance = _sub_visual_odom.get().COVARIANCE_MATRIX_X_VARIANCE;
+ uint8_t y_variance = _sub_visual_odom.get().COVARIANCE_MATRIX_Y_VARIANCE;
+ uint8_t z_variance = _sub_visual_odom.get().COVARIANCE_MATRIX_Z_VARIANCE;
+
+ if (PX4_ISFINITE(_sub_visual_odom.get().pose_covariance[x_variance])) {
// check if the vision data is valid based on the covariances
- _vision_eph = sqrtf(fmaxf(_sub_visual_odom.get().pose_covariance[0], _sub_visual_odom.get().pose_covariance[6]));
- _vision_epv = sqrtf(_sub_visual_odom.get().pose_covariance[11]);
+ _vision_eph = sqrtf(fmaxf(_sub_visual_odom.get().pose_covariance[x_variance],
+ _sub_visual_odom.get().pose_covariance[y_variance]));
+ _vision_epv = sqrtf(_sub_visual_odom.get().pose_covariance[z_variance]);
_vision_xy_valid = _vision_eph <= EP_MAX_STD_DEV;
_vision_z_valid = _vision_epv <= EP_MAX_STD_DEV;
@@ -83,13 +88,20 @@ int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
return -1;
} else {
- y.setZero();
- y(Y_vision_x) = _sub_visual_odom.get().x;
- y(Y_vision_y) = _sub_visual_odom.get().y;
- y(Y_vision_z) = _sub_visual_odom.get().z;
- _visionStats.update(y);
_time_last_vision_p = _sub_visual_odom.get().timestamp;
- return OK;
+
+ if (PX4_ISFINITE(_sub_visual_odom.get().x)) {
+ y.setZero();
+ y(Y_vision_x) = _sub_visual_odom.get().x;
+ y(Y_vision_y) = _sub_visual_odom.get().y;
+ y(Y_vision_z) = _sub_visual_odom.get().z;
+ _visionStats.update(y);
+
+ return OK;
+
+ } else {
+ return -1;
+ }
}
}
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
index 790a72c7ca..17b5229402 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
@@ -792,13 +792,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
static float last_vision_y = 0.0f;
static float last_vision_z = 0.0f;
- vision_xy_valid = PX4_ISFINITE(visual_odom.pose_covariance[0]) ? sqrtf(fmaxf(visual_odom.pose_covariance[0],
- visual_odom.pose_covariance[6])) > ep_max_std_dev : true;
- vision_z_valid = PX4_ISFINITE(visual_odom.pose_covariance[0]) ? visual_odom.pose_covariance[11] > ep_max_std_dev :
+ vision_xy_valid = PX4_ISFINITE(visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE]) ? sqrtf(fmaxf(
+ visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE],
+ visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_Y_VARIANCE])) > ep_max_std_dev : true;
+ vision_z_valid = PX4_ISFINITE(visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE]) ?
+ visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_Z_VARIANCE] > ep_max_std_dev :
true;
- vision_vxy_valid = PX4_ISFINITE(visual_odom.velocity_covariance[0]) ? sqrtf(fmaxf(visual_odom.velocity_covariance[0],
- visual_odom.velocity_covariance[6])) > ev_max_std_dev : true;
- vision_vz_valid = PX4_ISFINITE(visual_odom.velocity_covariance[0]) ? visual_odom.velocity_covariance[11] >
+ vision_vxy_valid = PX4_ISFINITE(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE]) ? sqrtf(
+ fmaxf(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE],
+ visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VY_VARIANCE])) > ev_max_std_dev : true;
+ vision_vz_valid = PX4_ISFINITE(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE]) ?
+ visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VZ_VARIANCE] >
ep_max_std_dev : true;
/* reset position estimate on first vision update */
@@ -913,9 +917,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(vehicle_mocap_odometry), mocap_position_sub, &mocap);
- mocap_xy_valid = (PX4_ISFINITE(mocap.pose_covariance[0]) ? sqrtf(fmaxf(mocap.pose_covariance[0],
- mocap.pose_covariance[6])) > ep_max_std_dev : true) ? false : true;
- mocap_z_valid = (PX4_ISFINITE(mocap.pose_covariance[0]) ? mocap.pose_covariance[11] > ep_max_std_dev : true) ? false :
+ mocap_xy_valid = (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE]) ? sqrtf(fmaxf(
+ mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE],
+ mocap.pose_covariance[mocap.COVARIANCE_MATRIX_Y_VARIANCE])) > ep_max_std_dev : true) ? false : true;
+ mocap_z_valid = (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE]) ?
+ mocap.pose_covariance[mocap.COVARIANCE_MATRIX_Z_VARIANCE] > ep_max_std_dev : true) ? false :
true;
if (!params.disable_mocap) {
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 4b195135f7..dc52ca3dc8 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -1131,6 +1131,8 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
odom.timestamp = timestamp;
+ const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
+
if (odom_mavlink->msgid == MAVLINK_MSG_ID_ODOMETRY) {
mavlink_odometry_t odom_msg;
mavlink_msg_odometry_decode(odom_mavlink, &odom_msg);
@@ -1155,7 +1157,6 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
matrix::Quatf q(odom_msg.q[0], odom_msg.q[1], odom_msg.q[2], odom_msg.q[3]);
q.copyTo(odom.q);
- const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
static_assert(POS_URT_SIZE == (sizeof(odom_msg.pose_covariance) / sizeof(odom_msg.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
@@ -1194,8 +1195,11 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw));
q.copyTo(odom.q);
+ static_assert(POS_URT_SIZE == (sizeof(ev.covariance) / sizeof(ev.covariance[0])),
+ "Vision Position Estimate Pose Covariance matrix URT array size mismatch");
+
/* The pose covariance URT */
- for (size_t i = 0; i < (sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0])); i++) {
+ for (size_t i = 0; i < POS_URT_SIZE; i++) {
odom.pose_covariance[i] = ev.covariance[i];
}
--
GitLab