yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

Support odometry velocity in body and local frame (#14703)

Browse Source 
* Update submodule ECL

* increase lower bound on EVV param
This commit is contained in:
kritz
2020-05-13 12:43:02 +02:00
committed by GitHub
parent 8804dae480
commit 3897030c6f
8 changed files with 86 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
msg/vehicle_odometry.msg
View File

@@ -21,6 +21,7 @@ uint8 COVARIANCE_MATRIX_YAWRATE_VARIANCE=20
uint8 LOCAL_FRAME_NED=0 # NED earth-fixed frame
uint8 LOCAL_FRAME_FRD=1 # FRD earth-fixed frame, arbitrary heading reference
uint8 LOCAL_FRAME_OTHER=2 # Not aligned with the std frames of reference
uint8 BODY_FRAME_FRD=3 # FRD body-fixed frame
# Position and linear velocity local frame of reference
uint8 local_frame
@@ -41,7 +42,10 @@ float32[4] q_offset # Quaternion rotation from odometry reference frame to navi
# If orientation covariance invalid/unknown, 16th cell is NaN
float32[21] pose_covariance
# Velocity in meters/sec. Frame of reference defined by local_frame. NaN if invalid/unknown
# Reference frame of the velocity data
uint8 velocity_frame
# Velocity in meters/sec. Frame of reference defined by velocity_frame variable. NaN if invalid/unknown
float32 vx # North velocity
float32 vy # East velocity
float32 vz # Down velocity

2
src/lib/ecl

Submodule src/lib/ecl updated: 03191847f9...24f2e60b7e

46
src/modules/ekf2/ekf2_main.cpp
View File

@@ -1117,18 +1117,28 @@ void Ekf2::Run()
ev_data.vel(1) = _ev_odom.vy;
ev_data.vel(2) = _ev_odom.vz;
if (_ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) {
ev_data.vel_frame = estimator::BODY_FRAME_FRD;
} else {
ev_data.vel_frame = estimator::LOCAL_FRAME_FRD;
}
// velocity measurement error from ev_data or parameters
float param_evv_noise_var = sq(_param_ekf2_evv_noise.get());
if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(_ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE])
&& PX4_ISFINITE(_ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VY_VARIANCE])
&& PX4_ISFINITE(_ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE])) {
ev_data.velVar(0) = fmaxf(param_evv_noise_var, _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE]);
ev_data.velVar(1) = fmaxf(param_evv_noise_var, _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VY_VARIANCE]);
ev_data.velVar(2) = fmaxf(param_evv_noise_var, _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE]);
ev_data.velCov(0, 0) = _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE];
ev_data.velCov(0, 1) = ev_data.velCov(1, 0) = _ev_odom.velocity_covariance[1];
ev_data.velCov(0, 2) = ev_data.velCov(2, 0) = _ev_odom.velocity_covariance[2];
ev_data.velCov(1, 1) = _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VY_VARIANCE];
ev_data.velCov(1, 2) = ev_data.velCov(2, 1) = _ev_odom.velocity_covariance[7];
ev_data.velCov(2, 2) = _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE];
} else {
ev_data.velVar.setAll(param_evv_noise_var);
ev_data.velCov = matrix::eye<float, 3>() * param_evv_noise_var;
}
}
@@ -1234,7 +1244,7 @@ void Ekf2::Run()
odom.timestamp = hrt_absolute_time();
odom.timestamp_sample = now;
odom.local_frame = odom.LOCAL_FRAME_NED;
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
// Position of body origin in local NED frame
Vector3f position = _ekf.getPosition();
@@ -1256,6 +1266,7 @@ void Ekf2::Run()
lpos.vz = velocity(2);
// Vehicle odometry linear velocity
odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
odom.vx = lpos.vx;
odom.vy = lpos.vy;
odom.vz = lpos.vz;
@@ -1417,10 +1428,27 @@ void Ekf2::Run()
aligned_ev_odom.y = aligned_pos(1);
aligned_ev_odom.z = aligned_pos(2);
const Vector3f aligned_vel = ev_rot_mat * Vector3f(_ev_odom.vx, _ev_odom.vy, _ev_odom.vz);
aligned_ev_odom.vx = aligned_vel(0);
aligned_ev_odom.vy = aligned_vel(1);
aligned_ev_odom.vz = aligned_vel(2);
switch (_ev_odom.velocity_frame) {
case vehicle_odometry_s::BODY_FRAME_FRD: {
const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) *
Vector3f(_ev_odom.vx, _ev_odom.vy, _ev_odom.vz);
aligned_ev_odom.vx = aligned_vel(0);
aligned_ev_odom.vy = aligned_vel(1);
aligned_ev_odom.vz = aligned_vel(2);
break;
}
case vehicle_odometry_s::LOCAL_FRAME_FRD: {
const Vector3f aligned_vel = ev_rot_mat *
Vector3f(_ev_odom.vx, _ev_odom.vy, _ev_odom.vz);
aligned_ev_odom.vx = aligned_vel(0);
aligned_ev_odom.vy = aligned_vel(1);
aligned_ev_odom.vz = aligned_vel(2);
break;
}
}
aligned_ev_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
// Compute orientation in EKF navigation frame
Quatf ev_quat_aligned = quat_ev2ekf * matrix::Quatf(_ev_odom.q) ;

2
src/modules/ekf2/ekf2_params.c
View File

@@ -750,7 +750,7 @@ PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f);
* Measurement noise for vision angle observations used to lower bound or replace the uncertainty included in the message
*
* @group EKF2
* @min 0.01
* @min 0.05
* @unit rad
* @decimal 2
*/

3
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@@ -638,7 +638,7 @@ void BlockLocalPositionEstimator::publishOdom()
&& PX4_ISFINITE(_x(X_vz))) {
_pub_odom.get().timestamp = hrt_absolute_time();
_pub_odom.get().timestamp_sample = _timeStamp;
_pub_odom.get().local_frame = _pub_odom.get().LOCAL_FRAME_NED;
_pub_odom.get().local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
// position
_pub_odom.get().x = xLP(X_x); // north
@@ -656,6 +656,7 @@ void BlockLocalPositionEstimator::publishOdom()
q.copyTo(_pub_odom.get().q);
// linear velocity
_pub_odom.get().velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
_pub_odom.get().vx = xLP(X_vx); // vel north
_pub_odom.get().vy = xLP(X_vy); // vel east
_pub_odom.get().vz = xLP(X_vz); // vel down

27
src/modules/mavlink/mavlink_messages.cpp
View File

@@ -2572,16 +2572,27 @@ protected:
msg.q[2] = odom.q[2];
msg.q[3] = odom.q[3];
// Body-FRD frame to local NED frame Dcm matrix
matrix::Dcmf R_body_to_local(matrix::Quatf(odom.q));
switch (odom.velocity_frame) {
case vehicle_odometry_s::BODY_FRAME_FRD:
msg.vx = odom.vx;
msg.vy = odom.vy;
msg.vz = odom.vz;
break;
// Rotate linear and angular velocity from local NED to body-NED frame
matrix::Vector3f linvel_body(R_body_to_local.transpose() * matrix::Vector3f(odom.vx, odom.vy, odom.vz));
case vehicle_odometry_s::LOCAL_FRAME_FRD:
case vehicle_odometry_s::LOCAL_FRAME_NED:
// Body frame to local frame
const matrix::Dcmf R_body_to_local(matrix::Quatf(odom.q));
// Current linear velocity
msg.vx = linvel_body(0);
msg.vy = linvel_body(1);
msg.vz = linvel_body(2);
// Rotate linear velocity from local to body frame
const matrix::Vector3f linvel_body(R_body_to_local.transpose() *
matrix::Vector3f(odom.vx, odom.vy, odom.vz));
msg.vx = linvel_body(0);
msg.vy = linvel_body(1);
msg.vz = linvel_body(2);
break;
}
// Current body rates
msg.rollspeed = odom.rollspeed;

36
src/modules/mavlink/mavlink_receiver.cpp
View File

@@ -792,6 +792,7 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
mocap_odom.pose_covariance[i] = mocap.covariance[i];
}
mocap_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
mocap_odom.vx = NAN;
mocap_odom.vy = NAN;
mocap_odom.vz = NAN;
@@ -1266,7 +1267,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
// - add a MAV_FRAME_*_OTHER to the Mavlink MAV_FRAME enum IOT define
// a frame of reference which is not aligned with NED or ENU
// - add usage on the estimator side
visual_odom.local_frame = visual_odom.LOCAL_FRAME_NED;
visual_odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
const size_t URT_SIZE = sizeof(visual_odom.pose_covariance) / sizeof(visual_odom.pose_covariance[0]);
static_assert(URT_SIZE == (sizeof(ev.covariance) / sizeof(ev.covariance[0])),
@@ -1276,6 +1277,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
visual_odom.pose_covariance[i] = ev.covariance[i];
}
visual_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
visual_odom.vx = NAN;
visual_odom.vy = NAN;
visual_odom.vz = NAN;
@@ -1312,7 +1314,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
// TODO:
// add usage of this information on the estimator side
// The heading of the local frame is not aligned with north
odometry.local_frame = odometry.LOCAL_FRAME_FRD;
odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
// pose_covariance
static constexpr size_t POS_URT_SIZE = sizeof(odometry.pose_covariance) / sizeof(odometry.pose_covariance[0]);
@@ -1336,40 +1338,20 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
* body (fcu_frd) frame.
*/
if (odom.child_frame_id == MAV_FRAME_BODY_FRD) {
/* Linear velocity has to be rotated to the local NED frame
* Angular velocities are already in the right body frame */
const matrix::Dcmf R_body_to_local = matrix::Dcmf(q_body_to_local);
/* the linear velocities needs to be transformed to the local frame FRD*/
matrix::Vector3<float> linvel_local(R_body_to_local * matrix::Vector3<float>(odom.vx, odom.vy, odom.vz));
odometry.vx = linvel_local(0);
odometry.vy = linvel_local(1);
odometry.vz = linvel_local(2);
odometry.velocity_frame = vehicle_odometry_s::BODY_FRAME_FRD;
odometry.vx = odom.vx;
odometry.vy = odom.vy;
odometry.vz = odom.vz;
odometry.rollspeed = odom.rollspeed;
odometry.pitchspeed = odom.pitchspeed;
odometry.yawspeed = odom.yawspeed;
/* the linear velocity's covariance needs to be transformed to the local frame FRD*/
matrix::Matrix3f lin_vel_cov_body;
lin_vel_cov_body(0, 0) = odom.velocity_covariance[odometry.COVARIANCE_MATRIX_VX_VARIANCE];
lin_vel_cov_body(0, 1) = lin_vel_cov_body(1, 0) = odom.velocity_covariance[1];
lin_vel_cov_body(0, 2) = lin_vel_cov_body(2, 0) = odom.velocity_covariance[2];
lin_vel_cov_body(1, 1) = odom.velocity_covariance[odometry.COVARIANCE_MATRIX_VY_VARIANCE];
lin_vel_cov_body(1, 2) = lin_vel_cov_body(2, 1) = odom.velocity_covariance[7];
lin_vel_cov_body(2, 2) = odom.velocity_covariance[odometry.COVARIANCE_MATRIX_VZ_VARIANCE];
matrix::Matrix3f lin_vel_cov_local = R_body_to_local * lin_vel_cov_body * R_body_to_local.transpose();
/* Only the linear velocity variance elements are used */
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odometry.velocity_covariance[i] = NAN;
odometry.velocity_covariance[i] = odom.velocity_covariance[i];
}
odometry.velocity_covariance[odometry.COVARIANCE_MATRIX_VX_VARIANCE] = lin_vel_cov_local(0, 0);
odometry.velocity_covariance[odometry.COVARIANCE_MATRIX_VY_VARIANCE] = lin_vel_cov_local(1, 1);
odometry.velocity_covariance[odometry.COVARIANCE_MATRIX_VZ_VARIANCE] = lin_vel_cov_local(2, 2);
} else {
PX4_ERR("Body frame %u not supported. Unable to publish velocity", odom.child_frame_id);
}

33
src/modules/simulator/simulator_mavlink.cpp
View File

@@ -1027,24 +1027,17 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink)
mavlink_odometry_t odom_msg;
mavlink_msg_odometry_decode(odom_mavlink, &odom_msg);
/* The quaternion of the ODOMETRY msg represents a rotation from
* FRD body frame to the NED local frame */
matrix::Quatf q(odom_msg.q[0], odom_msg.q[1], odom_msg.q[2], odom_msg.q[3]);
q.normalize();
q.copyTo(odom.q);
/* Dcm rotation matrix from body frame to local NED frame */
const matrix::Dcmf R_body_to_local(q);
/* since odom.child_frame_id == MAV_FRAME_BODY_FRD, WRT to estimated vehicle body-fixed frame */
/* No need to transform the covariance matrices since the non-diagonal values are all zero */
/* The position in the local NED frame */
odom.x = odom_msg.x;
odom.y = odom_msg.y;
odom.z = odom_msg.z;
odom.local_frame = odom.LOCAL_FRAME_FRD;
/* The quaternion of the ODOMETRY msg represents a rotation from
* NED earth/local frame to XYZ body frame */
matrix::Quatf q(odom_msg.q[0], odom_msg.q[1], odom_msg.q[2], odom_msg.q[3]);
q.copyTo(odom.q);
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
static_assert(POS_URT_SIZE == (sizeof(odom_msg.pose_covariance) / sizeof(odom_msg.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
@@ -1054,14 +1047,12 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink)
odom.pose_covariance[i] = odom_msg.pose_covariance[i];
}
/* the linear velocities needs to be transformed from the body frame to the local frame */
const matrix::Vector3<float> linvel_local(R_body_to_local *
matrix::Vector3<float>(odom_msg.vx, odom_msg.vy, odom_msg.vz));
/* The velocity in the body-fixed frame */
odom.velocity_frame = vehicle_odometry_s::BODY_FRAME_FRD;
odom.vx = odom_msg.vx;
odom.vy = odom_msg.vy;
odom.vz = odom_msg.vz;
/* The velocity in the local NED frame */
odom.vx = linvel_local(0);
odom.vy = linvel_local(1);
odom.vz = linvel_local(2);
/* The angular velocity in the body-fixed frame */
odom.rollspeed = odom_msg.rollspeed;
odom.pitchspeed = odom_msg.pitchspeed;
@@ -1089,7 +1080,7 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink)
matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw));
q.copyTo(odom.q);
odom.local_frame = odom.LOCAL_FRAME_NED;
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
static_assert(POS_URT_SIZE == (sizeof(ev.covariance) / sizeof(ev.covariance[0])),
"Vision Position Estimate Pose Covariance matrix URT array size mismatch");