//! Get heading rate value.
//! @return heading rate.
inline double
getHeadingRate(void)
{
double pitch = getEuler(AXIS_Y);
// Avoid division by zero.
if (!std::cos(pitch))
return 0;
double roll = getEuler(AXIS_X);
double p, q, r;
if (m_sum_euler_inc)
{
if (!m_edelta_readings)
return 0.0;
// Make sure the following corresponds to angular velocity in all IMUs.
p = getEulerDelta(AXIS_X) / getEulerDeltaTimestep();
q = getEulerDelta(AXIS_Y) / getEulerDeltaTimestep();
r = getEulerDelta(AXIS_Z) / getEulerDeltaTimestep();
}
else
{
p = getAngularVelocity(AXIS_X);
q = getAngularVelocity(AXIS_Y);
r = getAngularVelocity(AXIS_Z);
}
extractEarthRotation(p, q, r);
return (std::sin(roll) * q + std::cos(roll) * r) / std::cos(pitch);
}
void FreeIMU1::getEuler(float * angles) {
int i;
double dAngles[3];
getEuler(dAngles);
for (i = 0; i < 3; i++)
angles[i] = (float)dAngles[i];
}
//! Routine to clear euler angles buffer.
//! @param[in] filter filter value.
inline void
updateEuler(float filter)
{
for (unsigned i = 0; i < 3; ++i)
m_euler_bfr[i] = getEuler(i) * filter;
m_euler_readings = filter;
}
void
task(void)
{
if(!BasicNavigation::isActive())
return;
// Compute time delta.
double tstep = getTimeStep();
// Check if we have a valid time delta.
if (tstep < 0)
return;
m_heading += Angles::minSignedAngle(m_heading, Angles::normalizeRadian(getEuler(AXIS_Z)));
m_estate.psi = Angles::normalizeRadian(m_heading);
m_estate.r = getAngularVelocity(AXIS_Z);
// Update estimated state.
onDispatchNavigation();
// Some (experimental) sanity checks. This is not standard EKF!
// If any of this conditions is met, some kind of warning should be raised.
double wx = Math::trimValue(m_kal.getState(STATE_WX), -m_args.max_current, m_args.max_current);
double wy = Math::trimValue(m_kal.getState(STATE_WY), -m_args.max_current, m_args.max_current);
m_kal.setState(STATE_WX, wx);
m_kal.setState(STATE_WY, wy);
if (m_kal.getCovariance(STATE_WX) > std::pow(m_args.max_current, 2))
m_kal.setProcessNoise(STATE_WX, 0);
else
m_kal.setProcessNoise(STATE_WX, m_process_noise[1] * tstep);
if (m_kal.getCovariance(STATE_WY) > std::pow(m_args.max_current, 2))
m_kal.setProcessNoise(STATE_WY, 0);
else
m_kal.setProcessNoise(STATE_WY, m_process_noise[1] * tstep);
// Reset and discretize transition matrix function.
Matrix a(NUM_STATE, NUM_STATE, 0.0);
a(STATE_X, STATE_K) = m_rpm * std::cos(m_estate.theta) * std::cos(m_estate.psi);
a(STATE_Y, STATE_K) = m_rpm * std::cos(m_estate.theta) * std::sin(m_estate.psi);
a(STATE_X, STATE_WX) = 1.0;
a(STATE_Y, STATE_WY) = 1.0;
m_kal.setTransitions((a * tstep).expmts());
// Run EKF prediction.
m_kal.predict();
checkUncertainty();
// Fill and dispatch data.
m_estate.u = m_rpm * m_kal.getState(STATE_K) * std::cos(m_estate.theta);
// Consider water speed to get ground speed
m_estate.vx += m_kal.getState(STATE_WX);
m_estate.vy += m_kal.getState(STATE_WY);
// Log Navigation Uncertainty.
m_uncertainty.u = m_kal.getCovariance(STATE_K);
// Log Navigation Data.
m_navdata.custom_x = m_kal.getCovariance(STATE_WX);
m_navdata.custom_y = m_kal.getCovariance(STATE_WY);
m_navdata.custom_z = m_kal.getState(STATE_K);
m_navdata.cog = std::atan2(m_estate.vy, m_estate.vx);
// Fill and dispatch estimated water velocity message.
m_ewvel.x = m_kal.getState(STATE_WX);
m_ewvel.y = m_kal.getState(STATE_WY);
m_ewvel.z = 0;
reportToBus();
updateBuffers(c_wma_filter);
}
