// Reset heading and magnetic field states
bool Ekf::resetMagHeading(Vector3f &mag_init)
{
// If we don't a tilt estimate then we cannot initialise the yaw
if (!_control_status.flags.tilt_align) {
return false;
}
// get the roll, pitch, yaw estimates and set the yaw to zero
matrix::Quaternion<float> q(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
_state.quat_nominal(3));
matrix::Euler<float> euler_init(q);
euler_init(2) = 0.0f;
// rotate the magnetometer measurements into earth axes
matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init;
euler_init(2) = _mag_declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
// calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps
_state.quat_nominal = Quaternion(euler_init);
// reset the angle error variances because the yaw angle could have changed by a significant amount
// by setting them to zero we avoid 'kicks' in angle when 3-D fusion starts and the imu process noise
// will grow them again.
zeroRows(P, 0, 2);
zeroCols(P, 0, 2);
// calculate initial earth magnetic field states
matrix::Dcm<float> R_to_earth(euler_init);
_state.mag_I = R_to_earth * mag_init;
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
for (uint8_t index = 16; index <= 21; index ++) {
P[index][index] = sq(_params.mag_noise);
}
return true;
}
bool Ekf::initialiseFilter(void)
{
_state.ang_error.setZero();
_state.vel.setZero();
_state.pos.setZero();
_state.gyro_bias.setZero();
_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
_state.accel_z_bias = 0.0f;
_state.mag_I.setZero();
_state.mag_B.setZero();
_state.wind_vel.setZero();
// get initial roll and pitch estimate from accel vector, assuming vehicle is static
Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt;
float pitch = 0.0f;
float roll = 0.0f;
if (accel_init.norm() > 0.001f) {
accel_init.normalize();
pitch = asinf(accel_init(0));
roll = -asinf(accel_init(1) / cosf(pitch));
}
matrix::Euler<float> euler_init(roll, pitch, 0.0f);
// Get the latest magnetic field measurement.
// If we don't have a measurement then we cannot initialise the filter
magSample mag_init = _mag_buffer.get_newest();
if (mag_init.time_us == 0) {
return false;
}
// rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination
// TODO use declination if available
matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init.mag;
float declination = 0.0f;
euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
// calculate initial quaternion states
_state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
// calculate initial earth magnetic field states
matrix::Dcm<float> R_to_earth(euler_init);
_state.mag_I = R_to_earth * mag_init.mag;
resetVelocity();
resetPosition();
// initialize vertical position with newest baro measurement
baroSample baro_init = _baro_buffer.get_newest();
if (baro_init.time_us == 0) {
return false;
}
_state.pos(2) = -baro_init.hgt;
_output_new.pos(2) = -baro_init.hgt;
initialiseCovariance();
return true;
}
void Ekf::controlExternalVisionFusion()
{
// Check for new exernal vision data
if (_ev_data_ready) {
// 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;
ECL_INFO("EKF switching to external vision position fusion");
// 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);
// save a copy of the quaternion state for later use in calculating the amount of reset change
Quaternion quat_before_reset = _state.quat_nominal;
// calculate initial quaternion states for the ekf
_state.quat_nominal = Quaternion(euler_init);
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
// add the reset amount to the output observer buffered data
outputSample output_states;
unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= _state_reset_status.quat_change;
_output_buffer.push_to_index(i,output_states);
}
// capture the reset event
_state_reset_status.quat_counter++;
// 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;
ECL_INFO("EKF switching to external vision yaw fusion");
}
}
// determine if we should use the height observation
if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = true;
_fuse_height = true;
}
// determine if we should use the horizontal position observations
if (_control_status.flags.ev_pos) {
_fuse_pos = 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);
}
// determine if we should use the yaw observation
if (_control_status.flags.ev_yaw) {
fuseHeading();
}
}
}
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);
// save a copy of the quaternion state for later use in calculating the amount of reset change
Quaternion quat_before_reset = _state.quat_nominal;
// calculate initial quaternion states for the ekf
_state.quat_nominal = Quaternion(euler_init);
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
// add the reset amount to the output observer buffered data
outputSample output_states;
unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= _state_reset_status.quat_change;
_output_buffer.push_to_index(i,output_states);
}
// capture the reset event
_state_reset_status.quat_counter++;
// 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");
}
}
}
bool Ekf::initialiseFilter(void)
{
// Keep accumulating measurements until we have a minimum of 10 samples for the baro and magnetoemter
// Sum the IMU delta angle measurements
imuSample imu_init = _imu_buffer.get_newest();
_delVel_sum += imu_init.delta_vel;
// Sum the magnetometer measurements
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
if (_mag_counter == 0) {
_mag_filt_state = _mag_sample_delayed.mag;
}
_mag_counter ++;
_mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f;
}
// set the default height source from the adjustable parameter
if (_hgt_counter == 0) {
_primary_hgt_source = _params.vdist_sensor_type;
}
if (_primary_hgt_source == VDIST_SENSOR_RANGE) {
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
if (_hgt_counter == 0) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
_hgt_filt_state = _range_sample_delayed.rng;
}
_hgt_counter ++;
_hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _range_sample_delayed.rng;
}
} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) {
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
if (_hgt_counter == 0) {
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_hgt_filt_state = _baro_sample_delayed.hgt;
}
_hgt_counter ++;
_hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _baro_sample_delayed.hgt;
}
} else {
return false;
}
// check to see if we have enough measurements and return false if not
if (_hgt_counter <= 10 || _mag_counter <= 10) {
return false;
} else {
// reset variables that are shared with post alignment GPS checks
_gps_drift_velD = 0.0f;
_gps_alt_ref = 0.0f;
// Zero all of the states
_state.ang_error.setZero();
_state.vel.setZero();
_state.pos.setZero();
_state.gyro_bias.setZero();
_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
_state.accel_z_bias = 0.0f;
_state.mag_I.setZero();
_state.mag_B.setZero();
_state.wind_vel.setZero();
// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
float pitch = 0.0f;
float roll = 0.0f;
if (_delVel_sum.norm() > 0.001f) {
_delVel_sum.normalize();
pitch = asinf(_delVel_sum(0));
roll = atan2f(-_delVel_sum(1), -_delVel_sum(2));
} else {
return false;
}
// calculate initial tilt alignment
matrix::Euler<float> euler_init(roll, pitch, 0.0f);
_state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
// initialise the filtered alignment error estimate to a larger starting value
_tilt_err_length_filt = 1.0f;
// calculate the averaged magnetometer reading
Vector3f mag_init = _mag_filt_state;
// calculate the initial magnetic field and yaw alignment
resetMagHeading(mag_init);
// calculate the averaged height reading to calulate the height of the origin
_hgt_sensor_offset = _hgt_filt_state;
// if we are not using the baro height as the primary source, then calculate an offset relative to the origin
// so it can be used as a backup
if (!_control_status.flags.baro_hgt) {
baroSample baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt - _hgt_sensor_offset;
} else {
_baro_hgt_offset = 0.0f;
}
// initialise the state covariance matrix
initialiseCovariance();
// initialise the terrain estimator
initHagl();
return true;
}
}
