void Ekf::controlOpticalFlowFusion() { // Check for new optical flow data that has fallen behind the fusion time horizon if (_flow_data_ready) { // optical flow fusion mode selection logic if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user && !_control_status.flags.opt_flow // we are not yet using flow data && _control_status.flags.tilt_align // we know our tilt attitude && (_time_last_imu - _time_last_hagl_fuse) < 5e5) // we have a valid distance to ground estimate { // If the heading is not aligned, reset the yaw and magnetic field states if (!_control_status.flags.yaw_align) { _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } // If the heading is valid, start using optical flow aiding if (_control_status.flags.yaw_align) { // set the flag and reset the fusion timeout _control_status.flags.opt_flow = true; _time_last_of_fuse = _time_last_imu; // if we are not using GPS then the velocity and position states and covariances need to be set if (!_control_status.flags.gps) { // constrain height above ground to be above minimum possible float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); // calculate absolute distance from focal point to centre of frame assuming a flat earth float range = heightAboveGndEst / _R_to_earth(2, 2); if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) { // we should have reliable OF measurements so // calculate X and Y body relative velocities from OF measurements Vector3f vel_optflow_body; vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt; vel_optflow_body(1) = range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt; vel_optflow_body(2) = 0.0f; // rotate from body to earth frame Vector3f vel_optflow_earth; vel_optflow_earth = _R_to_earth * vel_optflow_body; // take x and Y components _state.vel(0) = vel_optflow_earth(0); _state.vel(1) = vel_optflow_earth(1); } else { _state.vel(0) = 0.0f; _state.vel(1) = 0.0f; } // reset the velocity covariance terms zeroRows(P,4,5); zeroCols(P,4,5); // reset the horizontal velocity variance using the optical flow noise variance P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar(); if (!_control_status.flags.in_air) { // we are likely starting OF for the first time so reset the horizontal position and vertical velocity states _state.pos(0) = 0.0f; _state.pos(1) = 0.0f; // reset the corresponding covariances // we are by definition at the origin at commencement so variances are also zeroed zeroRows(P,7,8); zeroCols(P,7,8); // align the output observer to the EKF states alignOutputFilter(); } } } } else if (!(_params.fusion_mode & MASK_USE_OF)) { _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(); } } // fuse the data if (_control_status.flags.opt_flow) { // Update optical flow bias estimates calcOptFlowBias(); // Fuse optical flow LOS rate observations into the main filter fuseOptFlow(); _last_known_posNE(0) = _state.pos(0); _last_known_posNE(1) = _state.pos(1); } } }
bool Ekf::update() { if (!_filter_initialised) { _filter_initialised = initialiseFilter(); if (!_filter_initialised) { return false; } } // Only run the filter if IMU data in the buffer has been updated if (_imu_updated) { // perform state and covariance prediction for the main filter predictState(); predictCovariance(); // perform state and variance prediction for the terrain estimator if (!_terrain_initialised) { _terrain_initialised = initHagl(); } else { predictHagl(); } // control logic controlFusionModes(); // measurement updates // Fuse magnetometer data using the selected fuson method and only if angular alignment is complete if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) { fuseMag(); if (_control_status.flags.mag_dec) { fuseDeclination(); } } else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) { fuseMag2D(); } else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) { // fusion of a Euler yaw angle from either a 321 or 312 rotation sequence fuseHeading(); } else { // do no fusion at all } } // determine if range finder data has fallen behind the fusin time horizon fuse it if we are // not tilted too much to use it if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed) && (_R_prev(2, 2) > 0.7071f)) { // if we have range data we always try to estimate terrain height _fuse_hagl_data = true; // only use range finder as a height observation in the main filter if specifically enabled if (_control_status.flags.rng_hgt) { _fuse_height = true; } } else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) { // If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements // and are on the ground, then synthesise a measurement at the expected on ground value if (!_in_air) { _range_sample_delayed.rng = _params.rng_gnd_clearance; _range_sample_delayed.time_us = _imu_sample_delayed.time_us; } _fuse_height = true; } // determine if baro data has fallen behind the fuson time horizon and fuse it in the main filter if enabled if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { if (_control_status.flags.baro_hgt) { _fuse_height = true; } else { // calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference float offset_error = _state.pos(2) + _baro_sample_delayed.hgt - _hgt_sensor_offset - _baro_hgt_offset; _baro_hgt_offset += 0.02f * math::constrain(offset_error, -5.0f, 5.0f); } } // If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) { // Only use GPS data for position and velocity aiding if enabled if (_control_status.flags.gps) { _fuse_pos = true; _fuse_vert_vel = true; _fuse_hor_vel = true; } // only use gps height observation in the main filter if specifically enabled if (_control_status.flags.gps_hgt) { _fuse_height = true; } } // 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 && (_R_prev(2, 2) > 0.7071f)) { _fuse_flow = true; } // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz 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; } // fuse available range finder data into a terrain height estimator if it has been initialised if (_fuse_hagl_data && _terrain_initialised) { fuseHagl(); _fuse_hagl_data = false; } // Fuse available NED velocity and position data into the main filter if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) { fuseVelPosHeight(); _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false; } // Update optical flow bias estimates calcOptFlowBias(); // Fuse optical flow LOS rate observations into the main filter if (_fuse_flow) { fuseOptFlow(); _last_known_posNE(0) = _state.pos(0); _last_known_posNE(1) = _state.pos(1); _fuse_flow = false; } // TODO This is just to get the logic inside but we will only start fusion once we tested this again if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed) && false) { fuseAirspeed(); } } // the output observer always runs calculateOutputStates(); // check for NaN on attitude states if (isnan(_state.quat_nominal(0)) || isnan(_output_new.quat_nominal(0))) { return false; } // We don't have valid data to output until tilt and yaw alignment is complete if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) { return true; } else { return false; } }