void Ekf::predictState()
{
if (!_earth_rate_initialised) {
if (_gps_initialised) {
calcEarthRateNED(_earth_rate_NED, _posRef.lat_rad);
_earth_rate_initialised = true;
}
}
// attitude error state prediciton
matrix::Dcm<float> R_to_earth(_state.quat_nominal); // transformation matrix from body to world frame
Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_prev * _earth_rate_NED *
_imu_sample_delayed.delta_ang_dt;
Quaternion dq; // delta quaternion since last update
dq.from_axis_angle(corrected_delta_ang);
_state.quat_nominal = dq * _state.quat_nominal;
_state.quat_nominal.normalize();
_R_prev = R_to_earth.transpose();
Vector3f vel_last = _state.vel;
// predict velocity states
_state.vel += R_to_earth * _imu_sample_delayed.delta_vel;
_state.vel(2) += 9.81f * _imu_sample_delayed.delta_vel_dt;
// predict position states via trapezoidal integration of velocity
_state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f;
constrainStates();
}
// 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;
}
void Ekf::calculateOutputStates()
{
imuSample imu_new = _imu_sample_new;
Vector3f delta_angle;
// Note: We do no not need to consider any bias or scale correction here
// since the base class has already corrected the imu sample
delta_angle(0) = imu_new.delta_ang(0);
delta_angle(1) = imu_new.delta_ang(1);
delta_angle(2) = imu_new.delta_ang(2);
Vector3f delta_vel = imu_new.delta_vel;
delta_angle += _delta_angle_corr;
Quaternion dq;
dq.from_axis_angle(delta_angle);
_output_new.time_us = imu_new.time_us;
_output_new.quat_nominal = dq * _output_new.quat_nominal;
_output_new.quat_nominal.normalize();
matrix::Dcm<float> R_to_earth(_output_new.quat_nominal);
Vector3f delta_vel_NED = R_to_earth * delta_vel + _delta_vel_corr;
delta_vel_NED(2) += 9.81f * imu_new.delta_vel_dt;
Vector3f vel_last = _output_new.vel;
_output_new.vel += delta_vel_NED;
_output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f) + _vel_corr * imu_new.delta_vel_dt;
if (_imu_updated) {
_output_buffer.push(_output_new);
_imu_updated = false;
}
_output_sample_delayed = _output_buffer.get_oldest();
Quaternion quat_inv = _state.quat_nominal.inversed();
Quaternion q_error = _output_sample_delayed.quat_nominal * quat_inv;
q_error.normalize();
Vector3f delta_ang_error;
float scalar;
if (q_error(0) >= 0.0f) {
scalar = -2.0f;
} else {
scalar = 2.0f;
}
delta_ang_error(0) = scalar * q_error(1);
delta_ang_error(1) = scalar * q_error(2);
delta_ang_error(2) = scalar * q_error(3);
_delta_angle_corr = delta_ang_error * imu_new.delta_ang_dt;
_delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt;
_vel_corr = (_state.pos - _output_sample_delayed.pos);
}
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;
}
// Reset heading and magnetic field states
bool Ekf::resetMagHeading(Vector3f &mag_init)
{
// 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 the variance for the rotation estimate expressed as a rotation vector
// this will be used later to reset the quaternion state covariances
Vector3f angle_err_var_vec = calcRotVecVariances();
// update transformation matrix from body to world frame using the current estimate
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// calculate the initial quaternion
// determine if a 321 or 312 Euler sequence is best
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
// use a 321 sequence
// rotate the magnetometer measurement into earth frame
matrix::Euler<float> euler321(_state.quat_nominal);
// Set the yaw angle to zero and calculate the rotation matrix from body to earth frame
euler321(2) = 0.0f;
matrix::Dcm<float> R_to_earth(euler321);
// calculate the observed yaw angle
if (_params.fusion_mode & MASK_USE_EVYAW) {
// 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
euler321(2) = atan2f(R_to_earth_ev(1, 0) , R_to_earth_ev(0, 0));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
// the angle of the projection onto the horizontal gives the yaw angle
euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
} else {
// there is no yaw observation
return false;
}
// calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps
_state.quat_nominal = Quaternion(euler321);
} else {
// use a 312 sequence
// Calculate the 312 sequence euler angles that rotate from earth to body frame
// See http://www.atacolorado.com/eulersequences.doc
Vector3f euler312;
euler312(0) = atan2f(-_R_to_earth(0, 1) , _R_to_earth(1, 1)); // first rotation (yaw)
euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
euler312(2) = atan2f(-_R_to_earth(2, 0) , _R_to_earth(2, 2)); // third rotation (pitch)
// Set the first rotation (yaw) to zero and calculate the rotation matrix from body to earth frame
euler312(0) = 0.0f;
// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
float c2 = cosf(euler312(2));
float s2 = sinf(euler312(2));
float s1 = sinf(euler312(1));
float c1 = cosf(euler312(1));
float s0 = sinf(euler312(0));
float c0 = cosf(euler312(0));
matrix::Dcm<float> R_to_earth;
R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
R_to_earth(1, 1) = c0 * c1;
R_to_earth(2, 2) = c2 * c1;
R_to_earth(0, 1) = -c1 * s0;
R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
R_to_earth(2, 0) = -s2 * c1;
R_to_earth(2, 1) = s1;
// calculate the observed yaw angle
if (_params.fusion_mode & MASK_USE_EVYAW) {
// 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
euler312(0) = atan2f(-R_to_earth_ev(0, 1) , R_to_earth_ev(1, 1));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
// the angle of the projection onto the horizontal gives the yaw angle
euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
} else {
// there is no yaw observation
return false;
}
// re-calculate the rotation matrix using the updated yaw angle
s0 = sinf(euler312(0));
c0 = cosf(euler312(0));
R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
R_to_earth(1, 1) = c0 * c1;
R_to_earth(2, 2) = c2 * c1;
R_to_earth(0, 1) = -c1 * s0;
R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
R_to_earth(2, 0) = -s2 * c1;
R_to_earth(2, 1) = s1;
// calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps
_state.quat_nominal = Quaternion(R_to_earth);
}
// update transformation matrix from body to world frame using the current estimate
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// update the yaw angle variance using the variance of the measurement
if (_params.fusion_mode & MASK_USE_EVYAW) {
// using error estimate from external vision data
angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// using magnetic heading tuning parameter
angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
}
// reset the quaternion covariances using the rotation vector variances
initialiseQuatCovariances(angle_err_var_vec);
// calculate initial earth magnetic field states
_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);
}
// 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);
}
// apply the change in attitude quaternion to our newest quaternion estimate
// which was already taken out from the output buffer
_output_new.quat_nominal *= _state_reset_status.quat_change;
// capture the reset event
_state_reset_status.quat_counter++;
return true;
}
