forked from Archive/PX4-Autopilot
Merge pull request #106 from PX4/pr-visionSupport
EKF: add support for external vision data
This commit is contained in:
Paul Riseborough
commit
d5046b078e
31
EKF/common.h
31
EKF/common.h
|
|
@ -71,6 +71,13 @@ struct flow_message {
|
|||
uint32_t dt; // integration time of flow samples
|
||||
};
|
||||
|
||||
struct ext_vision_message {
|
||||
Vector3f posNED; // measured NED position relative to the local origin (m)
|
||||
Quaternion quat; // measured quaternion orientation defining rotation from NED to body frame
|
||||
float posErr; // 1-Sigma spherical position accuracy (m)
|
||||
float angErr; // 1-Sigma angular error (rad)
|
||||
};
|
||||
|
||||
struct outputSample {
|
||||
Quaternion quat_nominal; // nominal quaternion describing vehicle attitude
|
||||
Vector3f vel; // NED velocity estimate in earth frame in m/s
|
||||
|
|
@ -126,10 +133,19 @@ struct flowSample {
|
|||
uint64_t time_us; // timestamp in microseconds of the integration period mid-point
|
||||
};
|
||||
|
||||
struct extVisionSample {
|
||||
Vector3f posNED; // measured NED position relative to the local origin (m)
|
||||
Quaternion quat; // measured quaternion orientation defining rotation from NED to body frame
|
||||
float posErr; // 1-Sigma spherical position accuracy (m)
|
||||
float angErr; // 1-Sigma angular error (rad)
|
||||
uint64_t time_us; // timestamp of the measurement in microseconds
|
||||
};
|
||||
|
||||
// Integer definitions for vdist_sensor_type
|
||||
#define VDIST_SENSOR_BARO 0 // Use baro height
|
||||
#define VDIST_SENSOR_GPS 1 // Use GPS height
|
||||
#define VDIST_SENSOR_RANGE 2 // Use range finder height
|
||||
#define VDIST_SENSOR_EV 3 // USe external vision
|
||||
|
||||
// Bit locations for mag_declination_source
|
||||
#define MASK_USE_GEO_DECL (1<<0) // set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
|
||||
|
|
@ -140,6 +156,8 @@ struct flowSample {
|
|||
#define MASK_USE_GPS (1<<0) // set to true to use GPS data
|
||||
#define MASK_USE_OF (1<<1) // set to true to use optical flow data
|
||||
#define MASK_INHIBIT_ACC_BIAS (1<<2) // set to true to inhibit estimation of accelerometer delta velocity bias
|
||||
#define MASK_USE_EVPOS (1<<3) // set to true to use external vision NED position data
|
||||
#define MASK_USE_EVYAW (1<<4) // set to true to use exernal vision quaternion data for yaw
|
||||
|
||||
// Integer definitions for mag_fusion_type
|
||||
#define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic
|
||||
|
|
@ -150,6 +168,7 @@ struct flowSample {
|
|||
#define GPS_MAX_INTERVAL 5e5
|
||||
#define BARO_MAX_INTERVAL 2e5
|
||||
#define RNG_MAX_INTERVAL 2e5
|
||||
#define EV_MAX_INTERVAL 2e5
|
||||
|
||||
struct parameters {
|
||||
// measurement source control
|
||||
|
|
@ -163,6 +182,7 @@ struct parameters {
|
|||
float airspeed_delay_ms; // airspeed measurement delay relative to the IMU (msec)
|
||||
float flow_delay_ms; // optical flow measurement delay relative to the IMU (msec) - this is to the middle of the optical flow integration interval
|
||||
float range_delay_ms; // range finder measurement delay relative to the IMU (msec)
|
||||
float ev_delay_ms; // off-board vision measurement delay relative to the IMU (msec)
|
||||
|
||||
// input noise
|
||||
float gyro_noise; // IMU angular rate noise used for covariance prediction (rad/sec)
|
||||
|
|
@ -205,6 +225,11 @@ struct parameters {
|
|||
float range_innov_gate; // range finder fusion innovation consistency gate size (STD)
|
||||
float rng_gnd_clearance; // minimum valid value for range when on ground (m)
|
||||
|
||||
// vision position fusion
|
||||
float ev_noise; // observation noise for vision sensor estimates (m)
|
||||
float ev_innov_gate; // vision estimator fusion innovation consistency gate size (STD)
|
||||
float ev_gnd_clearance; // minimum valid value for vision based height estimate when on ground (m)
|
||||
|
||||
// optical flow fusion
|
||||
float flow_noise; // observation noise for optical flow LOS rate measurements (rad/sec)
|
||||
float flow_noise_qual_min; // observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
|
||||
|
|
@ -228,6 +253,7 @@ struct parameters {
|
|||
Vector3f gps_pos_body; // xyz position of the GPS antenna in body frame (m)
|
||||
Vector3f rng_pos_body; // xyz position of range sensor in body frame (m)
|
||||
Vector3f flow_pos_body; // xyz position of range sensor focal point in body frame (m)
|
||||
Vector3f ev_pos_body; // xyz position of VI-sensor focal point in body frame (m)
|
||||
|
||||
// output complementary filter tuning
|
||||
float vel_Tau; // velocity state correction time constant (1/sec)
|
||||
|
|
@ -247,6 +273,7 @@ struct parameters {
|
|||
airspeed_delay_ms = 200.0f;
|
||||
flow_delay_ms = 5.0f;
|
||||
range_delay_ms = 5.0f;
|
||||
ev_delay_ms = 100.0f;
|
||||
|
||||
// input noise
|
||||
gyro_noise = 1.5e-2f;
|
||||
|
|
@ -311,10 +338,12 @@ struct parameters {
|
|||
gps_pos_body = {};
|
||||
rng_pos_body = {};
|
||||
flow_pos_body = {};
|
||||
ev_pos_body = {};
|
||||
|
||||
// output complementary filter tuning time constants
|
||||
vel_Tau = 0.5f;
|
||||
pos_Tau = 0.25f;
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
|
|
@ -383,6 +412,8 @@ union filter_control_status_u {
|
|||
uint16_t baro_hgt : 1; // 9 - true when baro height is being fused as a primary height reference
|
||||
uint16_t rng_hgt : 1; // 10 - true when range finder height is being fused as a primary height reference
|
||||
uint16_t gps_hgt : 1; // 11 - true when range finder height is being fused as a primary height reference
|
||||
uint16_t ev_pos : 1; // 12 - true when local position data from external vision is being fused
|
||||
uint16_t ev_yaw : 1; // 13 - true when yaw data from external vision measurements is being fused
|
||||
} flags;
|
||||
uint16_t value;
|
||||
};
|
||||
|
|
|
|||
146
EKF/control.cpp
146
EKF/control.cpp
|
|
@ -56,6 +56,71 @@ void Ekf::controlFusionModes()
|
|||
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
|
||||
}
|
||||
|
||||
// control use of various external souces for positon and velocity aiding
|
||||
controlExternalVisionAiding();
|
||||
controlOpticalFlowAiding();
|
||||
controlGpsAiding();
|
||||
controlHeightAiding();
|
||||
controlMagAiding();
|
||||
|
||||
}
|
||||
|
||||
void Ekf::controlExternalVisionAiding()
|
||||
{
|
||||
// external vision position aiding selection logic
|
||||
if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) {
|
||||
// check for a exernal vision measurement that has fallen behind the fusion time horizon
|
||||
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
|
||||
// turn on use of external vision measurements for position and height
|
||||
_control_status.flags.ev_pos = true;
|
||||
printf("EKF switching to external vision position fusion\n");
|
||||
// turn off other forms of height aiding
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
// reset the position, height and velocity
|
||||
resetPosition();
|
||||
resetVelocity();
|
||||
resetHeight();
|
||||
}
|
||||
}
|
||||
|
||||
// external vision yaw aiding selection logic
|
||||
if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
|
||||
// check for a exernal vision measurement that has fallen behind the fusion time horizon
|
||||
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
|
||||
// reset the yaw angle to the value from the observaton quaternion
|
||||
// get the roll, pitch, yaw estimates from the quaternion states
|
||||
matrix::Quaternion<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
|
||||
_state.quat_nominal(3));
|
||||
matrix::Euler<float> euler_init(q_init);
|
||||
|
||||
// get initial yaw from the observation quaternion
|
||||
extVisionSample ev_newest = _ext_vision_buffer.get_newest();
|
||||
matrix::Quaternion<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3));
|
||||
matrix::Euler<float> euler_obs(q_obs);
|
||||
euler_init(2) = euler_obs(2);
|
||||
|
||||
// calculate initial quaternion states for the ekf
|
||||
_state.quat_nominal = Quaternion(euler_init);
|
||||
|
||||
// flag the yaw as aligned
|
||||
_control_status.flags.yaw_align = true;
|
||||
|
||||
// turn on fusion of external vision yaw measurements and disable all magnetoemter fusion
|
||||
_control_status.flags.ev_yaw = true;
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_3D = false;
|
||||
_control_status.flags.mag_dec = false;
|
||||
|
||||
printf("EKF switching to external vision yaw fusion\n");
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Ekf::controlOpticalFlowAiding()
|
||||
{
|
||||
// optical flow fusion mode selection logic
|
||||
// to start using optical flow data we need angular alignment complete, and fresh optical flow and height above terrain data
|
||||
if ((_params.fusion_mode & MASK_USE_OF) && !_control_status.flags.opt_flow && _control_status.flags.tilt_align
|
||||
|
|
@ -127,6 +192,22 @@ void Ekf::controlFusionModes()
|
|||
_control_status.flags.opt_flow = false;
|
||||
}
|
||||
|
||||
// handle the case when we are relying on optical flow fusion and lose it
|
||||
if (_control_status.flags.opt_flow && !_control_status.flags.gps) {
|
||||
// We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something
|
||||
if ((_time_last_imu - _time_last_of_fuse > 5e6)) {
|
||||
// Switch to the non-aiding mode, zero the velocity states
|
||||
// and set the synthetic position to the current estimate
|
||||
_control_status.flags.opt_flow = false;
|
||||
_last_known_posNE(0) = _state.pos(0);
|
||||
_last_known_posNE(1) = _state.pos(1);
|
||||
_state.vel.setZero();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlGpsAiding()
|
||||
{
|
||||
// GPS fusion mode selection logic
|
||||
// To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data
|
||||
if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
|
||||
|
|
@ -177,7 +258,10 @@ void Ekf::controlFusionModes()
|
|||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlHeightSensorTimeouts()
|
||||
{
|
||||
/*
|
||||
* Handle the case where we have not fused height measurements recently and
|
||||
* uncertainty exceeds the max allowable. Reset using the best available height
|
||||
|
|
@ -351,18 +435,40 @@ void Ekf::controlFusionModes()
|
|||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// handle the case when we are relying on optical flow fusion and lose it
|
||||
if (_control_status.flags.opt_flow && !_control_status.flags.gps) {
|
||||
// We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something
|
||||
if ((_time_last_imu - _time_last_of_fuse > 5e6)) {
|
||||
// Switch to the non-aiding mode, zero the veloity states
|
||||
// and set the synthetic position to the current estimate
|
||||
_control_status.flags.opt_flow = false;
|
||||
_last_known_posNE(0) = _state.pos(0);
|
||||
_last_known_posNE(1) = _state.pos(1);
|
||||
_state.vel.setZero();
|
||||
}
|
||||
void Ekf::controlHeightAiding()
|
||||
{
|
||||
// check for height sensor timeouts and reset and change sensor if necessary
|
||||
controlHeightSensorTimeouts();
|
||||
|
||||
// Control the soure of height measurements for the main filter
|
||||
if (_control_status.flags.ev_pos) {
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) {
|
||||
_control_status.flags.baro_hgt = true;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
|
||||
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS && !_gps_hgt_faulty) || _control_status.flags.gps_hgt) {
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = true;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
|
||||
} else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE && !_rng_hgt_faulty) {
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = true;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlMagAiding()
|
||||
{
|
||||
// If we are using external vision data for heading then no magnetometer fusion is used
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
|
||||
|
|
@ -415,28 +521,10 @@ void Ekf::controlFusionModes()
|
|||
_control_status.flags.mag_dec = false;
|
||||
}
|
||||
|
||||
// Control the soure of height measurements for the main filter
|
||||
if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) {
|
||||
_control_status.flags.baro_hgt = true;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
|
||||
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS && !_gps_hgt_faulty) || _control_status.flags.gps_hgt) {
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = true;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
|
||||
} else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE && !_rng_hgt_faulty) {
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = true;
|
||||
}
|
||||
|
||||
// if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid
|
||||
if (_time_last_imu - _time_last_arsp_fuse > 10e6 || _time_last_arsp_fuse == 0) {
|
||||
_control_status.flags.wind = false;
|
||||
} else {
|
||||
_control_status.flags.wind = true;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
32
EKF/ekf.cpp
32
EKF/ekf.cpp
|
|
@ -303,6 +303,26 @@ bool Ekf::update()
|
|||
|
||||
}
|
||||
|
||||
// If we are using external vision aiding and data has fallen behind the fusion time horizon then fuse it
|
||||
if (_ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed)) {
|
||||
// use external vision posiiton and height observations
|
||||
if (_control_status.flags.ev_pos) {
|
||||
_fuse_pos = true;
|
||||
_fuse_height = true;
|
||||
|
||||
// correct position and height for offset relative to IMU
|
||||
Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
|
||||
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
||||
_ev_sample_delayed.posNED(0) -= pos_offset_earth(0);
|
||||
_ev_sample_delayed.posNED(1) -= pos_offset_earth(1);
|
||||
_ev_sample_delayed.posNED(2) -= pos_offset_earth(2);
|
||||
}
|
||||
// use external vision yaw observation
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
fuseHeading();
|
||||
}
|
||||
}
|
||||
|
||||
// If we are using optical flow aiding and data has fallen behind the fusion time horizon, then fuse it
|
||||
if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
|
||||
&& _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5
|
||||
|
|
@ -315,8 +335,6 @@ bool Ekf::update()
|
|||
if (!_control_status.flags.gps && !_control_status.flags.opt_flow
|
||||
&& ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
|
||||
_fuse_pos = true;
|
||||
_gps_sample_delayed.pos(0) = _last_known_posNE(0);
|
||||
_gps_sample_delayed.pos(1) = _last_known_posNE(1);
|
||||
_time_last_fake_gps = _time_last_imu;
|
||||
}
|
||||
|
||||
|
|
@ -390,7 +408,11 @@ bool Ekf::initialiseFilter(void)
|
|||
|
||||
// set the default height source from the adjustable parameter
|
||||
if (_hgt_counter == 0) {
|
||||
_primary_hgt_source = _params.vdist_sensor_type;
|
||||
if (_params.fusion_mode & MASK_USE_EVPOS) {
|
||||
_primary_hgt_source = VDIST_SENSOR_EV;
|
||||
} else {
|
||||
_primary_hgt_source = _params.vdist_sensor_type;
|
||||
}
|
||||
}
|
||||
|
||||
if (_primary_hgt_source == VDIST_SENSOR_RANGE) {
|
||||
|
|
@ -409,7 +431,7 @@ bool Ekf::initialiseFilter(void)
|
|||
}
|
||||
}
|
||||
|
||||
} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) {
|
||||
} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS || _primary_hgt_source == VDIST_SENSOR_EV) {
|
||||
// if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS
|
||||
// later when it passes checks.
|
||||
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
|
||||
|
|
@ -550,7 +572,7 @@ void Ekf::predictState()
|
|||
|
||||
// filter and limit input between -50% and +100% of nominal value
|
||||
input = math::constrain(input,0.0005f * (float)(FILTER_UPDATE_PERRIOD_MS),0.002f * (float)(FILTER_UPDATE_PERRIOD_MS));
|
||||
_dt_ekf_avg = 0.99f*_dt_ekf_avg + 0.005f*(_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
|
||||
_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
|
||||
}
|
||||
|
||||
bool Ekf::collect_imu(imuSample &imu)
|
||||
|
|
|
|||
18
EKF/ekf.h
18
EKF/ekf.h
|
|
@ -330,6 +330,24 @@ private:
|
|||
// Control the filter fusion modes
|
||||
void controlFusionModes();
|
||||
|
||||
// control fusion of external vision observations
|
||||
void controlExternalVisionAiding();
|
||||
|
||||
// control fusion of optical flow observtions
|
||||
void controlOpticalFlowAiding();
|
||||
|
||||
// control fusion of GPS observations
|
||||
void controlGpsAiding();
|
||||
|
||||
// control fusion of height position observations
|
||||
void controlHeightAiding();
|
||||
|
||||
// control fusion of magnetometer observations
|
||||
void controlMagAiding();
|
||||
|
||||
// control for height sensor timeouts, sensor changes and state resets
|
||||
void controlHeightSensorTimeouts();
|
||||
|
||||
// return the square of two floating point numbers - used in auto coded sections
|
||||
inline float sq(float var)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -51,15 +51,22 @@
|
|||
// gps measurement then use for velocity initialisation
|
||||
bool Ekf::resetVelocity()
|
||||
{
|
||||
// if we have a valid GPS measurement use it to initialise velocity states
|
||||
gpsSample gps_newest = _gps_buffer.get_newest();
|
||||
if (_control_status.flags.gps) {
|
||||
// if we have a valid GPS measurement use it to initialise velocity states
|
||||
gpsSample gps_newest = _gps_buffer.get_newest();
|
||||
|
||||
if (_time_last_imu - gps_newest.time_us < 400000) {
|
||||
_state.vel = gps_newest.vel;
|
||||
if (_time_last_imu - gps_newest.time_us < 2*GPS_MAX_INTERVAL) {
|
||||
_state.vel = gps_newest.vel;
|
||||
return true;
|
||||
|
||||
} else {
|
||||
// XXX use the value of the last known velocity
|
||||
return false;
|
||||
}
|
||||
} else if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) {
|
||||
_state.vel.setZero();
|
||||
return true;
|
||||
|
||||
} else {
|
||||
// XXX use the value of the last known velocity
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
|
@ -68,18 +75,48 @@ bool Ekf::resetVelocity()
|
|||
// gps measurement then use for position initialisation
|
||||
bool Ekf::resetPosition()
|
||||
{
|
||||
// if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay
|
||||
gpsSample gps_newest = _gps_buffer.get_newest();
|
||||
if (_control_status.flags.gps) {
|
||||
// if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay
|
||||
gpsSample gps_newest = _gps_buffer.get_newest();
|
||||
|
||||
float time_delay = 1e-6f * (float)(_time_last_imu - gps_newest.time_us);
|
||||
float time_delay = 1e-6f * (float)(_imu_sample_delayed.time_us - gps_newest.time_us);
|
||||
float max_time_delay = 1e-6f * (float)GPS_MAX_INTERVAL;
|
||||
|
||||
if (time_delay < 0.4f) {
|
||||
_state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay;
|
||||
_state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay;
|
||||
if (time_delay < max_time_delay) {
|
||||
_state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay;
|
||||
_state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay;
|
||||
return true;
|
||||
|
||||
} else {
|
||||
// XXX use the value of the last known position
|
||||
return false;
|
||||
}
|
||||
} else if (_control_status.flags.opt_flow) {
|
||||
_state.pos(0) = 0.0f;
|
||||
_state.pos(1) = 0.0f;
|
||||
return true;
|
||||
} else if (_control_status.flags.ev_pos) {
|
||||
// if we have fresh data, reset the position to the measurement
|
||||
extVisionSample ev_newest = _ext_vision_buffer.get_newest();
|
||||
if (_time_last_imu - ev_newest.time_us < 2*EV_MAX_INTERVAL) {
|
||||
// use the most recent data if it's time offset from the fusion time horizon is smaller
|
||||
int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
|
||||
int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
|
||||
if (abs(dt_newest) < abs(dt_delayed)) {
|
||||
_state.pos(0) = ev_newest.posNED(0);
|
||||
_state.pos(1) = ev_newest.posNED(1);
|
||||
} else {
|
||||
_state.pos(0) = _ev_sample_delayed.posNED(0);
|
||||
_state.pos(1) = _ev_sample_delayed.posNED(1);
|
||||
}
|
||||
return true;
|
||||
|
||||
} else {
|
||||
// XXX use the value of the last known position
|
||||
return false;
|
||||
}
|
||||
|
||||
} else {
|
||||
// XXX use the value of the last known position
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
|
@ -116,13 +153,14 @@ void Ekf::resetHeight()
|
|||
|
||||
vert_pos_reset = true;
|
||||
|
||||
// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
|
||||
baroSample baro_newest = _baro_buffer.get_newest();
|
||||
_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
|
||||
|
||||
} else {
|
||||
// TODO: reset to last known range based estimate
|
||||
}
|
||||
|
||||
// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
|
||||
baroSample baro_newest = _baro_buffer.get_newest();
|
||||
_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
|
||||
|
||||
} else if (_control_status.flags.baro_hgt) {
|
||||
// initialize vertical position with newest baro measurement
|
||||
|
|
@ -158,13 +196,24 @@ void Ekf::resetHeight()
|
|||
|
||||
vert_pos_reset = true;
|
||||
|
||||
// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
|
||||
baroSample baro_newest = _baro_buffer.get_newest();
|
||||
_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
|
||||
|
||||
} else {
|
||||
// TODO: reset to last known gps based estimate
|
||||
}
|
||||
|
||||
// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
|
||||
baroSample baro_newest = _baro_buffer.get_newest();
|
||||
_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
|
||||
} else if (_control_status.flags.ev_pos) {
|
||||
// initialize vertical position with newest measurement
|
||||
extVisionSample ev_newest = _ext_vision_buffer.get_newest();
|
||||
|
||||
if (_time_last_imu - ev_newest.time_us < 2 * EV_MAX_INTERVAL) {
|
||||
_state.pos(2) = ev_newest.posNED(2);
|
||||
|
||||
} else {
|
||||
// TODO: reset to last known baro based estimate
|
||||
}
|
||||
}
|
||||
|
||||
// reset the vertical velocity covariance values
|
||||
|
|
|
|||
|
|
@ -65,6 +65,7 @@ EstimatorInterface::EstimatorInterface():
|
|||
_time_last_baro(0),
|
||||
_time_last_range(0),
|
||||
_time_last_airspeed(0),
|
||||
_time_last_ext_vision(0),
|
||||
_mag_declination_gps(0.0f),
|
||||
_mag_declination_to_save_deg(0.0f)
|
||||
{
|
||||
|
|
@ -284,6 +285,30 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
|
|||
}
|
||||
}
|
||||
|
||||
// set attitude and position data derived from an external vision system
|
||||
void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message *evdata)
|
||||
{
|
||||
if (!_initialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
// if data passes checks, push to buffer
|
||||
if (time_usec > _time_last_ext_vision) {
|
||||
extVisionSample ev_sample_new;
|
||||
// calculate the system time-stamp for the mid point of the integration period
|
||||
ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000;
|
||||
// copy required data
|
||||
ev_sample_new.angErr = evdata->angErr;
|
||||
ev_sample_new.posErr = evdata->posErr;
|
||||
ev_sample_new.quat = evdata->quat;
|
||||
ev_sample_new.posNED = evdata->posNED;
|
||||
// record time for comparison next measurement
|
||||
_time_last_ext_vision = time_usec;
|
||||
// push to buffer
|
||||
_ext_vision_buffer.push(ev_sample_new);
|
||||
}
|
||||
}
|
||||
|
||||
bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||
{
|
||||
|
||||
|
|
@ -294,6 +319,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
|||
_range_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_airspeed_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_flow_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_ext_vision_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_output_buffer.allocate(IMU_BUFFER_LENGTH))) {
|
||||
printf("EKF buffer allocation failed!");
|
||||
unallocate_buffers();
|
||||
|
|
@ -314,6 +340,8 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
|||
_airspeed_buffer.push(airspeed_sample_init);
|
||||
flowSample flow_sample_init = {};
|
||||
_flow_buffer.push(flow_sample_init);
|
||||
extVisionSample ext_vision_sample_init = {};
|
||||
_ext_vision_buffer.push(ext_vision_sample_init);
|
||||
}
|
||||
|
||||
// zero the data in the imu data and output observer state buffers
|
||||
|
|
@ -343,8 +371,8 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
|||
_time_last_range = 0;
|
||||
_time_last_airspeed = 0;
|
||||
_time_last_optflow = 0;
|
||||
|
||||
memset(&_fault_status.flags, 0, sizeof(_fault_status.flags));
|
||||
_time_last_ext_vision = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -357,6 +385,7 @@ void EstimatorInterface::unallocate_buffers()
|
|||
_range_buffer.unallocate();
|
||||
_airspeed_buffer.unallocate();
|
||||
_flow_buffer.unallocate();
|
||||
_ext_vision_buffer.unallocate();
|
||||
_output_buffer.unallocate();
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -145,6 +145,9 @@ public:
|
|||
// set optical flow data
|
||||
void setOpticalFlowData(uint64_t time_usec, flow_message *flow);
|
||||
|
||||
// set external vision position and attitude data
|
||||
void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata);
|
||||
|
||||
// return a address to the parameters struct
|
||||
// in order to give access to the application
|
||||
parameters *getParamHandle() {return &_params;}
|
||||
|
|
@ -245,6 +248,7 @@ protected:
|
|||
rangeSample _range_sample_delayed;
|
||||
airspeedSample _airspeed_sample_delayed;
|
||||
flowSample _flow_sample_delayed;
|
||||
extVisionSample _ev_sample_delayed;
|
||||
|
||||
outputSample _output_sample_delayed; // filter output on the delayed time horizon
|
||||
outputSample _output_new; // filter output on the non-delayed time horizon
|
||||
|
|
@ -277,6 +281,7 @@ protected:
|
|||
RingBuffer<rangeSample> _range_buffer;
|
||||
RingBuffer<airspeedSample> _airspeed_buffer;
|
||||
RingBuffer<flowSample> _flow_buffer;
|
||||
RingBuffer<extVisionSample> _ext_vision_buffer;
|
||||
RingBuffer<outputSample> _output_buffer;
|
||||
|
||||
uint64_t _time_last_imu; // timestamp of last imu sample in microseconds
|
||||
|
|
@ -285,6 +290,7 @@ protected:
|
|||
uint64_t _time_last_baro; // timestamp of last barometer measurement in microseconds
|
||||
uint64_t _time_last_range; // timestamp of last range measurement in microseconds
|
||||
uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds
|
||||
uint64_t _time_last_ext_vision; // timestamp of last external vision measurement in microseconds
|
||||
uint64_t _time_last_optflow;
|
||||
|
||||
fault_status_u _fault_status;
|
||||
|
|
|
|||
|
|
@ -379,12 +379,11 @@ void Ekf::fuseHeading()
|
|||
float q2 = _state.quat_nominal(2);
|
||||
float q3 = _state.quat_nominal(3);
|
||||
|
||||
float R_YAW = fmaxf(_params.mag_heading_noise, 1.0e-2f);
|
||||
R_YAW = R_YAW * R_YAW;
|
||||
|
||||
float R_YAW = 1.0f;
|
||||
float predicted_hdg;
|
||||
float H_YAW[4];
|
||||
matrix::Vector3f mag_earth_pred;
|
||||
float measured_hdg;
|
||||
|
||||
// determine if a 321 or 312 Euler sequence is best
|
||||
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
|
||||
|
|
@ -426,8 +425,21 @@ void Ekf::fuseHeading()
|
|||
euler321(2) = 0.0f;
|
||||
matrix::Dcm<float> R_to_earth(euler321);
|
||||
|
||||
// rotate the magnetometer measurements into earth frame using a zero yaw angle
|
||||
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
|
||||
// calculate the observed yaw angle
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
// rotate the magnetometer measurements into earth frame using a zero yaw angle
|
||||
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
|
||||
// the angle of the projection onto the horizontal gives the yaw angle
|
||||
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
|
||||
} else if (_control_status.flags.ev_yaw) {
|
||||
// convert the observed quaternion to a rotation matrix
|
||||
matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
|
||||
// calculate the yaw angle for a 312 sequence
|
||||
measured_hdg = atan2f(R_to_earth_ev(1, 0) , R_to_earth_ev(0, 0));
|
||||
} else {
|
||||
// there is no yaw observation
|
||||
return;
|
||||
}
|
||||
|
||||
} else {
|
||||
// calculate observaton jacobian when we are observing a rotation in a 312 sequence
|
||||
|
|
@ -491,15 +503,39 @@ void Ekf::fuseHeading()
|
|||
R_to_earth(2, 0) = -s2 * c1;
|
||||
R_to_earth(2, 1) = s1;
|
||||
|
||||
// rotate the magnetometer measurements into earth frame using a zero yaw angle
|
||||
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
|
||||
// calculate the observed yaw angle
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
// rotate the magnetometer measurements into earth frame using a zero yaw angle
|
||||
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
|
||||
// the angle of the projection onto the horizontal gives the yaw angle
|
||||
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
|
||||
} else if (_control_status.flags.ev_yaw) {
|
||||
// convert the observed quaternion to a rotation matrix
|
||||
matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
|
||||
// calculate the yaw angle for a 312 sequence
|
||||
measured_hdg = atan2f(-R_to_earth_ev(0, 1) , R_to_earth_ev(1, 1));
|
||||
} else {
|
||||
// there is no yaw observation
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Calculate the observation variance
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
// using magnetic heading tuning parameter
|
||||
R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
|
||||
} else if (_control_status.flags.ev_yaw) {
|
||||
// using error estimate from external vision data
|
||||
R_YAW = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
|
||||
} else {
|
||||
// there is no yaw observation
|
||||
return;
|
||||
}
|
||||
|
||||
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
|
||||
// calculate the innovaton variance
|
||||
float PH[4];
|
||||
_heading_innov_var = R_YAW;
|
||||
|
||||
for (unsigned row = 0; row <= 3; row++) {
|
||||
PH[row] = 0.0f;
|
||||
|
||||
|
|
@ -553,9 +589,6 @@ void Ekf::fuseHeading()
|
|||
}
|
||||
}
|
||||
|
||||
// Use the difference between the horizontal projection of the mag field and declination to give the measured heading
|
||||
float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
|
||||
|
||||
// wrap the heading to the interval between +-pi
|
||||
measured_hdg = matrix::wrap_pi(measured_hdg);
|
||||
|
||||
|
|
|
|||
|
|
@ -83,24 +83,31 @@ void Ekf::fuseVelPosHeight()
|
|||
|
||||
if (_fuse_pos) {
|
||||
fuse_map[3] = fuse_map[4] = true;
|
||||
// horizontal position innovations
|
||||
_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
|
||||
_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
|
||||
|
||||
// observation variance - user parameter defined
|
||||
// if we are in flight and not using GPS, then use a specific parameter
|
||||
if (!_control_status.flags.gps) {
|
||||
// Calculate innovations and observation variance depending on type of observations
|
||||
// being used
|
||||
if (!_control_status.flags.gps && !_control_status.flags.ev_pos) {
|
||||
// No observations - use a static position to constrain drift
|
||||
if (_control_status.flags.in_air) {
|
||||
R[3] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
|
||||
|
||||
} else {
|
||||
R[3] = _params.gps_pos_noise;
|
||||
}
|
||||
_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
|
||||
_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
|
||||
|
||||
} else {
|
||||
} else if (_control_status.flags.gps) {
|
||||
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
|
||||
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
|
||||
R[3] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
|
||||
_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
|
||||
_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
|
||||
|
||||
} else if (_control_status.flags.ev_pos) {
|
||||
R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
|
||||
_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
|
||||
_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -145,6 +152,15 @@ void Ekf::fuseVelPosHeight()
|
|||
R[5] = R[5] * R[5];
|
||||
// innovation gate size
|
||||
gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f);
|
||||
} else if (_control_status.flags.ev_pos) {
|
||||
fuse_map[5] = true;
|
||||
// calculate the innovation assuming the external vision observaton is in local NED frame
|
||||
_vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);
|
||||
// observation variance - defined externally
|
||||
R[5] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
|
||||
R[5] = R[5] * R[5];
|
||||
// innovation gate size
|
||||
gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue