Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_VisualOdom: support covariance from msg and add pos, ang err parameters #14516

Merged
merged 9 commits into from
Jun 9, 2020
Merged

AP_VisualOdom: support covariance from msg and add pos, ang err parameters #14516

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
873b741
AP_NavEKF2: use err info from ext nav interface
chobitsfan Jun 4, 2020
7251318
GCS_MAVLink: support covariance field in vis pose msg
chobitsfan Jun 4, 2020
f5217fa
AP_VisualOdom: support covariance from msg and add pos, ang err param…
chobitsfan Jun 4, 2020
d71bd21
AP_Logger: log ext nav pos err and ang err
chobitsfan Jun 4, 2020
fbc097e
AP_NavEKF3: use err info from ext nav interface
chobitsfan Jun 4, 2020
676b79c
AP_Logger: shorten VISP and VISV log field names
rmackay9 Jun 5, 2020
fe873e0
AP_Logger: fix missing unit description in VISP
chobitsfan Jun 9, 2020
f7bcf2f
AP_VisualOdom: clarify POS_M_NSE and YAW_M_NSE description
chobitsfan Jun 9, 2020
1f9c03a
AP_VisualOdom: fix desc typo
chobitsfan Jun 9, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion libraries/AP_Logger/AP_Logger.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -281,7 +281,7 @@ class AP_Logger
uint8_t sequence,
const RallyLocation &rally_point);
void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence);
void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, uint8_t reset_counter);
void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter);
void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter);
void Write_AOA_SSA(AP_AHRS &ahrs);
void Write_Beacon(AP_Beacon &beacon);
Expand Down
4 changes: 3 additions & 1 deletion libraries/AP_Logger/LogFile.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -911,7 +911,7 @@ void AP_Logger::Write_VisualOdom(float time_delta, const Vector3f &angle_delta,
}

// Write visual position sensor data. x,y,z are in meters, angles are in degrees
void AP_Logger::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, uint8_t reset_counter)
void AP_Logger::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter)
{
const struct log_VisualPosition pkt_visualpos {
LOG_PACKET_HEADER_INIT(LOG_VISUALPOS_MSG),
Expand All @@ -924,6 +924,8 @@ void AP_Logger::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms,
roll : roll,
pitch : pitch,
yaw : yaw,
pos_err : pos_err,
ang_err : ang_err,
reset_counter : reset_counter
};
WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition));
Expand Down
16 changes: 10 additions & 6 deletions libraries/AP_Logger/LogStructure.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -625,6 +625,8 @@ struct PACKED log_VisualPosition {
float roll; // degrees
float pitch; // degrees
float yaw; // degrees
float pos_err; // meters
float ang_err; // radians
uint8_t reset_counter;
};

Expand Down Expand Up @@ -2126,26 +2128,28 @@ struct PACKED log_Arm_Disarm {
// @LoggerMessage: VISP
// @Description: Vision Position
// @Field: TimeUS: System time
// @Field: RemTimeUS: Remote system time
// @Field: RTimeUS: Remote system time
// @Field: CTimeMS: Corrected system time
// @Field: PX: Position X-axis (North-South)
// @Field: PY: Position Y-axis (East-West)
// @Field: PZ: Position Z-axis (Down-Up)
// @Field: Roll: Roll lean angle
// @Field: Pitch: Pitch lean angle
// @Field: Yaw: Yaw angle
// @Field: ResetCnt: Position reset counter
// @Field: PErr: Position estimate error
// @Field: AErr: Attitude estimate error
// @Field: RstCnt: Position reset counter

// @LoggerMessage: VISV
// @Description: Vision Velocity
// @Field: TimeUS: System time
// @Field: RemTimeUS: Remote system time
// @Field: RTimeUS: Remote system time
// @Field: CTimeMS: Corrected system time
// @Field: VX: Velocity X-axis (North-South)
// @Field: VY: Velocity Y-axis (East-West)
// @Field: VZ: Velocity Z-axis (Down-Up)
// @Field: VErr: Velocity estimate error
// @Field: ResetCnt: Position reset counter
// @Field: RstCnt: Velocity reset counter

// @LoggerMessage: WENC
// @Description: Wheel encoder measurements
Expand Down Expand Up @@ -2504,9 +2508,9 @@ struct PACKED log_Arm_Disarm {
{ LOG_VISUALODOM_MSG, sizeof(log_VisualOdom), \
"VISO", "Qffffffff", "TimeUS,dt,AngDX,AngDY,AngDZ,PosDX,PosDY,PosDZ,conf", "ssrrrmmm-", "FF000000-" }, \
{ LOG_VISUALPOS_MSG, sizeof(log_VisualPosition), \
"VISP", "QQIffffffb", "TimeUS,RemTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,ResetCnt", "sssmmmddh-", "FFC000000-" }, \
"VISP", "QQIffffffffb", "TimeUS,RTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,PErr,AErr,RstCnt", "sssmmmddhmd-", "FFC00000000-" }, \
{ LOG_VISUALVEL_MSG, sizeof(log_VisualVelocity), \
"VISV", "QQIffffb", "TimeUS,RemTimeUS,CTimeMS,VX,VY,VZ,VErr,ResetCnt", "sssnnnn-", "FFC0000-" }, \
"VISV", "QQIffffb", "TimeUS,RTimeUS,CTimeMS,VX,VY,VZ,VErr,RstCnt", "sssnnnn-", "FFC0000-" }, \
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow), \
"OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEnn", "F-0000" }, \
{ LOG_WHEELENCODER_MSG, sizeof(log_WheelEncoder), \
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -788,6 +788,7 @@ void NavEKF2_core::fuseEulerYaw()
measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination());
} else if (extNavUsedForYaw) {
// Get the yaw angle from the external vision data
R_YAW = sq(extNavDataDelayed.angErr);
extNavDataDelayed.quat.to_euler(euler321.x, euler321.y, euler321.z);
measured_yaw = euler321.z;
} else {
Expand Down Expand Up @@ -847,6 +848,7 @@ void NavEKF2_core::fuseEulerYaw()
measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination());
} else if (extNavUsedForYaw) {
// Get the yaw angle from the external vision data
R_YAW = sq(extNavDataDelayed.angErr);
euler312 = extNavDataDelayed.quat.to_vector312();
measured_yaw = euler312.z;
} else {
Expand Down
6 changes: 1 addition & 5 deletions libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -909,11 +909,7 @@ void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,

extNavDataNew.pos = pos;
extNavDataNew.quat = quat;
if (posErr > 0) {
extNavDataNew.posErr = posErr;
} else {
extNavDataNew.posErr = frontend->_gpsHorizPosNoise;
}
extNavDataNew.posErr = posErr;
extNavDataNew.angErr = angErr;
timeStamp_ms = timeStamp_ms - delay_ms;
// Correct for the average intersampling delay due to the filter updaterate
Expand Down
6 changes: 1 addition & 5 deletions libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -963,11 +963,7 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
}

extNavDataNew.pos = pos;
if (posErr > 0) {
extNavDataNew.posErr = posErr;
} else {
extNavDataNew.posErr = frontend->_gpsHorizPosNoise;
}
extNavDataNew.posErr = posErr;

// calculate timestamp
timeStamp_ms = timeStamp_ms - delay_ms;
Expand Down
22 changes: 19 additions & 3 deletions libraries/AP_VisualOdom/AP_VisualOdom.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,22 @@ const AP_Param::GroupInfo AP_VisualOdom::var_info[] = {
// @User: Advanced
AP_GROUPINFO("_VEL_M_NSE", 5, AP_VisualOdom, _vel_noise, 0.1),

// @Param: _POS_M_NSE
// @DisplayName: Visual odometry position measurement noise
// @Description: Visual odometry position measurement noise minimum (meters). This value will be used if the sensor provides a lower noise value (or no noise value)
// @Units: m
// @Range: 0.1 10.0
// @User: Advanced
AP_GROUPINFO("_POS_M_NSE", 6, AP_VisualOdom, _pos_noise, 0.2f),

// @Param: _YAW_M_NSE
// @DisplayName: Visual odometry yaw measurement noise
// @Description: Visual odometry yaw measurement noise minimum (radians), This value will be used if the sensor provides a lower noise value (or no noise value)
// @Units: rad
// @Range: 0.05 1.0
// @User: Advanced
AP_GROUPINFO("_YAW_M_NSE", 7, AP_VisualOdom, _yaw_noise, 0.2f),

AP_GROUPEND
};

Expand Down Expand Up @@ -156,7 +172,7 @@ void AP_VisualOdom::handle_vision_position_delta_msg(const mavlink_message_t &ms

// general purpose method to consume position estimate data and send to EKF
// distances in meters, roll, pitch and yaw are in radians
void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, uint8_t reset_counter)
void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter)
{
// exit immediately if not enabled
if (!enabled()) {
Expand All @@ -168,7 +184,7 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin
// convert attitude to quaternion and call backend
Quaternion attitude;
attitude.from_euler(roll, pitch, yaw);
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, reset_counter);
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter);
}
}

Expand All @@ -182,7 +198,7 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin

// call backend
if (_driver != nullptr) {
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, reset_counter);
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, 0, 0, reset_counter);
}
}

Expand Down
10 changes: 9 additions & 1 deletion libraries/AP_VisualOdom/AP_VisualOdom.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,13 +73,19 @@ class AP_VisualOdom

// return velocity measurement noise in m/s
float get_vel_noise() const { return _vel_noise; }

// return position measurement noise in m
float get_pos_noise() const { return _pos_noise; }

// return yaw measurement noise in rad
float get_yaw_noise() const { return _yaw_noise; }

// consume vision_position_delta mavlink messages
void handle_vision_position_delta_msg(const mavlink_message_t &msg);

// general purpose methods to consume position estimate data and send to EKF
// distances in meters, roll, pitch and yaw are in radians
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, uint8_t reset_counter);
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter);
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter);

// general purpose methods to consume velocity estimate data and send to EKF
Expand All @@ -105,6 +111,8 @@ class AP_VisualOdom
AP_Float _pos_scale; // position scale factor applied to sensor values
AP_Int16 _delay_ms; // average delay relative to inertial measurements
AP_Float _vel_noise; // velocity measurement noise in m/s
AP_Float _pos_noise; // position measurement noise in meters
AP_Float _yaw_noise; // yaw measurement noise in radians

// reference to backends
AP_VisualOdom_Backend *_driver;
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ class AP_VisualOdom_Backend
void handle_vision_position_delta_msg(const mavlink_message_t &msg);

// consume vision position estimate data and send to EKF. distances in meters
virtual void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) = 0;
virtual void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) = 0;

// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
virtual void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) = 0;
Expand Down
8 changes: 4 additions & 4 deletions libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
extern const AP_HAL::HAL& hal;

// consume vision position estimate data and send to EKF. distances in meters
void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter)
void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
{
const float scale_factor = _frontend.get_pos_scale();
Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
Expand All @@ -41,9 +41,9 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
rotate_and_correct_position(pos);
rotate_attitude(att);

posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f);
angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f);
// send attitude and position to EKF
const float posErr = 0; // parameter required?
const float angErr = 0; // parameter required?
AP::ahrs().writeExtNavData(pos, att, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));

// calculate euler orientation for logging
Expand All @@ -53,7 +53,7 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
att.to_euler(roll, pitch, yaw);

// log sensor data
AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), reset_counter);
AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), posErr, angErr, reset_counter);

// store corrected attitude for use in pre-arm checks
_attitude_last = att;
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ class AP_VisualOdom_IntelT265 : public AP_VisualOdom_Backend
using AP_VisualOdom_Backend::AP_VisualOdom_Backend;

// consume vision position estimate data and send to EKF. distances in meters
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) override;
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override;

// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) override;
Expand Down
8 changes: 4 additions & 4 deletions libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,14 +30,14 @@ AP_VisualOdom_MAV::AP_VisualOdom_MAV(AP_VisualOdom &frontend) :
}

// consume vision position estimate data and send to EKF. distances in meters
void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter)
void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
{
const float scale_factor = _frontend.get_pos_scale();
Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};

posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f);
angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f);
// send attitude and position to EKF
const float posErr = 0; // parameter required?
const float angErr = 0; // parameter required?
AP::ahrs().writeExtNavData(pos, attitude, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));

// calculate euler orientation for logging
Expand All @@ -47,7 +47,7 @@ void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us,
attitude.to_euler(roll, pitch, yaw);

// log sensor data
AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), reset_counter);
AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter);

// record time for health monitoring
_last_update_ms = AP_HAL::millis();
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_VisualOdom/AP_VisualOdom_MAV.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ class AP_VisualOdom_MAV : public AP_VisualOdom_Backend
AP_VisualOdom_MAV(AP_VisualOdom &frontend);

// consume vision position estimate data and send to EKF. distances in meters
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) override;
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override;

// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) override;
Expand Down
1 change: 1 addition & 0 deletions libraries/GCS_MAVLink/GCS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -771,6 +771,7 @@ class GCS_MAVLINK
const float roll,
const float pitch,
const float yaw,
const float covariance[21],
const uint8_t reset_counter,
const uint16_t payload_size);
void handle_vision_speed_estimate(const mavlink_message_t &msg);
Expand Down
17 changes: 13 additions & 4 deletions libraries/GCS_MAVLink/GCS_Common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2926,7 +2926,7 @@ void GCS_MAVLINK::handle_vision_position_estimate(const mavlink_message_t &msg)
mavlink_vision_position_estimate_t m;
mavlink_msg_vision_position_estimate_decode(&msg, &m);

handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.reset_counter,
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.covariance, m.reset_counter,
PAYLOAD_SIZE(chan, VISION_POSITION_ESTIMATE));
}

Expand All @@ -2935,7 +2935,7 @@ void GCS_MAVLINK::handle_global_vision_position_estimate(const mavlink_message_t
mavlink_global_vision_position_estimate_t m;
mavlink_msg_global_vision_position_estimate_decode(&msg, &m);

handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.reset_counter,
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.covariance, m.reset_counter,
PAYLOAD_SIZE(chan, GLOBAL_VISION_POSITION_ESTIMATE));
}

Expand All @@ -2945,7 +2945,7 @@ void GCS_MAVLINK::handle_vicon_position_estimate(const mavlink_message_t &msg)
mavlink_msg_vicon_position_estimate_decode(&msg, &m);

// vicon position estimate does not include reset counter
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, 0,
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.covariance, 0,
PAYLOAD_SIZE(chan, VICON_POSITION_ESTIMATE));
}

Expand All @@ -2959,18 +2959,27 @@ void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t use
const float roll,
const float pitch,
const float yaw,
const float covariance[21],
const uint8_t reset_counter,
const uint16_t payload_size)
{
#if HAL_VISUALODOM_ENABLED
float posErr = 0;
float angErr = 0;
// correct offboard timestamp to be in local ms since boot
uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(usec, payload_size);

AP_VisualOdom *visual_odom = AP::visualodom();
if (visual_odom == nullptr) {
return;
}
visual_odom->handle_vision_position_estimate(usec, timestamp_ms, x, y, z, roll, pitch, yaw, reset_counter);

if (!isnan(covariance[0])) {
rmackay9 marked this conversation as resolved.
Show resolved Hide resolved
posErr = cbrtf(sq(covariance[0])+sq(covariance[6])+sq(covariance[11]));
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@chobitsfan @rmackay9 Hi. This approximation of a mean std.dev over all axes seems wrong to me. For a cov.matrix with all diagonal elements equal to 2, it gives 2.3, while correct answer is 1.41.
It'd look more reasonable to remove squares (gives 1.82)
posErr = cbrtf(covariance[0]+covariance[6]+covariance[11]);
or to use square root (gives 1.41).
posErr = sqrtf((covariance[0]+covariance[6]+covariance[11])/3);

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi @pavloblindnology Thank you. Would you mind creating a PR for a fix? I think other developers may need to see this.

angErr = cbrtf(sq(covariance[15])+sq(covariance[18])+sq(covariance[20]));
}

visual_odom->handle_vision_position_estimate(usec, timestamp_ms, x, y, z, roll, pitch, yaw, posErr, angErr, reset_counter);
#endif
}

Expand Down