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();
}
// Set the NED origin to be used until the next filter reset
void NavEKF2_core::setOrigin()
{
// assume origin at current GPS location (no averaging)
EKF_origin = _ahrs->get_gps().location();
// define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
validOrigin = true;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u Origin set to GPS",(unsigned)imu_index);
}
// Set the NED origin to be used until the next filter reset
void NavEKF2_core::setOrigin()
{
// assume origin at current GPS location (no averaging)
EKF_origin = _ahrs->get_gps().location();
// define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
validOrigin = true;
hal.console->printf("EKF Origin Set\n");
}
// set the LLH location of the filters NED origin
bool NavEKF2_core::setOriginLLH(struct Location &loc)
{
if (PV_AidingMode == AID_ABSOLUTE) {
return false;
}
EKF_origin = loc;
// define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
validOrigin = true;
return true;
}
// set the LLH location of the filters NED origin
bool NavEKF3_core::setOriginLLH(const Location &loc)
{
if (PV_AidingMode == AID_ABSOLUTE) {
return false;
}
EKF_origin = loc;
ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
// define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
validOrigin = true;
return true;
}
// Set the NED origin to be used until the next filter reset
void NavEKF3_core::setOrigin()
{
// assume origin at current GPS location (no averaging)
EKF_origin = _ahrs->get_gps().location();
// if flying, correct for height change from takeoff so that the origin is at field elevation
if (inFlight) {
EKF_origin.alt += (int32_t)(100.0f * stateStruct.position.z);
}
ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
// define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
validOrigin = true;
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u Origin set to GPS",(unsigned)imu_index);
}
