void Estimator::run()
{
float acc_kp, ki;
uint64_t now_us = RF_.sensors_.data().imu_time;
if (last_time_ == 0)
{
last_time_ = now_us;
last_acc_update_us_ = last_time_;
return;
}
else if (now_us < last_time_)
{
// this shouldn't happen
RF_.state_manager_.set_error(StateManager::ERROR_TIME_GOING_BACKWARDS);
last_time_ = now_us;
return;
}
RF_.state_manager_.clear_error(StateManager::ERROR_TIME_GOING_BACKWARDS);
float dt = (now_us - last_time_) * 1e-6f;
last_time_ = now_us;
state_.timestamp_us = now_us;
// Crank up the gains for the first few seconds for quick convergence
if (now_us < static_cast<uint64_t>(RF_.params_.get_param_int(PARAM_INIT_TIME))*1000)
{
acc_kp = RF_.params_.get_param_float(PARAM_FILTER_KP)*10.0f;
ki = RF_.params_.get_param_float(PARAM_FILTER_KI)*10.0f;
}
else
{
acc_kp = RF_.params_.get_param_float(PARAM_FILTER_KP);
ki = RF_.params_.get_param_float(PARAM_FILTER_KI);
}
// Run LPF to reject a lot of noise
run_LPF();
// add in accelerometer
float a_sqrd_norm = accel_LPF_.sqrd_norm();
turbomath::Vector w_acc;
if (RF_.params_.get_param_int(PARAM_FILTER_USE_ACC)
&& a_sqrd_norm < 1.1f*1.1f*9.80665f*9.80665f && a_sqrd_norm > 0.9f*0.9f*9.80665f*9.80665f)
{
// Get error estimated by accelerometer measurement
last_acc_update_us_ = now_us;
// turn measurement into a unit vector
turbomath::Vector a = accel_LPF_.normalized();
// Get the quaternion from accelerometer (low-frequency measure q)
// (Not in either paper)
turbomath::Quaternion q_acc_inv(g_, a);
// Get the error quaternion between observer and low-freq q
// Below Eq. 45 Mahony Paper
turbomath::Quaternion q_tilde = q_acc_inv * state_.attitude;
// Correction Term of Eq. 47a and 47b Mahony Paper
// w_acc = 2*s_tilde*v_tilde
w_acc.x = -2.0f*q_tilde.w*q_tilde.x;
w_acc.y = -2.0f*q_tilde.w*q_tilde.y;
w_acc.z = 0.0f; // Don't correct z, because it's unobservable from the accelerometer
// integrate biases from accelerometer feedback
// (eq 47b Mahony Paper, using correction term w_acc found above
bias_.x -= ki*w_acc.x*dt;
bias_.y -= ki*w_acc.y*dt;
bias_.z = 0.0; // Don't integrate z bias, because it's unobservable
}
else
{
w_acc.x = 0.0f;
w_acc.y = 0.0f;
w_acc.z = 0.0f;
}
if (attitude_correction_next_run_)
{
attitude_correction_next_run_ = false;
w_acc += RF_.params_.get_param_float(PARAM_FILTER_KP_ATT_CORRECTION)*(q_correction_ - state_.attitude);
}
// Handle Gyro Measurements
turbomath::Vector wbar;
if (RF_.params_.get_param_int(PARAM_FILTER_USE_QUAD_INT))
{
// Quadratic Interpolation (Eq. 14 Casey Paper)
// this step adds 12 us on the STM32F10x chips
wbar = (w2_/-12.0f) + w1_*(8.0f/12.0f) + gyro_LPF_ * (5.0f/12.0f);
w2_ = w1_;
w1_ = gyro_LPF_;
}
else
{
wbar = gyro_LPF_;
}
// Build the composite omega vector for kinematic propagation
// This the stuff inside the p function in eq. 47a - Mahony Paper
turbomath::Vector wfinal = wbar - bias_ + w_acc * acc_kp;
// Propagate Dynamics (only if we've moved)
float sqrd_norm_w = wfinal.sqrd_norm();
if (sqrd_norm_w > 0.0f)
{
float p = wfinal.x;
float q = wfinal.y;
float r = wfinal.z;
if (RF_.params_.get_param_int(PARAM_FILTER_USE_MAT_EXP))
{
// Matrix Exponential Approximation (From Attitude Representation and Kinematic
// Propagation for Low-Cost UAVs by Robert T. Casey)
// (Eq. 12 Casey Paper)
// This adds 90 us on STM32F10x chips
float norm_w = sqrtf(sqrd_norm_w);
turbomath::Quaternion qhat_np1;
float t1 = cosf((norm_w*dt)/2.0f);
float t2 = 1.0f/norm_w * sinf((norm_w*dt)/2.0f);
qhat_np1.w = t1*state_.attitude.w + t2*(-p*state_.attitude.x - q*state_.attitude.y - r*state_.attitude.z);
qhat_np1.x = t1*state_.attitude.x + t2*( p*state_.attitude.w + r*state_.attitude.y - q*state_.attitude.z);
qhat_np1.y = t1*state_.attitude.y + t2*( q*state_.attitude.w - r*state_.attitude.x + p*state_.attitude.z);
qhat_np1.z = t1*state_.attitude.z + t2*( r*state_.attitude.w + q*state_.attitude.x - p*state_.attitude.y);
state_.attitude = qhat_np1.normalize();
}
else
{
// Euler Integration
// (Eq. 47a Mahony Paper), but this is pretty straight-forward
turbomath::Quaternion qdot(0.5f * (-p*state_.attitude.x - q*state_.attitude.y - r*state_.attitude.z),
0.5f * ( p*state_.attitude.w + r*state_.attitude.y - q*state_.attitude.z),
0.5f * ( q*state_.attitude.w - r*state_.attitude.x + p*state_.attitude.z),
0.5f * ( r*state_.attitude.w + q*state_.attitude.x - p*state_.attitude.y));
state_.attitude.w += qdot.w*dt;
state_.attitude.x += qdot.x*dt;
state_.attitude.y += qdot.y*dt;
state_.attitude.z += qdot.z*dt;
state_.attitude.normalize();
}
}
// Extract Euler Angles for controller
state_.attitude.get_RPY(&state_.roll, &state_.pitch, &state_.yaw);
// Save off adjust gyro measurements with estimated biases for control
state_.angular_velocity = gyro_LPF_ - bias_;
// If it has been more than 0.5 seconds since the acc update ran and we are supposed to be getting them
// then trigger an unhealthy estimator error
if (RF_.params_.get_param_int(PARAM_FILTER_USE_ACC) && now_us > 500000 + last_acc_update_us_
&& !RF_.params_.get_param_int(PARAM_FIXED_WING))
{
RF_.state_manager_.set_error(StateManager::ERROR_UNHEALTHY_ESTIMATOR);
}
else
{
RF_.state_manager_.clear_error(StateManager::ERROR_UNHEALTHY_ESTIMATOR);
}
}
void Estimator::run_estimator(const vector_t& accel, const vector_t& gyro, const uint64_t& imu_time)
{
_current_state.now_us = imu_time;
if (last_time == 0 || _current_state.now_us <= last_time)
{
last_time = _current_state.now_us;
return;
}
float dt = (_current_state.now_us - last_time) * 1e-6f;
last_time = _current_state.now_us;
// Crank up the gains for the first few seconds for quick convergence
if (imu_time < (uint64_t)params->get_param_int(Params::PARAM_INIT_TIME)*1000)
{
kp = params->get_param_float(Params::PARAM_FILTER_KP)*10.0f;
ki = params->get_param_float(Params::PARAM_FILTER_KI)*10.0f;
}
else
{
kp = params->get_param_float(Params::PARAM_FILTER_KP);
ki = params->get_param_float(Params::PARAM_FILTER_KI);
}
// Run LPF to reject a lot of noise
run_LPF(accel, gyro);
// add in accelerometer
float a_sqrd_norm = _accel_LPF.x*_accel_LPF.x + _accel_LPF.y*_accel_LPF.y + _accel_LPF.z*_accel_LPF.z;
if (use_acc && a_sqrd_norm < 1.15f*1.15f*9.80665f*9.80665f && a_sqrd_norm > 0.85f*0.85f*9.80665f*9.80665f)
{
// Get error estimated by accelerometer measurement
vector_t a = vector_normalize(_accel_LPF);
// Get the quaternion from accelerometer (low-frequency measure q)
// (Not in either paper)
quaternion_t q_acc_inv = quaternion_inverse(quat_from_two_vectors(a, g));
// Get the error quaternion between observer and low-freq q
// Below Eq. 45 Mahoney Paper
q_tilde = quaternion_multiply(q_acc_inv, q_hat);
// Correction Term of Eq. 47a and 47b Mahoney Paper
// w_acc = 2*s_tilde*v_tilde
w_acc.x = -2.0f*q_tilde.w*q_tilde.x;
w_acc.y = -2.0f*q_tilde.w*q_tilde.y;
w_acc.z = 0.0f; // Don't correct z, because it's unobservable from the accelerometer
// integrate biases from accelerometer feedback
// (eq 47b Mahoney Paper, using correction term w_acc found above)
b.x -= ki*w_acc.x*dt;
b.y -= ki*w_acc.y*dt;
b.z = 0.0; // Don't integrate z bias, because it's unobservable
}
else
{
w_acc.x = 0.0f;
w_acc.y = 0.0f;
w_acc.z = 0.0f;
}
// Pull out Gyro measurements
if (quad_int)
{
// Quadratic Integration (Eq. 14 Casey Paper)
// this integration step adds 12 us on the STM32F10x chips
wbar = vector_add(vector_add(scalar_multiply(-1.0f/12.0f,w2), scalar_multiply(8.0f/12.0f,w1)),
scalar_multiply(5.0f/12.0f,_gyro_LPF));
w2 = w1;
w1 = _gyro_LPF;
}
else
{
wbar = _gyro_LPF;
}
// Build the composite omega vector for kinematic propagation
// This the stuff inside the p function in eq. 47a - Mahoney Paper
wfinal = vector_add(vector_sub(wbar, b), scalar_multiply(kp, w_acc));
// Propagate Dynamics (only if we've moved)
float sqrd_norm_w = sqrd_norm(wfinal);
if (sqrd_norm_w > 0.0f)
{
float p = wfinal.x;
float q = wfinal.y;
float r = wfinal.z;
if (mat_exp)
{
// Matrix Exponential Approximation (From Attitude Representation and Kinematic
// Propagation for Low-Cost UAVs by Robert T. Casey)
// (Eq. 12 Casey Paper)
// This adds 90 us on STM32F10x chips
float norm_w = sqrtf(sqrd_norm_w);
quaternion_t qhat_np1;
float t1 = cosf((norm_w*dt)/2.0f);
float t2 = 1.0f/norm_w * sinf((norm_w*dt)/2.0f);
qhat_np1.w = t1*q_hat.w + t2*( - p*q_hat.x - q*q_hat.y - r*q_hat.z);
qhat_np1.x = t1*q_hat.x + t2*(p*q_hat.w + r*q_hat.y - q*q_hat.z);
qhat_np1.y = t1*q_hat.y + t2*(q*q_hat.w - r*q_hat.x + p*q_hat.z);
qhat_np1.z = t1*q_hat.z + t2*(r*q_hat.w + q*q_hat.x - p*q_hat.y);
q_hat = quaternion_normalize(qhat_np1);
}
else
{
// Euler Integration
// (Eq. 47a Mahoney Paper), but this is pretty straight-forward
quaternion_t qdot = {0.5f * ( - p*q_hat.x - q*q_hat.y - r*q_hat.z),
0.5f * ( p*q_hat.w + r*q_hat.y - q*q_hat.z),
0.5f * ( q*q_hat.w - r*q_hat.x + p*q_hat.z),
0.5f * ( r*q_hat.w + q*q_hat.x - p*q_hat.y)
};
q_hat.w += qdot.w*dt;
q_hat.x += qdot.x*dt;
q_hat.y += qdot.y*dt;
q_hat.z += qdot.z*dt;
q_hat = quaternion_normalize(q_hat);
}
}
// Save attitude estimate
_current_state.q = q_hat;
// Extract Euler Angles for controller
euler_from_quat(_current_state.q, &_current_state.euler.x, &_current_state.euler.y, &_current_state.euler.z);
// Save off adjust gyro measurements with estimated biases for control
_current_state.omega = vector_sub(_gyro_LPF, b);
}
