ardupiolt AP_AHRS库类的分析（一）AP_AHRS_NavEKF

65 阅读 0 评论 43 点赞

我是靠谱客的博主迷你书包，最近开发中收集的这篇文章主要介绍ardupiolt AP_AHRS库类的分析（一）AP_AHRS_NavEKF，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

class AP_AHRS_NavEKF : public AP_AHRS_DCM 
public:
    enum Flags {
        FLAG_NONE = 0,
        FLAG_ALWAYS_USE_EKF = 0x1,
    };

//构造函数
AP_AHRS_NavEKF(NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags = FLAG_NONE);

//不允许调用的函数
AP_AHRS_NavEKF(const AP_AHRS_NavEKF &other) = delete;
AP_AHRS_NavEKF &operator=(const AP_AHRS_NavEKF&) = delete;

//返回校正了漂移的平滑的陀螺仪数据矢量
const Vector3f &get_gyro(void) const override;
const Matrix3f &get_rotation_body_to_ned(void) const override;

//返回当前的漂移校正积分值
const Vector3f &get_gyro_drift(void) const override

//复位当前的积分漂移值，如果陀螺仪漂移值需要重新计算，则应该调用
void reset_gyro_drift() override;

void update(bool skip_ins_update = false) override;
void reset(bool recover_eulers = false) override;

//复位当前的姿态角，使用新的IMU校正值
void reset_attitude(const float &roll, const float &pitch, const float &yaw) override;

//dead-reckoning support
bool get_position (struct Location &loc) const override;

//获得最新的海拔高度估计值和有效性标志
bool get_hagl(float &hagl) const override;

//估计错误的状态报告
float get_error_rp() const override;
float get_error_yaw() const override;

//返回风速的估计矢量，单位为m/s
Vector3f wind_estimate(float *airspeed_ret) const override;

//如果磁罗盘被使用，则为true
bool use_compass() override;

//我们需要删除它们以完全隐藏我们正在使用的EKF
NavEKF2 &get_NavEKF2(void){
return EKF2;
}

const NavEKF2 &get_NavEKF2_const(void) const {
    return EKF2;
}

NavEKF3 &get_NavEKF3(void){
    return EKF3;
}

const NavEKF3 &get_NavEKF3_const(void) const{
    return EKF3;
}

//返回辅助姿态解决方案（如果有），欧拉角以弧度表示
bool get_secondary_attitude(Vector3f &eulers) const override;

//返回辅助姿态解决方案（如果有），以四元数表示
bool get_secondary_quaternion(Quaternion &quat) const override;

//返回辅助位置解决方案（如果有）
bool get_secondary_position(struct Location &loc) const override;

//EKF有一个更好的地面速度矢量估计
Vector2f groundspeed_vector() override;

const Vector3f &get_accel_ef(uint8_t) const override;
const Vector3f &get_accel_ef() const override;

//检索惯性导航使用的校正NED增量速度
void getCorrectedDeltaVelocityNED(Vector3f& ret; float& dt) const const override;

//大地坐标系下混合加速度计值 m/s/s
const Vector3f &get_accel_ef_blended() const override;

//将EKF的原始位置设置为10e7度。只有当EKF没有绝对的位置参考（GPS）时才调用，来确定它自己的原始位置
bool set_origin(const Location &loc) override;

//    以lat / lon / alt返回惯性导航原点

bool get_origin(Location & ret) const override;

bool have_inertial_nav() const override;

bool get_velocity_NED(Vector3f &vec) const override;

//将相关的位置NED返回到 home 或者 origin
//如果估计是有效的，则返回为真

bool get_relative_position_NED_home(Vector3f &vec) const override;
bool get_relative_position _NED_origin(Vector &vec) const override;

//将相关的位置NE返回到home 或者 origin
//如果估计是有效的，则返回为真

bool get_relative_position_NE_home(Vector2f &posNE) const override;
bool get_relative_position_NE_origin(Vector2f &posNE) const override

//将相关的位置D返回到home 或者 origin
//如果EKF不是，则baro将用于_home相对位置

void get_relative_position_D_home(float &posD) const override;
bool get_relative_position_D_origin(float &posD) const override;

//获取以m / s为单位的垂直位置的导数，该导数在运动学上与某些控制循环所需的垂直位置一致。
//这与EKF的垂直速度不同，由于各种错误已被校正，因此EKF并不总是与垂直位置一致

bool get_vert_pos_rate(float &velocity) const;

//将光流测量值写入EKF
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset);

//将里程表数据写入EKF
void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset)

//从外部导航系统写入位置和四元数数据
void writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms) override;

//禁止使用GPS
uint8_t setInhibitGPS(void);

//获得速度限制
 void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;

 void set_ekf_use(bool setting);

//AHRS 子系统是否健康
bool healthy() const override;

//如果AHRS已完成初始化，则为true
bool initialised() const override;

//得到滤波器状态---返回滤波器状态作为一系列标志
bool get_filter_status(nav_filter_status &status) const;

//得到磁罗盘漂移的估计值
//如果漂移值有效则为真
bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const;

//报告后端拒绝初始化的任何原因
const char *prearm_failure_reason(void) const override;


//返回由于最后一次偏航角重置而产生的偏航角变化量，以弧度为单位
//返回上一次偏航角复位的时间；如果未发生过复位，则返回0
 uint32_t getLastYawResetAngle(float &yawAng) const override;

//返回由于上次复位而导致的NE位置变化量（以米为单位）
//返回最后一次重置的时间；如果没有发生重置，则返回0
uint32_t getLastPosNorthEastReset(Vector2f &pos) const override;

//返回由于上次重置而导致的NE速度变化量，单位为米/秒
//返回上次重置的时间；如果没有发生重置，则返回0
uint32_t getLastVelNorthEastReset(Vector2f &vel) const override;

//返回由于最近一次重置而导致的垂直位置变化量（以米为单位）
//返回上次重置的时间；如果没有发生重置，则返回0
uint32_t getLastPosDownReset(float &posDelta) const override;

//重置baro，使其在当前高度读数为零
//将EKF高度重置为零
//调整EKf原点高度，使EKF高度+原点高度与之前相同
//如果已经执行了高度基准重置，则返回true
//如果将测距仪用于高度，则不执行任何重置，并返回false
bool resetHeightDatum() override;

//发送当前EKF的EKF_STATUS_REPORT
void send_ekf_status_report(mavlink_channel_t chan) const;

// get_hgt_ctrl_limit-获取以米为单位的控制循环和有效性标志所要观测的最大高度
//用于在光流导航期间限制高度
//不需要限制时将返回无效
bool get_hgt_ctrl_limit(float &limit) const override;

// get_location-用最新的信息更新提供的位置
//计算的位置，包括绝对高度
//成功返回true（即EKF知道它是最新的）
//位置），失败则为false
 bool get_location(struct Location &loc) const

// get_variances-提供使用新息方差归一化的新息，其中值为0
//表示测量结果与EKF解决方案之间的完美一致性，最大值为1
//滤波器会接受的不一致
//如果方差不可用，则返回boolean false
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const override

//返回期望的NED磁场
bool get_mag_field_NED(Vector3f& ret) const;

//返回估计的在体坐标系下的磁场偏移量
bool get_mag_field_correction(Vector3f &ret) const override;

void setTakeoffExpected(bool val);
void setTouchdownExpected(bool val);

bool getGpsGlitchStatus() const;

//由Reply用于强制在正确的时间戳开始
void force_ekf_start(void) { _force_ekf = true; }

// EKF后端是否在执行自己的传感器日志记录？
bool have_ekf_logging(void) const override;

//获取当前主加速度传感器的索引
uint8_t get_primary_accel_index(void) const override;

//获取当前主陀螺仪传感器的索引
uint8_t get_primary_gyro_index(void) const override;

private：
    enum EKF_TYPE {EKF_TYPE_NONE=0,
                   EKF_TYPE3=3,
                   EKF_TYPE2=2
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
                   ,EKF_TYPE_SITL=10
#endif
    };
EKF_TYPE active_EKF_type(void) const;

bool always_use_EKF() const {
     return _ekf_flags & FLAG_ALWAYS_USE_EKF;
}

    NavEKF2 &EKF2;
    NavEKF3 &EKF3;
    bool _ekf2_started;
    bool _ekf3_started;
    bool _force_ekf;
    Matrix3f _dcm_matrix;
    Vector3f _dcm_attitude;
    Vector3f _gyro_drift;
    Vector3f _gyro_estimate;
    Vector3f _accel_ef_ekf[INS_MAX_INSTANCES];
    Vector3f _accel_ef_ekf_blended;
    const uint16_t startup_delay_ms = 1000;
    uint32_t start_time_ms = 0;
    Flags _ekf_flags;

    uint8_t ekf_type(void) const;
    void update_DCM(bool skip_ins_update);
    void update_EKF2(void);
    void update_EKF3(void);

//获得当前主IMU的索引
 uint8_t get_primary_IMU_index(void) const

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    SITL::SITL *_sitl;
    uint32_t _last_body_odm_update_ms = 0;
    void update_SITL(void);
#endif    
};
#endif