首页 > 编程语言 >ArduSub程序学习(11)--EKF实现逻辑①

ArduSub程序学习(11)--EKF实现逻辑①

时间:2024-09-26 17:22:59浏览次数:11  
标签:11 DCM -- EKF update AHRS EKF2 gyro

1.read_AHRS()

进入EKF,路径ArduSub.cpp里面的fast_loop()里面的read_AHRS();

//从 AHRS(姿态与航向参考系统)中读取并更新与飞行器姿态有关的信息
void Sub::read_AHRS()
{
    // Perform IMU calculations and get attitude info
    //-----------------------------------------------
    // <true> tells AHRS to skip INS update as we have already done it in fast_loop()
    //告诉 AHRS 跳过惯性导航系统(INS)的更新,因为 INS 的更新已经在主循环的快速循环(fast_loop())中完成了。
    ahrs.update(true);
    ahrs_view.update(true);
}

2.update(ture)

负责更新AHRS的各个组件,并管理不同类型的扩展卡尔曼滤波器(EKF)

//通过EKF,读取姿态信息
void AP_AHRS_NavEKF::update(bool skip_ins_update)
{
    // support locked access functions to AHRS data
    //信号量锁定
    WITH_SEMAPHORE(_rsem);
    
    // drop back to normal priority if we were boosted by the INS
    // calling delay_microseconds_boost()
    //在完成传感器更新后,恢复调度器的正常优先级,之前可能使用过临时提升。
    hal.scheduler->boost_end();

    //更新DCM
    update_DCM(skip_ins_update);

    //针对SITL的条件编译
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    update_SITL();
#endif

    //如果外部AHRS启用,则调用外部更新函数。
#if HAL_EXTERNAL_AHRS_ENABLED
    update_external();
#endif

    //hal.console->printf("Current EKF Type: %d\n", static_cast<int>(_ekf_type.get()));

    //EKF选择和执行
    if (_ekf_type == 2) {
        // if EK2 is primary then run EKF2 first to give it CPU
        // priority
#if HAL_NAVEKF2_AVAILABLE
        update_EKF2();
#endif
#if HAL_NAVEKF3_AVAILABLE
        update_EKF3();
#endif
    } else {
        // otherwise run EKF3 first
#if HAL_NAVEKF3_AVAILABLE
        update_EKF3();
#endif
#if HAL_NAVEKF2_AVAILABLE
        update_EKF2();
#endif
    }

#if AP_MODULE_SUPPORTED
    // call AHRS_update hook if any
    //如果支持任何模块,调用钩子以允许它们执行与AHRS相关的附加更新。
    AP_Module::call_hook_AHRS_update(*this);
#endif

    // push gyros if optical flow present
    //如果光流传感器可用,获取陀螺仪漂移(偏差)并用此信息更新光流系统。
    if (hal.opticalflow) {
        const Vector3f &exported_gyro_bias = get_gyro_drift();
        hal.opticalflow->push_gyro_bias(exported_gyro_bias.x, exported_gyro_bias.y);
    }

    if (_view != nullptr) {
        // update optional alternative attitude view
        //如果存在可选的替代视图,则更新该视图的姿态。
        _view->update(skip_ins_update);
    }

#if !HAL_MINIMIZE_FEATURES && AP_AHRS_NAVEKF_AVAILABLE
    // update NMEA output
    //如果未最小化功能且EKF可用,更新NMEA输出,这是用于海洋导航的标准。
    update_nmea_out();
#endif

    //检查当前活动EKF类型是否自上次更新以来发生变化
    EKFType active = active_EKF_type();
    //如果发生变化,则向地面控制站(GCS)发送关于当前活动EKF类型的消息。
    if (active != last_active_ekf_type) {
        last_active_ekf_type = active;
        const char *shortname = "???";
        switch ((EKFType)active) {
        case EKFType::NONE:
            shortname = "DCM";
            break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        case EKFType::SITL:
            shortname = "SITL";
            break;
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
        case EKFType::EXTERNAL:
            shortname = "External";
            break;
#endif
#if HAL_NAVEKF3_AVAILABLE
        case EKFType::THREE:
            shortname = "EKF3";
            break;
#endif
#if HAL_NAVEKF2_AVAILABLE
        case EKFType::TWO:
            shortname = "EKF2";
            break;
#endif
        }
        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AHRS: %s active", shortname);
    }
}

 2.1update_DCM(bool skip_ins_update)

        更新DCM(方向余弦矩阵,Direction Cosine Matrix)的作用是保持和更新飞行器或自主车辆的姿态信息,使得其在三维空间中的朝向能够持续精确地被估计和修正。

        陀螺仪在长时间运行时会产生漂移误差。更新DCM时,可以利用传感器(如加速度计、磁力计等)的测量值,计算与当前DCM表示的姿态之间的误差,并据此调整陀螺仪读数。通过这种方式,DCM能够起到校正漂移的作用,确保姿态估计的准确性。

//更新姿态和航向参考系统(AHRS)中的方向余弦矩阵(DCM)。DCM是一种用来表示三维空间中物体姿态的数学模型。
void AP_AHRS_NavEKF::update_DCM(bool skip_ins_update)
{
    // we need to restore the old DCM attitude values as these are
    // used internally in DCM to calculate error values for gyro drift
    // correction
    //恢复旧的DCM姿态值:
    roll = _dcm_attitude.x;
    pitch = _dcm_attitude.y;
    yaw = _dcm_attitude.z;
    //更新DCM的内部数据,确保当前姿态和其他相关值同步。
    update_cd_values();

    AP_AHRS_DCM::update(skip_ins_update);

    // keep DCM attitude available for get_secondary_attitude()
    //将更新后的 roll、pitch 和 yaw 值重新赋值回 _dcm_attitude
    _dcm_attitude = {roll, pitch, yaw};
}

        ★★在使用EKF进行姿态估计的系统中,DCM通常作为备用机制存在。当EKF失效或者不稳定时,系统可以回退到DCM来提供可靠的姿态估计。通过不断更新DCM,系统可以确保即使在EKF不可用的情况下,仍然有一个稳定的姿态估计可用。 

if (_ekf_type == 2) {
    update_EKF2();
    update_EKF3();
} else {
    update_EKF3();
    update_EKF2();
}
update_DCM(skip_ins_update);
  • EKF2和EKF3 是不同的卡尔曼滤波器版本,它们可能分别处理不同的数据源或者计算策略。系统根据具体情况优先更新某个EKF,并依次更新另一个EKF。
  • DCM的更新 则在EKF更新后进行,确保即便EKF更新失败或不稳定,系统仍能通过DCM获得姿态数据。

        在一些导航系统中,DCM可能作为EKF的初始估计工具。由于DCM计算效率高,姿态估计在短期内较为准确,EKF可以使用DCM的估计值作为其初始状态,逐渐根据传感器数据和预测模型进行更复杂的修正。

        如果EKF在某些情况下出现问题(如传感器数据丢失、不可靠的传感器输入等),系统可以回退到DCM,继续保持姿态估计的基本功能。因此,DCM作为EKF的一种补充或者冗余备份,能够增强系统的鲁棒性。

2.2update_EKF2();

 这段代码实现了AP_AHRS_NavEKF::update_EKF2()函数,用于更新飞行器导航系统中使用的扩展卡尔曼滤波器2(EKF2)的状态。该函数主要负责初始化EKF2滤波器并在之后的每个周期内更新滤波器状态,获取姿态估计、陀螺仪漂移修正、加速度计数据等。

//更新飞行器导航系统中使用的扩展卡尔曼滤波器2(EKF2)的状态
void AP_AHRS_NavEKF::update_EKF2(void)
{
    //hal.console->printf("_ekf2_started=%d\r\n",_ekf2_started);
   
    if (!_ekf2_started) {
        // wait 1 second for DCM to output a valid tilt error estimate
        //hal.console->printf("AAAAAA1\r\n");
         //等待1秒以便DCM输出有效的倾斜误差估计
        if (start_time_ms == 0) {
            start_time_ms = AP_HAL::millis();
        }
        // if we're doing Replay logging then don't allow any data
        // into the EKF yet.  Don't allow it to block us for long.
        // 检查看门狗是否重置并确保不长时间阻塞数据
        if (!hal.util->was_watchdog_reset()) {
            if (AP_HAL::millis() - start_time_ms < 5000) {
                //hal.console->printf("AAAAAA2\r\n");
                if (!AP::logger().allow_start_ekf()) {
                    return;
                }
            }
        }
         等待预设的启动延迟时间,然后初始化EKF2
        if (AP_HAL::millis() - start_time_ms > startup_delay_ms) {
            _ekf2_started = EKF2.InitialiseFilter();
        }
    }
    if (_ekf2_started) {
        //hal.console->printf("BBBBB1\r\n");
        //EKF2更新,融合传感器数据来更新姿态、速度和位置估计。
        EKF2.UpdateFilter();
        //hal.console->printf("BBBBB2\r\n");
        if (active_EKF_type() == EKFType::TWO) {
            //hal.console->printf("CCCCC1\r\n");
            Vector3f eulers;
            //矩阵更新
            EKF2.getRotationBodyToNED(_dcm_matrix);
            //姿态获取
            EKF2.getEulerAngles(-1,eulers);
            roll  = eulers.x;
            pitch = eulers.y;
            yaw   = eulers.z;

            update_cd_values();
            update_trig();

            // Use the primary EKF to select the primary gyro
            //陀螺仪漂移修正
            const int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex();

            const AP_InertialSensor &_ins = AP::ins();

            // get gyro bias for primary EKF and change sign to give gyro drift
            // Note sign convention used by EKF is bias = measurement - truth
            //计算修正后的陀螺仪数据
            _gyro_drift.zero();
            EKF2.getGyroBias(-1,_gyro_drift);
            _gyro_drift = -_gyro_drift;

            // calculate corrected gyro estimate for get_gyro()
            _gyro_estimate.zero();
            if (primary_imu == -1 || !_ins.get_gyro_health(primary_imu)) {
                // the primary IMU is undefined so use an uncorrected default value from the INS library
                _gyro_estimate = _ins.get_gyro();
            } else {
                // use the same IMU as the primary EKF and correct for gyro drift
                _gyro_estimate = _ins.get_gyro(primary_imu) + _gyro_drift;
            }

            // get z accel bias estimate from active EKF (this is usually for the primary IMU)
            // 加速度计数据的处理与校正
            float abias = 0;
            EKF2.getAccelZBias(-1,abias);

            // This EKF is currently using primary_imu, and abias applies to only that IMU
            for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
                Vector3f accel = _ins.get_accel(i);
                if (i == primary_imu) {
                    accel.z -= abias;
                }
                if (_ins.get_accel_health(i)) {
                    _accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
                }
            }
            _accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()];
            //滤波器状态更新
            nav_filter_status filt_state;
            EKF2.getFilterStatus(-1,filt_state);
            AP_Notify::flags.gps_fusion = filt_state.flags.using_gps; // Drives AP_Notify flag for usable GPS.
            AP_Notify::flags.gps_glitching = filt_state.flags.gps_glitching;
            AP_Notify::flags.have_pos_abs = filt_state.flags.horiz_pos_abs;
        }
    }
}

 

  • 初始化EKF2:函数首先检查EKF2是否启动,并在适当时机进行初始化。
  • 姿态和传感器数据更新:在EKF2成功启动后,获取姿态数据并修正陀螺仪和加速度计的漂移,确保姿态估计准确。
  • 滤波器状态和融合:不断更新EKF2的状态,结合传感器数据(如GPS)进行位置和姿态融合,确保系统导航的可靠性。

2.3 update_EKF3();

AP_AHRS_NavEKF::update_EKF3() 函数实现了扩展卡尔曼滤波器3(EKF3)的更新流程。它与 update_EKF2() 类似,但专门用于 EKF3 的处理。主要功能包括初始化滤波器、更新姿态数据、陀螺仪漂移修正、加速度计偏置校正等。

未完待续~~

标签:11,DCM,--,EKF,update,AHRS,EKF2,gyro
From: https://blog.csdn.net/weixin_49839886/article/details/142551540

相关文章

  • Word2vec的应用
    目录1.分词2.模型训练 3.可视化 4.知识点个人理解pipinstallgensim-ihttps://pypi.tuna.tsinghua.edu.cn/simple#若在jupyternotebook中安装:!pipinstallgensim-ihttps://pypi.tuna.tsinghua.edu.cn/simple#导包importjiebaimportreimportnumpya......
  • 无人机的作战指挥中心-地面站!
    无人机与地面站的关系指挥与控制:地面站是无人机系统的核心控制部分,负责对无人机进行远程指挥和控制。无人机根据地面站下达的任务自主完成飞行任务,并实时向地面站反馈飞行状态和任务执行情况。任务规划与执行:地面站具备任务规划与执行功能,可以根据实际需求规划无人机的飞行路......
  • UniApp组件与微信小程序组件对照学习
    UniApp只是一个第三方的开发工具,借鉴各种平台的能力,UniApp的组件也借鉴了微信小程序的组件,我们学习时,可以进行对照学习,我们在用UniApp开发微信小程序时,UniApp也只是将代码转成了微信小程序的代码,还是需要了解微信小程序开发,才能开发出微信小程序的。下面我们来进行对应学习1......
  • 基于Django的农业害虫识别系统设计与实现
    文章目录前言一、详细操作演示视频二、具体实现截图三、技术栈1.前端-Vue.js2.后端-SpringBoot3.数据库-MySQL4.系统架构-B/S四、系统测试1.系统测试概述2.系统功能测试3.系统测试结论五、项目代码参考六、数据库代码参考七、项目论文示例结语前言......
  • 算法记录——链表
    2.链表2.1判断是否是回文链表1.方法一:利用栈反转链表/***Definitionforsingly-linkedlist.*publicclassListNode{*intval;*ListNodenext;*ListNode(){}*ListNode(intval){this.val=val;}*ListNode(intval,Lis......
  • 【leetcode】2. 两数相加
      总体思路:1.将两个链表里的数字相加:总左往右加,存入第三方链表L3里;2.设置一个进位符t,用来存储每位相加的进位信息;3.对多出来单独的链表进行处理(只需考虑进位),接入到L3的后面。/***Definitionforsingly-linkedlist.*structListNode{*intval;*s......
  • python字符串
    1定义字符串text="Hello,World!"2多行字符串multi_line_text="""Thisisamulti-linestring."""3 字符串拼接greeting="Hello"name="Alice"message=greeting+","+name+"!"4......
  • 用io跟信号实现售票
    一、实现目标1)售票员捕捉SIGINT(代表开车)信号,向司机发送SIGUSR1信号,司机打印(let'sgogogo)2)售票员捕捉SIGQUIT(代表停车)信号,向司机发送SIGUSR2信号,司机打印(stopthebus)3)司机捕捉SIGTSTP(代表到达终点站)信号,向售票员发送SIGUSR1信号,售票员打印(pleasegetoffthebus)4)司机等待......
  • (开题)flask框架宠物上门服务系统(程序+论文+python)
    本系统(程序+源码+数据库+调试部署+开发环境)带论文文档1万字以上,文末可获取,系统界面在最后面。系统程序文件列表开题报告内容研究背景在快节奏的现代生活中,宠物已成为许多家庭的重要成员,它们不仅是忠诚的伴侣,更是情感的寄托。然而,随着工作压力的增加和生活方式的转变,许多宠......
  • (开题)flask框架宠物医院管理系统(程序+论文+python)
    本系统(程序+源码+数据库+调试部署+开发环境)带论文文档1万字以上,文末可获取,系统界面在最后面。系统程序文件列表开题报告内容研究背景随着宠物饲养的普及和宠物主人对宠物健康关注度的提升,宠物医疗行业迎来了前所未有的发展机遇。然而,传统的宠物医院管理模式在应对日益增长......