mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 21:13:44 +08:00
Support vision velocity expressed in body frame too (#708)
* Support vision velocity expressed in body frame * Use switch statement for vision velocity frame * Robustify vision velocity frame test * Increase lower bound on vision velocity noise to 0.05 m/s
This commit is contained in:
kritz
GitHub
+5
-2
@@ -55,6 +55,8 @@ using matrix::Vector2f;
|
||||
using matrix::Vector3f;
|
||||
using matrix::wrap_pi;
|
||||
|
||||
enum velocity_frame_t {LOCAL_FRAME_FRD, BODY_FRAME_FRD};
|
||||
|
||||
struct gps_message {
|
||||
uint64_t time_usec;
|
||||
int32_t lat; ///< Latitude in 1E-7 degrees
|
||||
@@ -139,11 +141,12 @@ struct flowSample {
|
||||
|
||||
struct extVisionSample {
|
||||
Vector3f pos; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
|
||||
Vector3f vel; ///< XYZ velocity in external vision's local reference frame (m/sec) - Z must be aligned with down axis
|
||||
Vector3f vel; ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis
|
||||
Quatf quat; ///< quaternion defining rotation from body to earth frame
|
||||
Vector3f posVar; ///< XYZ position variances (m**2)
|
||||
Vector3f velVar; ///< XYZ velocity variances ((m/sec)**2)
|
||||
Matrix3f velCov; ///< XYZ velocity covariances ((m/sec)**2)
|
||||
float angVar; ///< angular heading variance (rad**2)
|
||||
velocity_frame_t vel_frame = BODY_FRAME_FRD;
|
||||
uint64_t time_us; ///< timestamp of the measurement (uSec)
|
||||
};
|
||||
|
||||
|
||||
+6
-21
@@ -202,14 +202,14 @@ void Ekf::controlExternalVisionFusion()
|
||||
if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) {
|
||||
_control_status.flags.ev_pos = true;
|
||||
resetPosition();
|
||||
ECL_INFO_TIMESTAMPED("commencing external vision position fusion");
|
||||
ECL_INFO_TIMESTAMPED("starting vision pos fusion");
|
||||
}
|
||||
|
||||
// turn on use of external vision measurements for velocity
|
||||
if (_params.fusion_mode & MASK_USE_EVVEL && !_control_status.flags.ev_vel) {
|
||||
_control_status.flags.ev_vel = true;
|
||||
resetVelocity();
|
||||
ECL_INFO_TIMESTAMPED("commencing external vision velocity fusion");
|
||||
ECL_INFO_TIMESTAMPED("starting vision vel fusion");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -235,7 +235,7 @@ void Ekf::controlExternalVisionFusion()
|
||||
stopMagHdgFusion();
|
||||
stopMag3DFusion();
|
||||
|
||||
ECL_INFO_TIMESTAMPED("commencing external vision yaw fusion");
|
||||
ECL_INFO_TIMESTAMPED("starting vision yaw fusion");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -324,22 +324,7 @@ void Ekf::controlExternalVisionFusion()
|
||||
Vector3f ev_vel_obs_var;
|
||||
Vector2f ev_vel_innov_gates;
|
||||
|
||||
Vector3f vel_aligned{_ev_sample_delayed.vel};
|
||||
Matrix3f ev_vel_var = matrix::diag(_ev_sample_delayed.velVar);
|
||||
|
||||
// rotate measurement into correct earth frame if required
|
||||
if (_params.fusion_mode & MASK_ROTATE_EV) {
|
||||
vel_aligned = _R_ev_to_ekf * _ev_sample_delayed.vel;
|
||||
ev_vel_var = _R_ev_to_ekf * ev_vel_var * _R_ev_to_ekf.transpose();
|
||||
}
|
||||
|
||||
// correct velocity for offset relative to IMU
|
||||
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
|
||||
const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
|
||||
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
|
||||
vel_aligned -= vel_offset_earth;
|
||||
|
||||
_ev_vel_innov = _state.vel - vel_aligned;
|
||||
_ev_vel_innov = _state.vel - getVisionVelocityInEkfFrame();
|
||||
|
||||
// check if we have been deadreckoning too long
|
||||
if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) {
|
||||
@@ -350,7 +335,7 @@ void Ekf::controlExternalVisionFusion()
|
||||
}
|
||||
}
|
||||
|
||||
ev_vel_obs_var = matrix::max(ev_vel_var.diag(), sq(0.01f));
|
||||
ev_vel_obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f));
|
||||
|
||||
ev_vel_innov_gates.setAll(fmaxf(_params.ev_vel_innov_gate, 1.0f));
|
||||
|
||||
@@ -368,7 +353,7 @@ void Ekf::controlExternalVisionFusion()
|
||||
|
||||
// Turn off EV fusion mode if no data has been received
|
||||
stopEvFusion();
|
||||
ECL_INFO_TIMESTAMPED("External Vision Data Stopped");
|
||||
ECL_INFO_TIMESTAMPED("vision data stopped");
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -645,6 +645,10 @@ private:
|
||||
// update the rotation matrix which transforms EV navigation frame measurements into NED
|
||||
void calcExtVisRotMat();
|
||||
|
||||
Vector3f getVisionVelocityInEkfFrame();
|
||||
|
||||
Vector3f getVisionVelocityVarianceInEkfFrame();
|
||||
|
||||
// limit the diagonal of the covariance matrix
|
||||
// force symmetry when the argument is true
|
||||
void fixCovarianceErrors(bool force_symmetry);
|
||||
|
||||
+55
-8
@@ -106,8 +106,8 @@ void Ekf::resetVelocityToVision() {
|
||||
if(_params.fusion_mode & MASK_ROTATE_EV){
|
||||
_ev_vel = _R_ev_to_ekf *_ev_sample_delayed.vel;
|
||||
}
|
||||
resetVelocityTo(_ev_vel);
|
||||
P.uncorrelateCovarianceSetVariance<3>(4, _ev_sample_delayed.velVar);
|
||||
resetVelocityTo(getVisionVelocityInEkfFrame());
|
||||
P.uncorrelateCovarianceSetVariance<3>(4, getVisionVelocityVarianceInEkfFrame());
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalVelocityToZero() {
|
||||
@@ -310,13 +310,15 @@ void Ekf::resetHeight()
|
||||
P.uncorrelateCovarianceSetVariance<1>(6, 10.0f);
|
||||
}
|
||||
|
||||
// store the reset amount and time to be published
|
||||
if (vert_pos_reset) {
|
||||
// store the reset amount and time to be published
|
||||
_state_reset_status.posD_change = _state.pos(2) - old_vert_pos;
|
||||
_state_reset_status.posD_counter++;
|
||||
}
|
||||
|
||||
// apply the change in height / height rate to our newest height / height rate estimate
|
||||
// which have already been taken out from the output buffer
|
||||
// apply the change in height / height rate to our newest height / height rate estimate
|
||||
// which have already been taken out from the output buffer
|
||||
if (vert_pos_reset) {
|
||||
_output_new.pos(2) += _state_reset_status.posD_change;
|
||||
}
|
||||
|
||||
@@ -392,11 +394,11 @@ bool Ekf::realignYawGPS()
|
||||
|
||||
// correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned
|
||||
if (badMagYaw || !_control_status.flags.yaw_align) {
|
||||
ECL_WARN_TIMESTAMPED("bad yaw corrected using GPS course");
|
||||
ECL_WARN_TIMESTAMPED("bad yaw, using GPS course");
|
||||
|
||||
// declare the magnetometer as failed if a bad yaw has occurred more than once
|
||||
if (_control_status.flags.mag_aligned_in_flight && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) {
|
||||
ECL_WARN_TIMESTAMPED("stopping magnetometer use");
|
||||
ECL_WARN_TIMESTAMPED("stopping mag use");
|
||||
_control_status.flags.mag_fault = true;
|
||||
}
|
||||
|
||||
@@ -1504,6 +1506,51 @@ void Ekf::updateBaroHgtOffset()
|
||||
}
|
||||
}
|
||||
|
||||
Vector3f Ekf::getVisionVelocityInEkfFrame()
|
||||
{
|
||||
Vector3f vel;
|
||||
// correct velocity for offset relative to IMU
|
||||
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
|
||||
const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
|
||||
|
||||
// rotate measurement into correct earth frame if required
|
||||
switch(_ev_sample_delayed.vel_frame) {
|
||||
case BODY_FRAME_FRD:
|
||||
vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
|
||||
break;
|
||||
case LOCAL_FRAME_FRD:
|
||||
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
|
||||
if (_params.fusion_mode & MASK_ROTATE_EV)
|
||||
{
|
||||
vel = _R_ev_to_ekf *_ev_sample_delayed.vel - vel_offset_earth;
|
||||
} else {
|
||||
vel = _ev_sample_delayed.vel - vel_offset_earth;
|
||||
}
|
||||
break;
|
||||
}
|
||||
return vel;
|
||||
}
|
||||
|
||||
Vector3f Ekf::getVisionVelocityVarianceInEkfFrame()
|
||||
{
|
||||
Matrix3f ev_vel_cov = _ev_sample_delayed.velCov;
|
||||
|
||||
// rotate measurement into correct earth frame if required
|
||||
switch(_ev_sample_delayed.vel_frame) {
|
||||
case BODY_FRAME_FRD:
|
||||
ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose();
|
||||
break;
|
||||
|
||||
case LOCAL_FRAME_FRD:
|
||||
if(_params.fusion_mode & MASK_ROTATE_EV)
|
||||
{
|
||||
ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose();
|
||||
}
|
||||
break;
|
||||
}
|
||||
return ev_vel_cov.diag();
|
||||
}
|
||||
|
||||
// update the rotation matrix which rotates EV measurements into the EKF's navigation frame
|
||||
void Ekf::calcExtVisRotMat()
|
||||
{
|
||||
@@ -1755,7 +1802,7 @@ bool Ekf::resetYawToEKFGSF()
|
||||
// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
|
||||
// and cause another navigation failure
|
||||
_control_status.flags.mag_fault = true;
|
||||
ECL_INFO_TIMESTAMPED("Emergency yaw reset - magnetometer use stopped");
|
||||
ECL_INFO_TIMESTAMPED("Emergency yaw reset - mag use stopped");
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
+1
-3
@@ -99,11 +99,9 @@ bool Ekf::collect_gps(const gps_message &gps)
|
||||
// if the user has selected GPS as the primary height source, switch across to using it
|
||||
|
||||
if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
|
||||
ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set, using GPS height)");
|
||||
startGpsHgtFusion();
|
||||
} else {
|
||||
ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set)");
|
||||
}
|
||||
ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set)");
|
||||
}
|
||||
|
||||
// start collecting GPS if there is a 3D fix and the NED origin has been set
|
||||
|
||||
@@ -26,7 +26,12 @@ void Vio::setData(const extVisionSample& vio_data)
|
||||
|
||||
void Vio::setVelocityVariance(const Vector3f& velVar)
|
||||
{
|
||||
_vio_data.velVar = velVar;
|
||||
setVelocityCovariance(matrix::diag(velVar));
|
||||
}
|
||||
|
||||
void Vio::setVelocityCovariance(const Matrix3f& velCov)
|
||||
{
|
||||
_vio_data.velCov = velCov;
|
||||
}
|
||||
|
||||
void Vio::setPositionVariance(const Vector3f& posVar)
|
||||
@@ -54,6 +59,17 @@ void Vio::setOrientation(const Quatf& quat)
|
||||
_vio_data.quat = quat;
|
||||
}
|
||||
|
||||
void Vio::setVelocityFrameToBody()
|
||||
{
|
||||
_vio_data.vel_frame = BODY_FRAME_FRD;
|
||||
}
|
||||
|
||||
void Vio::setVelocityFrameToLocal()
|
||||
{
|
||||
_vio_data.vel_frame = LOCAL_FRAME_FRD;
|
||||
}
|
||||
|
||||
|
||||
extVisionSample Vio::dataAtRest()
|
||||
{
|
||||
extVisionSample vio_data;
|
||||
@@ -61,8 +77,9 @@ 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.velVar = Vector3f{0.1f, 0.1f, 0.1f};
|
||||
vio_data.velCov = matrix::eye<float ,3>() * 0.1f;
|
||||
vio_data.angVar = 0.05f;
|
||||
vio_data.vel_frame = LOCAL_FRAME_FRD;
|
||||
return vio_data;
|
||||
}
|
||||
|
||||
|
||||
@@ -52,11 +52,14 @@ 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);
|
||||
void setPosition(const Vector3f& pos);
|
||||
void setOrientation(const Quatf& quat);
|
||||
void setVelocityFrameToLocal();
|
||||
void setVelocityFrameToBody();
|
||||
|
||||
extVisionSample dataAtRest();
|
||||
|
||||
|
||||
@@ -59,7 +59,6 @@ class EkfExternalVisionTest : public ::testing::Test {
|
||||
void SetUp() override
|
||||
{
|
||||
_ekf->init(0);
|
||||
_sensor_simulator.runSeconds(3);
|
||||
}
|
||||
|
||||
// Use this method to clean up any memory, network etc. after each test
|
||||
@@ -70,6 +69,7 @@ class EkfExternalVisionTest : public ::testing::Test {
|
||||
|
||||
TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
|
||||
{
|
||||
_sensor_simulator.runSeconds(3);
|
||||
_ekf_wrapper.enableExternalVisionPositionFusion();
|
||||
_sensor_simulator.startExternalVision();
|
||||
_sensor_simulator.runSeconds(2);
|
||||
@@ -104,6 +104,7 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
|
||||
|
||||
TEST_F(EkfExternalVisionTest, visionVelocityReset)
|
||||
{
|
||||
_sensor_simulator.runSeconds(3);
|
||||
ResetLoggingChecker reset_logging_checker(_ekf);
|
||||
reset_logging_checker.capturePreResetState();
|
||||
|
||||
@@ -129,6 +130,7 @@ TEST_F(EkfExternalVisionTest, visionVelocityReset)
|
||||
|
||||
TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
|
||||
{
|
||||
_sensor_simulator.runSeconds(3);
|
||||
ResetLoggingChecker reset_logging_checker(_ekf);
|
||||
reset_logging_checker.capturePreResetState();
|
||||
// GIVEN: Drone is pointing north, and we use mag (ROTATE_EV)
|
||||
@@ -164,6 +166,7 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
|
||||
|
||||
TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset)
|
||||
{
|
||||
_sensor_simulator.runSeconds(3);
|
||||
const Vector3f simulated_position(8.3f, -1.0f, 0.0f);
|
||||
|
||||
_sensor_simulator._vio.setPosition(simulated_position);
|
||||
@@ -178,6 +181,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset)
|
||||
|
||||
TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment)
|
||||
{
|
||||
_sensor_simulator.runSeconds(3);
|
||||
// GIVEN: Drone is pointing north, and we use mag (ROTATE_EV)
|
||||
// Heading of drone in EKF frame is 0°
|
||||
|
||||
@@ -202,6 +206,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment)
|
||||
|
||||
TEST_F(EkfExternalVisionTest, visionVarianceCheck)
|
||||
{
|
||||
_sensor_simulator.runSeconds(3);
|
||||
const Vector3f velVar_init = _ekf->getVelocityVariance();
|
||||
EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
|
||||
|
||||
@@ -216,6 +221,7 @@ TEST_F(EkfExternalVisionTest, visionVarianceCheck)
|
||||
|
||||
TEST_F(EkfExternalVisionTest, visionAlignment)
|
||||
{
|
||||
_sensor_simulator.runSeconds(3);
|
||||
// GIVEN: Drone is pointing north, and we use mag (ROTATE_EV)
|
||||
// Heading of drone in EKF frame is 0°
|
||||
|
||||
@@ -244,3 +250,73 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
|
||||
EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(),
|
||||
estimatedExternalVisionFrameOffset.canonical()));
|
||||
}
|
||||
|
||||
TEST_F(EkfExternalVisionTest, velocityFrameBody)
|
||||
{
|
||||
// GIVEN: Drone is turned 90 degrees
|
||||
const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f)));
|
||||
_sensor_simulator.simulateOrientation(quat_sim);
|
||||
_sensor_simulator.runSeconds(3);
|
||||
|
||||
// Without any measurement x and y velocity variance are close
|
||||
const Vector3f velVar_init = _ekf->getVelocityVariance();
|
||||
EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
|
||||
|
||||
// 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_body(1.0f, 0.0f, 0.0f);
|
||||
_sensor_simulator._vio.setVelocityCovariance(vel_cov_body);
|
||||
_sensor_simulator._vio.setVelocity(vel_body);
|
||||
_ekf_wrapper.enableExternalVisionVelocityFusion();
|
||||
_sensor_simulator.startExternalVision();
|
||||
_sensor_simulator.runSeconds(4);
|
||||
|
||||
// THEN: As the drone is turned 90 degrees, velocity variance
|
||||
// along local y axis is expected to be bigger
|
||||
const Vector3f velVar_new = _ekf->getVelocityVariance();
|
||||
EXPECT_NEAR(velVar_new(1) / velVar_new(0), 80.f, 10.f);
|
||||
|
||||
const Vector3f vel_earth_est = _ekf->getVelocity();
|
||||
EXPECT_NEAR(vel_earth_est(0), 0.0f, 0.1f);
|
||||
EXPECT_NEAR(vel_earth_est(1), 1.0f, 0.1f);
|
||||
}
|
||||
|
||||
TEST_F(EkfExternalVisionTest, velocityFrameLocal)
|
||||
{
|
||||
// GIVEN: Drone is turned 90 degrees
|
||||
const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f)));
|
||||
_sensor_simulator.simulateOrientation(quat_sim);
|
||||
_sensor_simulator.runSeconds(3);
|
||||
|
||||
// Without any measurement x and y velocity variance are close
|
||||
const Vector3f velVar_init = _ekf->getVelocityVariance();
|
||||
EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
|
||||
|
||||
// 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_earth(1.0f, 0.0f, 0.0f);
|
||||
_sensor_simulator._vio.setVelocityCovariance(vel_cov_earth);
|
||||
_sensor_simulator._vio.setVelocity(vel_earth);
|
||||
_ekf_wrapper.enableExternalVisionVelocityFusion();
|
||||
_sensor_simulator.startExternalVision();
|
||||
_sensor_simulator.runSeconds(4);
|
||||
|
||||
// THEN: Independently on drones heading, velocity variance
|
||||
// along local x axis is expected to be bigger
|
||||
const Vector3f velVar_new = _ekf->getVelocityVariance();
|
||||
EXPECT_NEAR(velVar_new(0) / velVar_new(1), 80.f, 10.f);
|
||||
|
||||
const Vector3f vel_earth_est = _ekf->getVelocity();
|
||||
EXPECT_NEAR(vel_earth_est(0), 1.0f, 0.1f);
|
||||
EXPECT_NEAR(vel_earth_est(1), 0.0f, 0.1f);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user