Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

add covariance matrices index aliases

Browse Source
This commit is contained in:
TSC21
2018-09-14 11:43:48 +01:00
committed by Lorenz Meier
parent 1cfaccfd7b
commit cc73f214d1
7 changed files with 98 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/vehicle_odometry.msg
+18 -2
View File
@@ -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
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+8 -6
View File
@@ -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);
src/modules/ekf2/ekf2_main.cpp
+9 -6
View File
@@ -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();
src/modules/local_position_estimator/sensors/mocap.cpp
+21 -9
View File
@@ -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;
}
}
}
src/modules/local_position_estimator/sensors/vision.cpp
+21 -9
View File
@@ -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;
}
}
}
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+15 -9
View File
@@ -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) {
src/modules/simulator/simulator_mavlink.cpp
+6 -2
View File
@@ -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];
}