fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 05:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Support vision velocity expressed in body frame too (#708)

Browse Source 
* 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
2020-05-12 16:03:35 +02:00
committed by GitHub
parent c3de452e8e
commit 98801ad17b
8 changed files with 170 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+5 -2
View File
@@ -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)
};
EKF/control.cpp
+6 -21
View File
@@ -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");
}
}
EKF/ekf.h
+4
View File
@@ -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);
EKF/ekf_helper.cpp
+55 -8
View File
@@ -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;
EKF/gps_checks.cpp
+1 -3
View File
@@ -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