自学教程：C++ using_EKF函数代码示例

// return the smoothed gyro vector corrected for driftconst Vector3f &AP_AHRS_NavEKF::get_gyro(void) const{    if (!using_EKF()) {        return AP_AHRS_DCM::get_gyro();    }    return _gyro_estimate;}

// blended accelerometer values in the earth frame in m/s/sconst Vector3f &AP_AHRS_NavEKF::get_accel_ef_blended(void) const{    if(!using_EKF()) {        return AP_AHRS_DCM::get_accel_ef_blended();    }    return _accel_ef_ekf_blended;}

// dead-reckoning supportbool AP_AHRS_NavEKF::get_position(struct Location &loc){    if (using_EKF() && EKF.getLLH(loc)) {        return true;    }    return AP_AHRS_DCM::get_position(loc);}

// return a relative ground position in meters/second, North/East/Down// order. Must only be called if have_inertial_nav() is truebool AP_AHRS_NavEKF::get_relative_position_NED(Vector3f &vec) const{    if (using_EKF()) {        return EKF.getPosNED(vec);    }    return false;}

// accelerometer values in the earth frame in m/s/sconst Vector3f &AP_AHRS_NavEKF::get_accel_ef(uint8_t i) const{    if(!using_EKF()) {        return AP_AHRS_DCM::get_accel_ef(i);    }    return _accel_ef_ekf[i];}

const Matrix3f &AP_AHRS_NavEKF::get_dcm_matrix(void) const{    if (!using_EKF()) {        return AP_AHRS_DCM::get_dcm_matrix();    }    return _dcm_matrix;}

const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const{    if (!using_EKF()) {        return AP_AHRS_DCM::get_gyro_drift();    }    return _gyro_bias;}

// true if compass is being usedbool AP_AHRS_NavEKF::use_compass(void){    if (using_EKF()) {        return EKF.use_compass();    }    return AP_AHRS_DCM::use_compass();}

// return a ground velocity in meters/second, North/East/Down// order. Must only be called if have_inertial_nav() is truebool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const{    if (using_EKF()) {        EKF.getVelNED(vec);        return true;    }    return false;}

// return a wind estimation vector, in m/sVector3f AP_AHRS_NavEKF::wind_estimate(void){    if (!using_EKF()) {        return AP_AHRS_DCM::wind_estimate();    }    Vector3f wind;    EKF.getWind(wind);    return wind;}

// EKF has a better ground speed vector estimateVector2f AP_AHRS_NavEKF::groundspeed_vector(void){    if (!using_EKF()) {        return AP_AHRS_DCM::groundspeed_vector();    }    Vector3f vec;    EKF.getVelNED(vec);    return Vector2f(vec.x, vec.y);}

// dead-reckoning supportbool AP_AHRS_NavEKF::get_position(struct Location &loc) const{    Vector3f ned_pos;    if (using_EKF() && EKF.getLLH(loc) && EKF.getPosNED(ned_pos)) {        // fixup altitude using relative position from AHRS home, not        // EKF origin        loc.alt = get_home().alt - ned_pos.z*100;        return true;    }    return AP_AHRS_DCM::get_position(loc);}

float AP_AHRS_NavEKF::get_yaw_for_control_cd(void) const{    if (!using_EKF()) {        return AP_AHRS_DCM::get_yaw_for_control_cd();    }    float ret = degrees(attitude_for_control.get_euler_yaw())*100.0f;    if (ret < 0) {        ret += 36000;    }    return ret;}

// return secondary attitude solution if available, as eulers in radiansbool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers){    if (using_EKF()) {        // return DCM attitude        eulers = _dcm_attitude;        return true;    }    if (ekf_started) {        // EKF is secondary        EKF.getEulerAngles(eulers);        return true;    }    // no secondary available    return false;}

// return secondary position solution if availablebool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc){    if (using_EKF()) {        // return DCM position        AP_AHRS_DCM::get_position(loc);        return true;    }        if (ekf_started) {        // EKF is secondary        EKF.getLLH(loc);        return true;    }    // no secondary available    return false;}

/*  check if the AHRS subsystem is healthy*/bool AP_AHRS_NavEKF::healthy(void) const{    // If EKF is started we switch away if it reports unhealthy. This could be due to bad    // sensor data. If EKF reversion is inhibited, we only switch across if the EKF encounters    // an internal processing error, but not for bad sensor data.    if (_ekf_use != EKF_DO_NOT_USE) {        bool ret = ekf_started && EKF.healthy();        if (!ret) {            return false;        }        if ((_vehicle_class == AHRS_VEHICLE_FIXED_WING ||             _vehicle_class == AHRS_VEHICLE_GROUND) &&            !using_EKF()) {            // on fixed wing we want to be using EKF to be considered            // healthy if EKF is enabled            return false;        }        return true;    }    return AP_AHRS_DCM::healthy();    }

void AP_AHRS_NavEKF::update(void){    AP_AHRS_DCM::update();    // keep DCM attitude available for get_secondary_attitude()    _dcm_attitude(roll, pitch, yaw);    if (!ekf_started) {        // if we have a compass set and GPS lock we can start the EKF        if (get_compass() && get_gps()->status() >= GPS::GPS_OK_FIX_3D) {            if (start_time_ms == 0) {                start_time_ms = hal.scheduler->millis();            }            if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) {                ekf_started = true;                EKF.InitialiseFilterDynamic();            }        }    }    if (ekf_started) {        EKF.UpdateFilter();        EKF.getRotationBodyToNED(_dcm_matrix);        if (using_EKF()) {            Vector3f eulers;            EKF.getEulerAngles(eulers);            roll  = eulers.x;            pitch = eulers.y;            yaw   = eulers.z;            roll_sensor  = degrees(roll) * 100;            pitch_sensor = degrees(pitch) * 100;            yaw_sensor   = degrees(yaw) * 100;            if (yaw_sensor < 0)                yaw_sensor += 36000;            update_trig();        }    }}

void AP_AHRS_NavEKF::update(void){    // we need to restore the old DCM attitude values as these are    // used internally in DCM to calculate error values for gyro drift    // correction    roll = _dcm_attitude.x;    pitch = _dcm_attitude.y;    yaw = _dcm_attitude.z;    update_cd_values();    AP_AHRS_DCM::update();    // keep DCM attitude available for get_secondary_attitude()    _dcm_attitude(roll, pitch, yaw);    if (!ekf_started) {        // wait 10 seconds        if (start_time_ms == 0) {            start_time_ms = hal.scheduler->millis();        }        if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) {            ekf_started = true;            EKF.InitialiseFilterDynamic();        }    }    if (ekf_started) {        EKF.UpdateFilter();        EKF.getRotationBodyToNED(_dcm_matrix);        if (using_EKF()) {            Vector3f eulers;            EKF.getEulerAngles(eulers);            roll  = eulers.x;            pitch = eulers.y;            yaw   = eulers.z;            update_cd_values();            update_trig();            // keep _gyro_bias for get_gyro_drift()            EKF.getGyroBias(_gyro_bias);            _gyro_bias = -_gyro_bias;            // calculate corrected gryo estimate for get_gyro()            _gyro_estimate.zero();            uint8_t healthy_count = 0;                for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {                if (_ins.get_gyro_health(i)) {                    _gyro_estimate += _ins.get_gyro(i);                    healthy_count++;                }            }            if (healthy_count > 1) {                _gyro_estimate /= healthy_count;            }            _gyro_estimate += _gyro_bias;            // update _accel_ef_ekf            for (uint8_t i=0; i<_ins.get_accel_count(); i++) {                if (_ins.get_accel_health(i)) {                    _accel_ef_ekf[i] = _dcm_matrix * _ins.get_accel(i);                }            }            // update _accel_ef_ekf_blended            EKF.getAccelNED(_accel_ef_ekf_blended);        }    }}

void AP_AHRS_NavEKF::update(void){    // we need to restore the old DCM attitude values as these are    // used internally in DCM to calculate error values for gyro drift    // correction    roll = _dcm_attitude.x;    pitch = _dcm_attitude.y;    yaw = _dcm_attitude.z;    update_cd_values();    AP_AHRS_DCM::update();    // keep DCM attitude available for get_secondary_attitude()    _dcm_attitude(roll, pitch, yaw);    if (!ekf_started) {        // wait 1 second for DCM to output a valid tilt error estimate        if (start_time_ms == 0) {            start_time_ms = hal.scheduler->millis();        }        if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) {            ekf_started = EKF.InitialiseFilterDynamic();        }    }    if (ekf_started) {        EKF.UpdateFilter();        EKF.getRotationBodyToNED(_dcm_matrix);        if (using_EKF()) {            Vector3f eulers;            EKF.getEulerAngles(eulers);            roll  = eulers.x;            pitch = eulers.y;            yaw   = eulers.z;            update_cd_values();            update_trig();            // keep _gyro_bias for get_gyro_drift()            EKF.getGyroBias(_gyro_bias);            _gyro_bias = -_gyro_bias;            // calculate corrected gryo estimate for get_gyro()            _gyro_estimate.zero();            uint8_t healthy_count = 0;            for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {                if (_ins.get_gyro_health(i)) {                    _gyro_estimate += _ins.get_gyro(i);                    healthy_count++;                }            }            if (healthy_count > 1) {                _gyro_estimate /= healthy_count;            }            _gyro_estimate += _gyro_bias;            float abias1, abias2;            EKF.getAccelZBias(abias1, abias2);            // update _accel_ef_ekf            for (uint8_t i=0; i<_ins.get_accel_count(); i++) {                Vector3f accel = _ins.get_accel(i);                if (i==0) {                    accel.z -= abias1;                } else if (i==1) {                    accel.z -= abias2;                }                if (_ins.get_accel_health(i)) {                    _accel_ef_ekf[i] = _dcm_matrix * accel;                }            }            if(_ins.get_accel_health(0) && _ins.get_accel_health(1)) {                float IMU1_weighting;                EKF.getIMU1Weighting(IMU1_weighting);                _accel_ef_ekf_blended = _accel_ef_ekf[0] * IMU1_weighting + _accel_ef_ekf[1] * (1.0f-IMU1_weighting);            } else {                _accel_ef_ekf_blended = _accel_ef_ekf[0];            }        }    }}

// return true if inertial navigation is activebool AP_AHRS_NavEKF::have_inertial_nav(void) const {    return using_EKF();}