精华内容
参与话题
问答
  • 互补滤波算法

    2013-11-28 14:30:59
    关于互补滤波算法的英文版的文献,主要讲述互补滤波算法的公式的推导和一些基本的概念
  • 前言:之前搜了很多关于互补滤波的文章,但基本上都是互相转载,内容也都是一些个人的理解,存在很多细节的东西。而这篇博文就很权威一点,因为里面很多都是weiji百科里的。转载:...

    前言:之前搜了很多关于互补滤波的文章,但基本上都是互相转载,内容也都是一些个人的理解,存在很多细节的东西。而这篇博文就很权威一点,因为里面很多都是weiji百科里的。


    转载:http://blog.csdn.net/qing101hua/article/details/53029121

    亮点:关于校正部分很符合源码的格式
    给出了 各个修正量的值、某些参数的含义
    主程序流程图
    很有自己的特色,原创性很高

    展开全文
  • Mahony 互补滤波算法

    万次阅读 2017-06-21 08:59:20
     ... 上接【互补滤波器】,继续学习互补滤波。。。。 参考: Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs. PX4/Pixhawk—uORB深入理解和
    

    http://blog.csdn.net/luoshi006/article/details/51513580

    上接【互补滤波器】,继续学习互补滤波。。。。

    参考:
    Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs.
    PX4/Pixhawk—uORB深入理解和应用


    应用场景

    本文中 mahony 的应用场景为 多旋翼无人机姿态估计


    mc

    陀螺仪、加速度计、MPU6050 详述,请参考:传送门

    名词解释

    陀螺仪

    陀螺仪,测量角速度,具有高动态特性,它是一个间接测量角度的器件。它测量的是角度的导数,即角速度,要将角速度对时间积分才能得到角度。由于噪声等误差影响,在积分作用下不断积累,最终导致陀螺仪的低频干扰和漂移。

    加速度计

    输出当前加速度(包含重力加速度 g )的方向【也叫重力感应器】,在悬停时,输出为 g 。由其测量原理导致的高频信号敏感,使得加速度计在振动环境中高频干扰较大。

    磁力计

    输出为当前机体与地磁场的夹角。测量原理与指南针相似。低频特性较好,易受周围磁场干扰。
    磁力计的工作原理参考:WalkAnt的博客

    坐标系

    导航坐标系:在多旋翼中,又叫地球坐标系、地理坐标系。通常,采用北东地(NED)构成坐标系的 X,Y,Z  轴。

    机体坐标系 :固联在多旋翼飞行器上,即,坐标系原点定位于飞行器中心点(假设中心点与重心点重合)。

    关于航空导航用到的坐标系,请参考 AIAA 系列丛书。在多旋翼中,因为只在低空飞行,且时间较短,只需要以上两个。

    姿态表示

    欧拉角 :较直观,描述刚体在三维欧式空间中的姿态。此时,坐标系为机体坐标系,随着刚体的旋转而旋转。缺点:万向节锁。
    详情参加:Wikipedia 传送门

    四元数:由一组四维向量,表示刚体的三维旋转。适合用于计算机计算。
    详情参加:Wikipedia 传送门

    方向余弦矩阵DCM:用欧拉角余弦值或四元数,表示的坐标系的旋转。

    mahony 滤波原理

    互补滤波要求两个信号的干扰噪声处在不同的频率,通过设置两个滤波器的截止频率,确保融合后的信号能够覆盖需求频率。
    在 IMU 的姿态估计中,互补滤波器对陀螺仪(低频噪声)使用高通滤波;对加速度/磁力计(高频噪声)使用低频滤波。
    (此处尚未对传感器数据实测,噪声和有用频率未知。。。。待后期补足)

    互补滤波中,两个滤波器的截止频率一致,此时就需要根据有用频率的值对其进行取舍。

    机体水平时,加速度计无法测量绕 Z 轴的旋转量,即偏航角。磁力计也有同样问题,无法测得要磁轴的旋转量。故,需要加速度计和磁力计同时对陀螺仪进行校正。

    mahony 滤波主要过程

    q ^  ˙ =12 q ^ P(Ω ¯ ¯ ¯  +δ)(a) 

    δ=k P e+k I e(b) 

    e=v ¯ ×v ^ (c) 

    式中,q ^   表示系统姿态估计的四元数表示; δ  是经过 PI 调节器产生的新息; e  表示实测的惯性向量 v ¯   和预测的向量 v ^   之间的相对旋转(误差)。
    P()  —— pure quaternion operator(四元数实部为0),表示只有旋转。


    PI 调节器中:[20160901更新]

    • 参数 k p   用于控制加速度计和陀螺仪之间的交叉频率
    • 参数 k I   用于校正陀螺仪误差

    预备知识

    主要是公式,不包含推导过程。。。。

    欧拉角与机体角速度的关系:

    Θ ˙ =W b ω 

    =⎡ ⎣ ⎢ 100 tanθsinϕcosϕsinϕ/cosθ tanθcosϕsinϕcosϕ/cosθ ⎤ ⎦ ⎥  b ω 

    旋转矩阵与机体角速度的关系:

    R e b  ˙ =R e b [ b ω] ×  

    四元数与机体角速度的关系

    q ˙  b e (t)=12 ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ 0ω x ω y ω z  ω x 0ω z ω y  ω y ω z 0ω x  ω z ω y ω x 0 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ q b e (t)1 

    参考:北航全权老师课件 第五章内容、惯性导航(秦永元)第九章内容。

    预测

    与卡尔曼滤波相似,mahony 滤波也分为预测-校正。
    在预测环节,由三轴陀螺仪测得的角速度,通过式(1)计算出四元数姿态预测。q b e   表示从地球坐标系到机体坐标系,或机体坐标系姿态在地球坐标系下的表示。

    q b e (k)=q b e (k1)+q ˙  b e (k)Δt 

    校正

    在预测环节得到的四元数 q b e (k)  ,通过加速度计和磁力计的值进行校正。该环节通常分为两部分:

    1. 通过加速度计得到 Δq acc  ˆ   ,然后校正四元数中的横滚(roll)和俯仰(pitch)分量。
    2. 当磁力计可读时,通过 Δq mag  ˆ   校正四元数中的偏航(yaw)分量。

    加速度计校正

    加速度计信号首先经过低通滤波器(消除高频噪声):

    y(k)=RCT+RC y(k1)+TT+RC x(k) 

    然后,对得到的结果进行归一化(normalized)

    Δq acc  ˆ =Δq acc  ¯ ¯ ¯ ¯ ¯ ¯ ¯ ¯  ||Δq acc  ¯ ¯ ¯ ¯ ¯ ¯ ¯ ¯  ||  

    计算偏差:

    e=Δq acc  ˆ ×v 

    式中, v  表示重力向量在机体坐标系的向量。

    ⎡ ⎣ ⎢ v x v y v z  ⎤ ⎦ ⎥ =C b n ⎡ ⎣ ⎢ 001 ⎤ ⎦ ⎥  

    =⎡ ⎣ ⎢ 2(q 1 q 3 q 0 q 2 )2(q 2 q 3 +q 0 q 1 )q 2 0 q 2 1 q 2 2 +q 2 3  ⎤ ⎦ ⎥  

    此时,由 v  与加速度计向量垂直分量叉乘,得到误差值。【两个物理意义相同的向量,理论上叉乘为零】

    磁力计校正

    数据预处理与加速度计相同,先滤波,然后归一化,得到Δq mag  ˆ  

    1. 无 GPS 校准时:

    计算误差:

    e=Δq mag  ˆ ×w 

    式中,w  计算过程如下:

    磁力计的输出(m )在机体坐标系下,将其转换到导航坐标系:

    ⎡ ⎣ ⎢ h x h y h z  ⎤ ⎦ ⎥ =C n b ⎡ ⎣ ⎢ m x m y m z  ⎤ ⎦ ⎥  

    导航坐标系的 x  轴与正北对齐,故,可以将磁力计在 xoy  平面的投影折算到 x  轴。

    b x =h 2 x +h 2 y  − − − − − −    

    b z =h z  

    再次变换到机体坐标系:

    ⎡ ⎣ ⎢ w x w y w z  ⎤ ⎦ ⎥ =C b n ⎡ ⎣ ⎢ b x 0b z  ⎤ ⎦ ⎥  

    2. 有 GPS 校准时:

    在 px4 中,磁力计使用 GPS 信息 [0,0,mag]  进行校准,故,公式与加速度计相同。

    ⎡ ⎣ ⎢ w x w y w z  ⎤ ⎦ ⎥ =C b n ⎡ ⎣ ⎢ 00mag ⎤ ⎦ ⎥  

    此时,由 w  与磁力计向量叉乘,得到误差值。【两个物理意义相同的向量,理论上叉乘为零】

    更新四元数

    由加速度计和磁力计校准得到的误差值:

    e=e acc +e mag  

    由该误差值得到修正值:

    δ=K P e+K I edt 

    修正后的角速度值:

    ω=ω gyro +δ 

    根据一阶龙格库塔方法求解一阶微分方程:

    q ˙ =f(q,ω) 

    q(t+T)=q(t)+Tf(q,ω) 

    可以求出四元数微分方程的差分形式:

    q 0 (t+T)=q 0 (t)+T2 [ω x q 1 (t)ω y q 2 (t)ω z q 3 (t)] 

    四元数规范化:

    q=q 0 +q 1 i+q 2 j+q 3 kq 2 0 +q 2 1 +q 2 2 +q 2 3  − − − − − − − − − − − − − −     

    源码分析

    该部分源码直接截取 px4 开源飞控源码(BSD证书)。
    px4 为 pixhawk 飞控的固件代码,内部涉及很多滤波及导航的算法。有较大参考价值。

    源码,参考日期:20160603;

    https://github.com/hsteinhaus/PX4Firmware/blob/master/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

    https://github.com/ArduPilot/PX4Firmware/blob/master/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

    参数 默认值  
    ATT_W_ACC 0.2f 加速度计权重
    ATT_W_MAG 0.1f 磁力计权重
    ATT_W_EXT_HDG 0.1f 外部航向权重
    ATT_W_GYRO_BIAS 0.1f 陀螺仪偏差权重
    ATT_MAG_DECL 0.0f 磁偏角(°)
    ATT_MAG_DECL_A 1 启用基于GPS的自动磁偏角校正
    ATT_EXT_HDG_M 0 外部航向模式
    ATT_ACC_COMP 1 启用基于GPS速度的加速度补偿
    ATT_BIAS_MAX 0.05f 陀螺仪偏差上限
    ATT_VIBE_THRESH 0.2f 振动水平报警阈值

    主程序运行流程图:

    这里写图片描述

    函数功能简述

    1. AttitudeEstimatorQ::AttitudeEstimatorQ();

      构造函数,初始化参数;

    2. AttitudeEstimatorQ::~AttitudeEstimatorQ();

      析构函数,杀掉所有任务;

    3. int AttitudeEstimatorQ::start();

      启动【attitude_estimator_q】进程,主函数入口: task_main_trampoline

    4. void AttitudeEstimatorQ::print();

      打印姿态信息;

    5. void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])

      {

      attitude_estimator_q::instance->task_main();

      }

    6. void AttitudeEstimatorQ::task_main()

      主任务进程;

    7. void AttitudeEstimatorQ::update_parameters(bool force);

      false: 查看参数是否更新;

      true: 获取新参数, 并由磁偏角更新四元数;

    8. bool AttitudeEstimatorQ::init();

      由加速度计和磁力计初始化旋转矩阵,有GPS时,校正磁偏角。

    9. bool AttitudeEstimatorQ::update(float dt);

      调用init(); 互补滤波

    10. void AttitudeEstimatorQ::update_mag_declination(float new_declination);

      使用磁偏角更新四元数

    11. int attitude_estimator_q_main(int argc, char *argv[]);

      attitude_estimator_q { start }:实例化instance,运行instance->start();

      attitude_estimator_q { stop }:delete instance,指针置空;

      attitude_estimator_q { status}:instance->print(),打印姿态信息。


    源码分析

    • 此处源码逐行分析,可以使用 Ctrl+f 快速定位;
    • uORB 相关的数据结构,请参考 px4/Firmware/msg;
    /*代码前的注释段(L34~L40)
     * @file attitude_estimator_q_main.cpp
     *
     * Attitude estimator (quaternion based)
     *姿态估计(基于四元数)
     * @author Anton Babushkin <anton.babushkin@me.com>
     */
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7

    头文件

    (l42~l76)
    #include <px4_config.h>
    #include <px4_posix.h>//add missing check;
    #include <unistd.h>
    #include <stdlib.h>
    #include <stdio.h>
    #include <stdbool.h>
    #include <poll.h>
    #include <fcntl.h>
    #include <float.h>
    #include <errno.h>
    #include <limits.h>
    #include <math.h>
    #include <uORB/uORB.h>
    #include <uORB/topics/sensor_combined.h>
    #include <uORB/topics/vehicle_attitude.h>
    #include <uORB/topics/control_state.h>
    #include <uORB/topics/vehicle_control_mode.h>
    #include <uORB/topics/vehicle_global_position.h>
    #include <uORB/topics/vision_position_estimate.h>//视觉位置估计, 未找到文件【待查】;
    #include <uORB/topics/att_pos_mocap.h>//mocap-->vicon;
    #include <uORB/topics/airspeed.h>
    #include <uORB/topics/parameter_update.h>
    #include <uORB/topics/estimator_status.h>
    #include <drivers/drv_hrt.h>
    
    #include <mathlib/mathlib.h>
    #include <mathlib/math/filter/LowPassFilter2p.hpp>
    #include <lib/geo/geo.h>
    #include <lib/ecl/validation/data_validator_group.h>
    
    #include <systemlib/systemlib.h>
    #include <systemlib/param/param.h>
    #include <systemlib/perf_counter.h>
    #include <systemlib/err.h>
    #include <systemlib/mavlink_log.h>
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36

    using @@@

    (l78~l82)
    extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]);
    
    using math::Vector;
    using math::Matrix;
    using math::Quaternion;
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6

    此处,extern “C” 表示以 C 格式编译; __EXPORT 表示 将函数名输出到链接器(Linker); using 关键字 表示引入名称到 using 说明出现的声明区域。。

    __export
    This keyword aids those programming Microsoft Windows. __export causes the function name to be exported to the linker.

    namespace attitude_estimator_q

    (l84~l89)
    class AttitudeEstimatorQ;
    
    namespace attitude_estimator_q
    {
    AttitudeEstimatorQ *instance;
    }//定义命名空间,通过命名空间调用instance;
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7

    类定义: class AttitudeEstimatorQ

          (l92~l210)
    class AttitudeEstimatorQ
    {//类定义; 
    public:
        AttitudeEstimatorQ();
        //构造函数
        ~AttitudeEstimatorQ();
        //析构函数
         int        start();
        //开始任务,成功--返回OK;
        static void task_main_trampoline(int argc, char *argv[]);
        //跳转到 task_main() ,未使用传入参数;static函数只能被本文件中的函数调用;
        void        task_main();
        void        print();
    private:
        static constexpr float _dt_max = 0.02;//最大时间间隔;
        //constexpr(constant expression)常数表达式,c11新关键字;
        //优化语法检查和编译速度;
        bool        _task_should_exit = false;
        //如果为 true ,任务退出;
        int     _control_task = -1;
        //进程ID, 默认-1表示没有任务;
        int     _sensors_sub = -1;//sensor_combined subscribe(订阅);
        int     _params_sub = -1;//parameter_update subscribe;
        int     _vision_sub = -1;//视觉位置估计;
        int     _mocap_sub = -1;//vicon姿态位置估计;
        int     _airspeed_sub = -1;//airspeed subscribe;
        int     _global_pos_sub = -1;//vehicle_global_position subscribe;
        orb_advert_t    _att_pub = nullptr;//vehicle_attitude publish(发布);
        orb_advert_t    _ctrl_state_pub = nullptr;//发布控制状态主题control_state;
        orb_advert_t    _est_state_pub = nullptr;//estimator_status
    
        struct {
            param_t w_acc;//ATT_W_ACC
            param_t w_mag;//ATT_W_MAG
            param_t w_ext_hdg;//ATT_W_EXT_HDG 外部航向权重;
            param_t w_gyro_bias;//ATT_W_GYRO_BIAS
            param_t mag_decl;//ATT_MAG_DECL
            param_t mag_decl_auto;//ATT_MAG_DECL_A 磁偏角自动校正;
            param_t acc_comp;//ATT_ACC_COMP
            param_t bias_max;//ATT_BIAS_MAX 陀螺仪偏差上限;
            param_t vibe_thresh;//ATT_VIBE_THRESH 振动报警阈值;
            param_t ext_hdg_mode;//ATT_EXT_HDG_M 外部航向模式;
        }       _params_handles;
        //有用参数的句柄;
    
        float       _w_accel = 0.0f;
        //ATT_W_ACC >>> w_acc >>> _w_accel;
        float       _w_mag = 0.0f;
        float       _w_ext_hdg = 0.0f;
        float       _w_gyro_bias = 0.0f;
        float       _mag_decl = 0.0f;
        bool        _mag_decl_auto = false;
        bool        _acc_comp = false;
        float       _bias_max = 0.0f;
        float       _vibration_warning_threshold = 1.0f;//振动警告阈值;
        hrt_abstime _vibration_warning_timestamp = 0;
        int     _ext_hdg_mode = 0;
    
        Vector<3>   _gyro;//陀螺仪;
        Vector<3>   _accel;//加速度计;
        Vector<3>   _mag;//磁力计;
    
        vision_position_estimate_s _vision = {};//buffer;
        Vector<3>   _vision_hdg;
    
        att_pos_mocap_s _mocap = {};//buffer;
        Vector<3>   _mocap_hdg;
    
        airspeed_s _airspeed = {};//buffer;
    
        Quaternion  _q;//四元数;
        Vector<3>   _rates;//姿态角速度;
        Vector<3>   _gyro_bias;//陀螺仪偏差;
    
        vehicle_global_position_s _gpos = {};//buffer;
        Vector<3>   _vel_prev;//前一时刻的速度:
        Vector<3>   _pos_acc;//加速度(body frame??)
    
        DataValidatorGroup _voter_gyro;//数据验证,剔除异常值;
        DataValidatorGroup _voter_accel;
        DataValidatorGroup _voter_mag;
    
        //姿态速度的二阶低通滤波器;
        math::LowPassFilter2p _lp_roll_rate;
        math::LowPassFilter2p _lp_pitch_rate;
        math::LowPassFilter2p _lp_yaw_rate;
    
        //绝对时间(ms)
        hrt_abstime _vel_prev_t = 0;//前一时刻计算速度时的绝对时间;
    
        bool        _inited = false;//初始化标识;
        bool        _data_good = false;//数据可用;
        bool        _failsafe = false;//故障保护;
        bool        _vibration_warning = false;//振动警告;
        bool        _ext_hdg_good = false;//外部航向可用;
    
        orb_advert_t    _mavlink_log_pub = nullptr;//mavlink log advert;
    
        //performance measuring tools (counters)
        perf_counter_t _update_perf;
        perf_counter_t _loop_perf;//未看到使用。。。;
    
        void update_parameters(bool force);//更新参数;
    
        int update_subscriptions();//未使用【待查??】
    
        bool init();
    
        bool update(float dt);
    
        // 偏航角旋转后,立即更新磁偏角;
        void update_mag_declination(float new_declination);
    };
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114

    构造函数

         (l213~l235)
    AttitudeEstimatorQ::AttitudeEstimatorQ() :
        _vel_prev(0, 0, 0),
        _pos_acc(0, 0, 0),
        _voter_gyro(3),//数据成员3个;
        _voter_accel(3),
        _voter_mag(3),
        _lp_roll_rate(250.0f, 30.0f),//低通滤波(采样频率,截止频率);
        _lp_pitch_rate(250.0f, 30.0f),
        _lp_yaw_rate(250.0f, 20.0f)
    {
        _voter_mag.set_timeout(200000);//磁力计超时;
    
        //读取Attitude_estimator_q_params.c中的参数;
        _params_handles.w_acc       = param_find("ATT_W_ACC");
        _params_handles.w_mag       = param_find("ATT_W_MAG");
        _params_handles.w_ext_hdg   = param_find("ATT_W_EXT_HDG");//外部航向权重;
        _params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");
        _params_handles.mag_decl    = param_find("ATT_MAG_DECL");
        _params_handles.mag_decl_auto   = param_find("ATT_MAG_DECL_A");//磁偏角自动校正;
        _params_handles.acc_comp    = param_find("ATT_ACC_COMP");
        _params_handles.bias_max    = param_find("ATT_BIAS_MAX");//陀螺仪偏差上限;
        _params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH");//振动警告阈值;
        _params_handles.ext_hdg_mode    = param_find("ATT_EXT_HDG_M");
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25

    析构函数

    l240~l262
    AttitudeEstimatorQ::~AttitudeEstimatorQ()
    {//杀掉所有任务;
        if (_control_task != -1) {
            /* task wakes up every 100ms or so at the longest */
            _task_should_exit = true;
    
            /* wait for a second for the task to quit at our request */
            unsigned i = 0;
    
            do {
                /* wait 20ms */
                usleep(20000);
    
                /* if we have given up, kill it */
                if (++i > 50) {
                    px4_task_delete(_control_task);
                    break;
                }
            } while (_control_task != -1);
        }
    
        attitude_estimator_q::instance = nullptr;
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24

    start();

    l264~l282
    int AttitudeEstimatorQ::start()
    {
        ASSERT(_control_task == -1);
    
        /* start the task */
        //启动任务,返回进程ID;
        _control_task = px4_task_spawn_cmd("attitude_estimator_q",/*name*/
                           SCHED_DEFAULT,/*任务调度程序*/
                           SCHED_PRIORITY_MAX - 5,/*优先级*/
                           2500,/*栈大小*/
                           (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,/*主函数入口*/
                           nullptr);
    
        if (_control_task < 0) {
            warn("task start failed");
            return -errno;
        }
    
        return OK;
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21

    print();

    l284~l292
    void AttitudeEstimatorQ::print()
    {//打印当前姿态信息;
        warnx("gyro status:");
        _voter_gyro.print();
        warnx("accel status:");
        _voter_accel.print();
        warnx("mag status:");
        _voter_mag.print();
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10

    task_main_trampoline();

    l294~l297
    void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
    {
        attitude_estimator_q::instance->task_main();
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 1
    • 2
    • 3
    • 4
    • 5

    task_main();

    l299~l655
    void AttitudeEstimatorQ::task_main()
    {
    
    #ifdef __PX4_POSIX
    //记录事件执行所花费的时间,performance counters;
        perf_counter_t _perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay"));
        perf_counter_t _perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay"));
        perf_counter_t _perf_mag(perf_alloc_once(PC_ELAPSED, "sim_mag_delay"));
    #endif
    //从uORB订阅主题;
        _sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
    //订阅传感器读数,包含数据参见:Firmware/msg/sensor_combined.msg
        _vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));//视觉;
        _mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));//vicon; 
    
        _airspeed_sub = orb_subscribe(ORB_ID(airspeed));//空速,见Firmware/msg/airspeed.msg;
    
        _params_sub = orb_subscribe(ORB_ID(parameter_update));//bool saved;
        _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));//位置估计值(GPS);
    
        update_parameters(true);//获取新参数;
    
        hrt_abstime last_time = 0;
    
        px4_pollfd_struct_t fds[1] = {};
        fds[0].fd = _sensors_sub;//文件描述符;
        fds[0].events = POLLIN;//读取事件标识;
        //POLLIN: data other than high-priority data may be read without blocking;
        while (!_task_should_exit) {
            int ret = px4_poll(fds, 1, 1000);
            //timeout = 1000; fds_size = 1; 详见Linux的poll函数;
            //对字符设备读写;
    
            if (ret < 0) {
                // Poll error, sleep and try again
                usleep(10000);
                PX4_WARN("Q POLL ERROR");
                continue;
    
            } else if (ret == 0) {
                // Poll timeout, do nothing
                PX4_WARN("Q POLL TIMEOUT");
                continue;
            }
    
            update_parameters(false);//检查orb是否更新;
    
            // Update sensors
            sensor_combined_s sensors;
    
            int best_gyro = 0;
            int best_accel = 0;
            int best_mag = 0;
    
            if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
                // Feed validator with recent sensor data
                //(put)将最近的传感器数据送入验证组(DataValidatorGroup)
    
                for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) {
                //遍历每个陀螺仪数据;
    
                    /* ignore empty fields */
                    //忽略空值;
                    if (sensors.gyro_timestamp[i] > 0) {
    
                        float gyro[3];
    
                        for (unsigned j = 0; j < 3; j++) {
                            if (sensors.gyro_integral_dt[i] > 0) {
                            //delta time 大于零;
                                gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);
                                //=角度/时间(1e6用于us->s转换);
                            } else {
                                /* fall back to angular rate */
                                //没有数据更新,回退;
                                gyro[j] = sensors.gyro_rad_s[i * 3 + j];
                            }
                        }
    
                        _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
                        //最后一个参数gyro_priority[]用于支持传感器优先级;
                    }
    
                    /* ignore empty fields */
                    if (sensors.accelerometer_timestamp[i] > 0) {
                        _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
                                 sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
                    }
                    //NED 坐标系下, 单位 m/s^2
    
                    /* ignore empty fields */
                    if (sensors.magnetometer_timestamp[i] > 0) {
                        _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
                                   sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
                    }
                    //NED 坐标系下, 单位 Gauss
                }
    
                // Get best measurement values
                //获取最佳测量值(DataValidatorGroup)
                hrt_abstime curr_time = hrt_absolute_time();
                _gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));
                _accel.set(_voter_accel.get_best(curr_time, &best_accel));
                _mag.set(_voter_mag.get_best(curr_time, &best_mag));
    
                if (_accel.length() < 0.01f) {
                    warnx("WARNING: degenerate accel!");
                    continue;
                }
                //退化,即非满秩,此处为奇异值(0);
    
                if (_mag.length() < 0.01f) {
                    warnx("WARNING: degenerate mag!");
                    continue;
                }//同上;
    
                _data_good = true;//数据可用;
    
                if (!_failsafe) {
                    uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;
    
    #ifdef __PX4_POSIX
                    perf_end(_perf_accel);//执行事件结束,计算均值方差等;
                    perf_end(_perf_mpu);
                    perf_end(_perf_mag);
    #endif
    
                    if (_voter_gyro.failover_count() > 0) {
                    //陀螺仪数据故障计数大于0, 记录错误日志;
                        _failsafe = true;
                        flags = _voter_gyro.failover_state();
                        mavlink_and_console_log_emergency(&_mavlink_log_pub, "Gyro #%i failure :%s%s%s%s%s!",
                                          _voter_gyro.failover_index(),
                                          ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
                                          ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
                                          ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
                                          ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
                                          ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
                    }
    
                    if (_voter_accel.failover_count() > 0) {
                    //同上,故障日志;
                        _failsafe = true;
                        flags = _voter_accel.failover_state();
                        mavlink_and_console_log_emergency(&_mavlink_log_pub, "Accel #%i failure :%s%s%s%s%s!",
                                          _voter_accel.failover_index(),
                                          ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
                                          ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
                                          ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
                                          ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
                                          ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
                    }
    
                    if (_voter_mag.failover_count() > 0) {
                    //同上,故障日志;
                        _failsafe = true;
                        flags = _voter_mag.failover_state();
                        mavlink_and_console_log_emergency(&_mavlink_log_pub, "Mag #%i failure :%s%s%s%s%s!",
                                          _voter_mag.failover_index(),
                                          ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
                                          ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
                                          ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
                                          ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
                                          ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
                    }
    
                    if (_failsafe) {
                    //故障安全机制;
                        mavlink_and_console_log_emergency(&_mavlink_log_pub, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
                    }
                }
    
                //若启用振动报警,且振动级别超过设定阈值,触发报警; 
                //振动级别由数据的方均根(RMS)给出;
                if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
                                _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
                                _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
    
                    if (_vibration_warning_timestamp == 0) {
                        _vibration_warning_timestamp = curr_time;
    
                    } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
                        _vibration_warning = true;
                        mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",
                                         (int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
                                         (int)(100 * _voter_accel.get_vibration_factor(curr_time)),
                                         (int)(100 * _voter_mag.get_vibration_factor(curr_time)));
                    }
    
                } else {
                    _vibration_warning_timestamp = 0;
                }
            }
    
            // Update vision and motion capture heading
            //更新视觉和vicon航向
            bool vision_updated = false;
            orb_check(_vision_sub, &vision_updated);
    
            bool mocap_updated = false;
            orb_check(_mocap_sub, &mocap_updated);
    
            if (vision_updated) {
                orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);//将订阅主题的内容复制到buffer(_vision)中;
                math::Quaternion q(_vision.q);
    
                math::Matrix<3, 3> Rvis = q.to_dcm();
                math::Vector<3> v(1.0f, 0.0f, 0.4f);
                //没看出 v 向量具体含义,疑似磁偏校正;
    
                // Rvis is Rwr (robot respect to world) while v is respect to world.
                // Hence Rvis must be transposed having (Rwr)' * Vw
                // Rrw * Vw = vn. This way we have consistency
                _vision_hdg = Rvis.transposed() * v;
            }
            //通过视觉得到的姿态估计q->Rvis,将v转换到机体坐标系;
    
            if (mocap_updated) {
                orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
                math::Quaternion q(_mocap.q);
                math::Matrix<3, 3> Rmoc = q.to_dcm();
    
                math::Vector<3> v(1.0f, 0.0f, 0.4f);
    
                // Rmoc is Rwr (robot respect to world) while v is respect to world.
                // Hence Rmoc must be transposed having (Rwr)' * Vw
                // Rrw * Vw = vn. This way we have consistency
                _mocap_hdg = Rmoc.transposed() * v;
            }
    
            // Update airspeed
            bool airspeed_updated = false;
            orb_check(_airspeed_sub, &airspeed_updated);
    
            if (airspeed_updated) {
                orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
            }
    
            // Check for timeouts on data
            if (_ext_hdg_mode == 1) {
                _ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
    
            } else if (_ext_hdg_mode == 2) {
                _ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
            }
    
            bool gpos_updated;
            orb_check(_global_pos_sub, &gpos_updated);
    
            if (gpos_updated) {
                orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);
    
                if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {
                    /* set magnetic declination automatically */
                                    update_mag_declination(math::radians(get_mag_declination(_gpos.lat, _gpos.lon)));
                }
                //磁偏自动校正,且水平偏差的标准差小于20,根据位置估计值(GPS)(vehicle_global_position)校正磁偏角;
                //get_mag_declination()函数查表得到地磁偏角,进行补偿。
            }
            if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f && _inited) {
                /* position data is actual */
                //基于GPS的位置信息,微分得到加速度值;
                if (gpos_updated) {
                    Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);
    
                    /* velocity updated */
                    if (_vel_prev_t != 0 && _gpos.timestamp != _vel_prev_t) {
                        float vel_dt = (_gpos.timestamp - _vel_prev_t) / 1000000.0f;//时间间隔,单位(s)
                        /* calculate acceleration in body frame */
                        _pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
                    }//由ned坐标系下的速度求出机体坐标系下的加速度;
    
                    _vel_prev_t = _gpos.timestamp;
                    _vel_prev = vel;
                }
    
            } else {
                /* position data is outdated, reset acceleration */
                //位置信息已过时,重置;
                _pos_acc.zero();
                _vel_prev.zero();
                _vel_prev_t = 0;
            }
    
            /* time from previous iteration */
            hrt_abstime now = hrt_absolute_time();
            float dt = (last_time > 0) ? ((now  - last_time) / 1000000.0f) : 0.00001f;//用极小值0.00001表示零,预防除零错误;
            last_time = now;
    
            if (dt > _dt_max) {
                dt = _dt_max;
            }//时间间隔上限;
    
            if (!update(dt)) {
                continue;
            }//调用update(dt),**互补滤波**,更新四元数;
            //############若不熟悉update(),请转到函数查看;
    
            Vector<3> euler = _q.to_euler();
    
            struct vehicle_attitude_s att = {};
            att.timestamp = sensors.timestamp;
    
            att.roll = euler(0);
            att.pitch = euler(1);
            att.yaw = euler(2);
    
            att.rollspeed = _rates(0);
            att.pitchspeed = _rates(1);
            att.yawspeed = _rates(2);
    
            for (int i = 0; i < 3; i++) {
                att.g_comp[i] = _accel(i) - _pos_acc(i);
            }//补偿重力向量;
    
            /* copy offsets */
            memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets));
            //memcpy(*dest,*src,size);
    
            Matrix<3, 3> R = _q.to_dcm();
    
            /* copy rotation matrix */
            memcpy(&att.R[0], R.data, sizeof(att.R));
            att.R_valid = true;
            memcpy(&att.q[0], _q.data, sizeof(att.q));
            att.q_valid = true;
            //获取姿态振动, RMS;
            att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time());
            att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time());
            att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time());
    
            /* the instance count is not used here */
            int att_inst;
            orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
            //广播姿态信息;
    
            {//使用当前姿态,更新control_state,并发布;
                struct control_state_s ctrl_state = {};
    
                ctrl_state.timestamp = sensors.timestamp;
    
                /* attitude quaternions for control state */
                ctrl_state.q[0] = _q(0);
                ctrl_state.q[1] = _q(1);
                ctrl_state.q[2] = _q(2);
                ctrl_state.q[3] = _q(3);
    
                /* attitude rates for control state */
                //低通滤波,输入参数为当前值;
                ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));
    
                ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));
    
                ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
    
                /* Airspeed - take airspeed measurement directly here as no wind is estimated */
                if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6
                    && _airspeed.timestamp > 0) {
                    ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
                    ctrl_state.airspeed_valid = true;
    
                } else {
                    ctrl_state.airspeed_valid = false;
                }
    
                /* the instance count is not used here */
                int ctrl_inst;
                /* publish to control state topic */
                //发布控制状态主题,control_state.msg。
                orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH);
            }
    
            {
                struct estimator_status_s est = {};
    
                est.timestamp = sensors.timestamp;
                est.vibe[0] = _voter_accel.get_vibration_offset(est.timestamp, 0);
                est.vibe[1] = _voter_accel.get_vibration_offset(est.timestamp, 1);
                est.vibe[2] = _voter_accel.get_vibration_offset(est.timestamp, 2);
    
                /* the instance count is not used here */
                int est_inst;
                /* publish to control state topic */
                orb_publish_auto(ORB_ID(estimator_status), &_est_state_pub, &est, &est_inst, ORB_PRIO_HIGH);
            }
        }
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 115
    • 116
    • 117
    • 118
    • 119
    • 120
    • 121
    • 122
    • 123
    • 124
    • 125
    • 126
    • 127
    • 128
    • 129
    • 130
    • 131
    • 132
    • 133
    • 134
    • 135
    • 136
    • 137
    • 138
    • 139
    • 140
    • 141
    • 142
    • 143
    • 144
    • 145
    • 146
    • 147
    • 148
    • 149
    • 150
    • 151
    • 152
    • 153
    • 154
    • 155
    • 156
    • 157
    • 158
    • 159
    • 160
    • 161
    • 162
    • 163
    • 164
    • 165
    • 166
    • 167
    • 168
    • 169
    • 170
    • 171
    • 172
    • 173
    • 174
    • 175
    • 176
    • 177
    • 178
    • 179
    • 180
    • 181
    • 182
    • 183
    • 184
    • 185
    • 186
    • 187
    • 188
    • 189
    • 190
    • 191
    • 192
    • 193
    • 194
    • 195
    • 196
    • 197
    • 198
    • 199
    • 200
    • 201
    • 202
    • 203
    • 204
    • 205
    • 206
    • 207
    • 208
    • 209
    • 210
    • 211
    • 212
    • 213
    • 214
    • 215
    • 216
    • 217
    • 218
    • 219
    • 220
    • 221
    • 222
    • 223
    • 224
    • 225
    • 226
    • 227
    • 228
    • 229
    • 230
    • 231
    • 232
    • 233
    • 234
    • 235
    • 236
    • 237
    • 238
    • 239
    • 240
    • 241
    • 242
    • 243
    • 244
    • 245
    • 246
    • 247
    • 248
    • 249
    • 250
    • 251
    • 252
    • 253
    • 254
    • 255
    • 256
    • 257
    • 258
    • 259
    • 260
    • 261
    • 262
    • 263
    • 264
    • 265
    • 266
    • 267
    • 268
    • 269
    • 270
    • 271
    • 272
    • 273
    • 274
    • 275
    • 276
    • 277
    • 278
    • 279
    • 280
    • 281
    • 282
    • 283
    • 284
    • 285
    • 286
    • 287
    • 288
    • 289
    • 290
    • 291
    • 292
    • 293
    • 294
    • 295
    • 296
    • 297
    • 298
    • 299
    • 300
    • 301
    • 302
    • 303
    • 304
    • 305
    • 306
    • 307
    • 308
    • 309
    • 310
    • 311
    • 312
    • 313
    • 314
    • 315
    • 316
    • 317
    • 318
    • 319
    • 320
    • 321
    • 322
    • 323
    • 324
    • 325
    • 326
    • 327
    • 328
    • 329
    • 330
    • 331
    • 332
    • 333
    • 334
    • 335
    • 336
    • 337
    • 338
    • 339
    • 340
    • 341
    • 342
    • 343
    • 344
    • 345
    • 346
    • 347
    • 348
    • 349
    • 350
    • 351
    • 352
    • 353
    • 354
    • 355
    • 356
    • 357
    • 358
    • 359
    • 360
    • 361
    • 362
    • 363
    • 364
    • 365
    • 366
    • 367
    • 368
    • 369
    • 370
    • 371
    • 372
    • 373
    • 374
    • 375
    • 376
    • 377
    • 378
    • 379
    • 380
    • 381
    • 382
    • 383
    • 384
    • 385
    • 386
    • 387
    • 388
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 115
    • 116
    • 117
    • 118
    • 119
    • 120
    • 121
    • 122
    • 123
    • 124
    • 125
    • 126
    • 127
    • 128
    • 129
    • 130
    • 131
    • 132
    • 133
    • 134
    • 135
    • 136
    • 137
    • 138
    • 139
    • 140
    • 141
    • 142
    • 143
    • 144
    • 145
    • 146
    • 147
    • 148
    • 149
    • 150
    • 151
    • 152
    • 153
    • 154
    • 155
    • 156
    • 157
    • 158
    • 159
    • 160
    • 161
    • 162
    • 163
    • 164
    • 165
    • 166
    • 167
    • 168
    • 169
    • 170
    • 171
    • 172
    • 173
    • 174
    • 175
    • 176
    • 177
    • 178
    • 179
    • 180
    • 181
    • 182
    • 183
    • 184
    • 185
    • 186
    • 187
    • 188
    • 189
    • 190
    • 191
    • 192
    • 193
    • 194
    • 195
    • 196
    • 197
    • 198
    • 199
    • 200
    • 201
    • 202
    • 203
    • 204
    • 205
    • 206
    • 207
    • 208
    • 209
    • 210
    • 211
    • 212
    • 213
    • 214
    • 215
    • 216
    • 217
    • 218
    • 219
    • 220
    • 221
    • 222
    • 223
    • 224
    • 225
    • 226
    • 227
    • 228
    • 229
    • 230
    • 231
    • 232
    • 233
    • 234
    • 235
    • 236
    • 237
    • 238
    • 239
    • 240
    • 241
    • 242
    • 243
    • 244
    • 245
    • 246
    • 247
    • 248
    • 249
    • 250
    • 251
    • 252
    • 253
    • 254
    • 255
    • 256
    • 257
    • 258
    • 259
    • 260
    • 261
    • 262
    • 263
    • 264
    • 265
    • 266
    • 267
    • 268
    • 269
    • 270
    • 271
    • 272
    • 273
    • 274
    • 275
    • 276
    • 277
    • 278
    • 279
    • 280
    • 281
    • 282
    • 283
    • 284
    • 285
    • 286
    • 287
    • 288
    • 289
    • 290
    • 291
    • 292
    • 293
    • 294
    • 295
    • 296
    • 297
    • 298
    • 299
    • 300
    • 301
    • 302
    • 303
    • 304
    • 305
    • 306
    • 307
    • 308
    • 309
    • 310
    • 311
    • 312
    • 313
    • 314
    • 315
    • 316
    • 317
    • 318
    • 319
    • 320
    • 321
    • 322
    • 323
    • 324
    • 325
    • 326
    • 327
    • 328
    • 329
    • 330
    • 331
    • 332
    • 333
    • 334
    • 335
    • 336
    • 337
    • 338
    • 339
    • 340
    • 341
    • 342
    • 343
    • 344
    • 345
    • 346
    • 347
    • 348
    • 349
    • 350
    • 351
    • 352
    • 353
    • 354
    • 355
    • 356
    • 357
    • 358
    • 359
    • 360
    • 361
    • 362
    • 363
    • 364
    • 365
    • 366
    • 367
    • 368
    • 369
    • 370
    • 371
    • 372
    • 373
    • 374
    • 375
    • 376
    • 377
    • 378
    • 379
    • 380
    • 381
    • 382
    • 383
    • 384
    • 385
    • 386
    • 387
    • 388

    update_parameters();

    l657~l686
    void AttitudeEstimatorQ::update_parameters(bool force)
    {
        bool updated = force;
    
        if (!updated) {
            orb_check(_params_sub, &updated);//查看参数是否更新;
        }
    
        if (updated) {//获取新参数;
            parameter_update_s param_update;
            orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
    
            param_get(_params_handles.w_acc, &_w_accel);
            param_get(_params_handles.w_mag, &_w_mag);
            param_get(_params_handles.w_ext_hdg, &_w_ext_hdg);
            param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
            float mag_decl_deg = 0.0f;
            param_get(_params_handles.mag_decl, &mag_decl_deg);
            update_mag_declination(math::radians(mag_decl_deg));
            int32_t mag_decl_auto_int;
            param_get(_params_handles.mag_decl_auto, &mag_decl_auto_int);
            _mag_decl_auto = mag_decl_auto_int != 0;//自动磁偏角校正;
            int32_t acc_comp_int;
            param_get(_params_handles.acc_comp, &acc_comp_int);
            _acc_comp = acc_comp_int != 0;
            param_get(_params_handles.bias_max, &_bias_max);//陀螺仪偏差上限;
            param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);//振动警告阈值;
            param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
        }
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31

    init();

    1. k  为导航坐标系(NED)的 z  轴(D)在机体坐标系中的表示;

      导航系中,D正方向朝下;

    2. i  为导航坐标系(NED)的 x  轴(N)在机体坐标系;

      i = (_mag - k * (_mag * k)); //施密特正交化;

      β 2 =α 2 (α 2 ,β 1 )(β 1 ,β 1 ) β 1  

      //因 向量 k 已归一化,故分母等于1;

    3. j  为导航坐标系(NED)的 y  轴(E)在机体坐标系;

      j = k % i //叉乘求正交向量;

      ∣ ∣ ∣ ∣ ∣ ik[0]i[0] jk[1]i[1] kk[2]i[2] ∣ ∣ ∣ ∣ ∣ =⎡ ⎣ ⎢ k[1]i[2]i[1]k[2]i[0]k[2]k[0]i[2]k[0]i[1]i[0]k[1] ⎤ ⎦ ⎥  

    4. 构造旋转矩阵 R 

      R.set_row(0, i); R.set_row(1, j); R.set_row(2, k);

      R=⎡ ⎣ ⎢ ijk ⎤ ⎦ ⎥  

    5. 转换为四元数 q  ,根据设定校正磁偏,归一化;

    l688~l728
    bool AttitudeEstimatorQ::init()
    {
        // Rotation matrix can be easily constructed from acceleration and mag field vectors
        // 'k' is Earth Z axis (Down) unit vector in body frame
        Vector<3> k = -_accel;
        k.normalize();
    
        // 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
        Vector<3> i = (_mag - k * (_mag * k));
        i.normalize();
    
        // 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
        Vector<3> j = k % i;
    
        // Fill rotation matrix
        Matrix<3, 3> R;
        R.set_row(0, i);
        R.set_row(1, j);
        R.set_row(2, k);
    
        // Convert to quaternion
        _q.from_dcm(R);
    
        // Compensate for magnetic declination
        Quaternion decl_rotation;
        decl_rotation.from_yaw(_mag_decl);
        _q = decl_rotation * _q;
    
        _q.normalize();
    
        if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
            PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
            _q.length() > 0.95f && _q.length() < 1.05f) {
            _inited = true;
    
        } else {
            _inited = false;
        }
    
        return _inited;
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42

    update();

    1. init();//执行一次;

      由加速度计和磁力计初始化旋转矩阵和四元数;

    2. mag_earth = _q.conjugate(_mag);

      不使用外部航向,或外部航向异常时,采用磁力计校准;
      将磁力计读数从机体坐标系转换到导航坐标系;

      R n b _mag 

    3. mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);

      将磁力计的读数由向量换算到角度; 该角度减去磁偏,得到磁场偏差;
      _mag_decl 由GPS得到;
      **atan2f: 2 表示两个输入参数; 支持四象限内角度换算; 输出弧度值;
      **_wrap_pi: 将(0~2pi)的角度映射到(-pi~pi);

    4. corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;

      将磁场偏差转换到机体坐标系,并乘以其对应权重;

      R b n ⎡ ⎣ ⎢ 00mag_err ⎤ ⎦ ⎥ _w m ag 

    5. _q.normalize();

      四元数归一化;
      这里的归一化用于校正update_mag_declination后的偏差。

    6. corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;

      向量 k 是重力加速度向量(0,0,1)转换到机体坐标系;
      _accel 是加速度计的读数;
      _pos_acc 是由位置估计(GPS) 微分得到的加速度;
      _accel - _pos_acc 表示飞行器加速度向量减去其水平分量;
      k 与 (_accel - _pos_acc)叉乘,得到偏差;

      k=R b n ⎡ ⎣ ⎢ 001 ⎤ ⎦ ⎥  

      e=k×[] 

    7. _gyro_bias += corr * (_w_gyro_bias * dt);

      得到陀螺仪修正值
      此处修正值为 mahony 滤波的 pi 控制器的 积分部分;
      因为 _gyro_bias 不归零,故不断累积;

      gyro_bias+=[Magw mag +Accw acc ]w gyro dt 

    8. _rates = _gyro + _gyro_bias;

      _rates 表示角速度;

      ω=ω g +δ 

    9. corr += _rates;

      这里的 corr = (Ea+Em) + (Ea+Em)*dt + gyro;
      个人认为这里的 corr 才是更新后的角速度;

    10. _q += _q.derivative(corr) * dt;

      更新四元数,derivative为求导函数,使用一阶龙格库塔求导。

      q ˙ =12 qω 

    l730~l817
    bool AttitudeEstimatorQ::update(float dt)
    {
        if (!_inited) {
    
            if (!_data_good) {
                return false;
            }
    
            return init();
        }
    
        Quaternion q_last = _q;//保存上一状态,以便恢复;
    
        // Angular rate of correction
        Vector<3> corr;//初始化元素为0;
    
        //_ext_hdg_good表示外部航向数据可用;
        if (_ext_hdg_mode > 0 && _ext_hdg_good) {
            if (_ext_hdg_mode == 1) {
                // Vision heading correction
                //视觉航向校正;
                // Project heading to global frame and extract XY component
                //将航向投影到导航坐标系,提取XY分量;
                Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg);
                float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
                // Project correction to body frame
                corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg;
            }
    
            if (_ext_hdg_mode == 2) {
                // Mocap heading correction
                // Project heading to global frame and extract XY component
                Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg);
                float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
                // Project correction to body frame
                corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg;
            }
        }
    
        if (_ext_hdg_mode == 0  || !_ext_hdg_good) {
            // Magnetometer correction
            // Project mag field vector to global frame and extract XY component
            Vector<3> mag_earth = _q.conjugate(_mag);
            float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
            // Project magnetometer correction to body frame
            corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
        }
    
        _q.normalize();
    
        // Accelerometer correction
        // Project 'k' unit vector of earth frame to body frame
        // Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
        // Optimized version with dropped zeros
        Vector<3> k(
            2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
            2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
            (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
        );
    
        corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
    
        // Gyro bias estimation
        _gyro_bias += corr * (_w_gyro_bias * dt);
    
        for (int i = 0; i < 3; i++) {//陀螺仪最大偏差上限;
            _gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
        }
    
        _rates = _gyro + _gyro_bias;
    
        // Feed forward gyro
        corr += _rates;
    
        // Apply correction to state
        _q += _q.derivative(corr) * dt;
    
        // Normalize quaternion
        _q.normalize();
        //判断四元数是否发散,若发散,则沿用之前的四元数;
        if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
              PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
            // Reset quaternion to last good state
            _q = q_last;
            _rates.zero();
            _gyro_bias.zero();
            return false;
        }
    
        return true;
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92

    update_mag_declination();

    l819~l832
    void AttitudeEstimatorQ::update_mag_declination(float new_declination)
    {
        // Apply initial declination or trivial rotations without changing estimation
        //忽略微小旋转;
        if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f) {
            _mag_decl = new_declination;
    
        } else {
            // Immediately rotate current estimation to avoid gyro bias growth
            //磁偏超过一定值后,校正姿态;
            Quaternion decl_rotation;
            decl_rotation.from_yaw(new_declination - _mag_decl);
            //由磁偏角度转化为四元数;
            _q = decl_rotation * _q;
            //四元数相乘表示角度相加;
            _mag_decl = new_declination;
        }
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19

    attitude_estimator_q_main();

    l834~l890
    int attitude_estimator_q_main(int argc, char *argv[])
    {//外部调用接口;
        if (argc < 2) {
            warnx("usage: attitude_estimator_q {start|stop|status}");
            return 1;
        }
    
        if (!strcmp(argv[1], "start")) {
    
            if (attitude_estimator_q::instance != nullptr) {
                warnx("already running");
                return 1;
            }
            //实例化,instance;
            attitude_estimator_q::instance = new AttitudeEstimatorQ;
    
            if (attitude_estimator_q::instance == nullptr) {
                warnx("alloc failed");
                return 1;
            }
    
            if (OK != attitude_estimator_q::instance->start()) {
                delete attitude_estimator_q::instance;
                attitude_estimator_q::instance = nullptr;
                warnx("start failed");
                return 1;
            }
    
            return 0;
        }
    
        if (!strcmp(argv[1], "stop")) {
            if (attitude_estimator_q::instance == nullptr) {
                warnx("not running");
                return 1;
            }
            //删除实例化对象,指针置空;
            delete attitude_estimator_q::instance;
            attitude_estimator_q::instance = nullptr;
            return 0;
        }
        //打印当前姿态信息;
        if (!strcmp(argv[1], "status")) {
            if (attitude_estimator_q::instance) {
                attitude_estimator_q::instance->print();
                warnx("running");
                return 0;
    
            } else {
                warnx("not running");
                return 1;
            }
        }
    
        warnx("unrecognized command");
        return 1;
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58

    展开全文
  • 互补滤波算法及理论推导

    千次阅读 2018-08-29 21:16:51
    四旋翼姿态解算——互补滤波算法及理论推导 2017年02月24日 17:37:11 阅读数:8763 标签: 算法数学传感器数据 更多 个人分类: STM32四旋翼无人机 版权声明:本文为博主原创文章,未经博主允许不得转载。 ...

    四旋翼姿态解算——互补滤波算法及理论推导

    2017年02月24日 17:37:11 阅读数:8763 标签: 算法数学传感器数据 更多

    个人分类: STM32四旋翼无人机

    版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/hongbin_xu/article/details/56846490

    转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 
    文章链接:http://blog.csdn.net/hongbin_xu/article/details/56846490 或 http://hongbin96.com/111

    上次我们讨论了姿态解算基础理论以及几个比较重要的公式的一些推导,如果有不熟悉的请点击这里打开链接。这次来介绍一些实际的姿态解算算法吧! 
    一般在程序中,姿态解算的方式有两种:一种是欧拉角法,一种是四元数法。这里不介绍欧拉角法,只介绍四元数法,如有兴趣可以去查找相关资料。

    互补滤波算法: 
    顾名思义,是多组数据结合互补,并进行滤波处理稳定输出,得到姿态的算法。而我们使用的传感器就是加速度计和陀螺仪。加速度计用于测量加速度,陀螺仪用于测量角速度。 加速度计的静态稳定性更好,而在运动时其数据相对不可靠;陀螺仪的动态稳定性更好,但是静止时数据相对不可靠。所以,我们可以通过加速度计的输出来修正陀螺仪的漂移误差,换句话说,通过加速度计来修正陀螺仪

    这个是我在网上找到的说明互补滤波法的框图:(原图下载:点击这里打开)

    首先,我们取定导航坐标系n中标准重力加速度g,定义为这里写图片描述,那么将导航坐标系n下的这里写图片描述 转换为载体坐标系b下的:这里写图片描述。 
    这里用到了这里写图片描述,前面在推导基础公式时导出了使用四元数表示的旋转矩阵这里写图片描述。 
    公式如下: 
    这里写图片描述 
    但是我们要用的是这里写图片描述,所以还要对这里写图片描述 做一个矩阵逆变换。由于它是正交矩阵,对于正交矩阵有这个性质:正交矩阵的逆矩阵等于其的转置。所以我们很容易得到: 
    这里写图片描述 
    将上式代入这里写图片描述 ,而且我们已知这里写图片描述,所以得到: 
    这里写图片描述

    接着再定义载体坐标系b中加速度计输出为a,由于前面计算导航坐标系时我们采用的重力加速度是标准重力加速度,所以还需要对其进行归一化,才能继续运算。 
    设加速度计三个轴的值分别是ax,ay,az。 
    首先求模:这里写图片描述。 
    归一化:这里写图片描述

    这里写图片描述
    根据框图的说明再整理一下前面得到的结果: 
    标准重力加速度这里写图片描述从n系转到b系中的矩阵表示: 
    这里写图片描述 
    b系下加速度计测量得到的加速度的矩阵表示: 
    这里写图片描述 (备注:这是归一化之后的值)

    这里写图片描述 
    这里写图片描述 和这里写图片描述 做向量叉乘,即可得到给陀螺仪的校正补偿值e。 
    这里写图片描述 
    表示成矩阵形式更为直观: 
    这里写图片描述

    然后再使用PI控制器进行滤波,准确地说事消除漂移误差,只要存在误差控制器便会持续作用,直至误差为0。控制的效果取决于P和I参数,分别对应比例控制和积分控制的参数。 
    这里写图片描述 
    这里给出PI控制的公式: 
    这里写图片描述 
    这里写图片描述 是我们要负反馈给陀螺仪进行校正补偿的值,这里写图片描述 是比例控制项,这里写图片描述 是积分控制项,在程序中采用离散累加计算。关于PID控制理论的东西,这里不做赘述。

    这里写图片描述 
    如框图中所写,接下来将前面得到的补偿值这里写图片描述加在陀螺仪输出的数据上进行校正。

    跟着框图往下走: 
    这里写图片描述 
    将前面的陀螺仪数据通过四元数微分方程转换为四元数输出。 
    因为有几个地方我也没搞懂,所以就简单介绍一下四元数微分方程,详细步骤请查阅秦永元的惯性导航一书(第三篇 9.2.3节): 
    由于载体的运动,四元数Q是变量,其参数可以表示成关于时间的函数。 
    使用四元数的三角形式:这里写图片描述 
    这里写图片描述 为刚体瞬时绕轴转过的角度,这里写图片描述 为归一化后的位置矢量。 
    设角速度为:这里写图片描述 
    对四元数的三角表示形式求导: 
    这里写图片描述 
    因为这里写图片描述 , 且这里写图片描述 
    所以: 
    这里写图片描述 
    将上式表示成矩阵形式: 
    这里写图片描述 
    或者这里写图片描述 
    上面的两个公式被称为四元数微分方程。利用陀螺仪的数据进行离散累积便可得到四元数的值,最后再转换成欧拉角形式输出即可。

    再附上代码:

    
    #define Kp 10.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
    #define Ki 0.008f                          // integral gain governs rate of convergence of gyroscope biases
    #define halfT 0.001f                   // half the sample period采样周期的一半
    
    float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
    float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error
    void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
    {
      float norm;
    //  float hx, hy, hz, bx, bz;
      float vx, vy, vz;// wx, wy, wz;
      float ex, ey, ez;
    
      // 先把这些用得到的值算好
      float q0q0 = q0*q0;
      float q0q1 = q0*q1;
      float q0q2 = q0*q2;
    //  float q0q3 = q0*q3;
      float q1q1 = q1*q1;
    //  float q1q2 = q1*q2;
      float q1q3 = q1*q3;
      float q2q2 = q2*q2;
      float q2q3 = q2*q3;
      float q3q3 = q3*q3;
    
        if(ax*ay*az==0)
            return;
    
      norm = sqrt(ax*ax + ay*ay + az*az);       //acc数据归一化
      ax = ax /norm;
      ay = ay / norm;
      az = az / norm;
    
      // estimated direction of gravity and flux (v and w)              估计重力方向和流量/变迁
      vx = 2*(q1q3 - q0q2);                                             //四元素中xyz的表示
      vy = 2*(q0q1 + q2q3);
      vz = q0q0 - q1q1 - q2q2 + q3q3 ;
    
      // error is sum of cross product between reference direction of fields and direction measured by sensors
      ex = (ay*vz - az*vy) ;                                             //向量外积在相减得到差分就是误差
      ey = (az*vx - ax*vz) ;
      ez = (ax*vy - ay*vx) ;
    
      exInt = exInt + ex * Ki;                                //对误差进行积分
      eyInt = eyInt + ey * Ki;
      ezInt = ezInt + ez * Ki;
    
      // adjusted gyroscope measurements
      gx = gx + Kp*ex + exInt;                                              //将误差PI后补偿到陀螺仪,即补偿零点漂移
      gy = gy + Kp*ey + eyInt;
      gz = gz + Kp*ez + ezInt;                                          //这里的gz由于没有观测者进行矫正会产生漂移,表现出来的就是积分自增或自减
    
      // integrate quaternion rate and normalise                           //四元素的微分方程
      q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
      q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
      q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
      q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
    
      // normalise quaternion
      norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
      q0 = q0 / norm;
      q1 = q1 / norm;
      q2 = q2 / norm;
      q3 = q3 / norm;
    
      //Q_ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
      Q_ANGLE.Y  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
      Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70

    这段代码网上很多地方都可以看见,匿名四轴(老版本的)的程序也是用的这段。 
    上面也添加了一些注释,把互补滤波算法的理论部分过了一遍后再来对着代码读一遍,应该不会觉得有多难了吧。程序中就只是把前面推导的公式一个个用上去了而已,如果不懂原理直接使用也不会有太大问题。 
    程序中的积分运算是采用离散累积的方法运算的。PID控制器只是起到了一个稳定数据消除漂移误差的作用。

    希望这些笔记能给更多的人提供帮助,欢迎交流指正。

    我的前一篇博文讲的是姿态解算的基础知识和一些相关公式的推导,有兴趣的可以去查看:http://blog.csdn.net/hongbin_xu/article/details/55667899

    最后再分享一些我在网上找到的资料,个人觉得对学习很有帮助。 
    链接: 
    http://blog.csdn.net/wkdwl/article/details/52119163 
    http://blog.csdn.net/nemol1990/article/details/16924745 
    http://blog.csdn.net/super_mice/article/details/45619945 
    http://www.openedv.com/thread-42657-1-1.html

    展开全文
  • 通过stm32对mpu6050用互补滤波算法算出角度用串口输出,数据和dmp一样稳定
  • 一阶互补 // a=tau / (tau + loop time) // newAngle = angle measured with atan2 using the accelerometer //加速度传感器输出值 // newRate = angle measured using the gyro // looptime = loop time in ...

    一阶互补

    // a=tau / (tau + loop time)
    // newAngle = angle measured with atan2 using the accelerometer
    //加速度传感器输出值
    // newRate = angle measured using the gyro
    // looptime = loop time in millis()
     
    float tau=0.075;
    float a=0.0;
    float Complementary(float newAngle, float newRate,int looptime) 
    {
    float dtC = float(looptime)/1000.0;
    a=tau/(tau+dtC);
    //以下代码更改成白色,下载后恢复成其他颜色即可看到
    x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);
    return x_angleC;

    }

     

    二阶互补

    // newAngle = angle measured with atan2 using the accelerometer
    // newRate = angle measured using the gyro
    // looptime = loop time in millis()
    float Complementary2(float newAngle, float newRate,int looptime) 
    {
    float k=10;
    float dtc2=float(looptime)/1000.0;
    //以下代码更改成白色,下载后恢复成其他颜色即可看到
    x1 = (newAngle -   x_angle2C)*k*k;
    y1 = dtc2*x1 + y1;
    z1= y1 + (newAngle -   x_angle2C)*2*k + newRate;
    x_angle2C = dtc2*z1 + x_angle2C;
    return x_angle2C;

    } Here too we just have to setthe k and magically we get the angle.

    卡尔曼滤波

    // KasBot V1 - Kalman filter module
     
    float Q_angle  =  0.01; //0.001
    float Q_gyro   =  0.0003;  //0.003
    float R_angle  =  0.01;  //0.03
     
    float x_bias = 0;
    float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
    float  y, S;
    float K_0, K_1;
     
    // newAngle = angle measured with atan2 using the accelerometer
    // newRate = angle measured using the gyro
    // looptime = loop time in millis()
     
    float kalmanCalculate(float newAngle, float newRate,int looptime)
    {
    float dt = float(looptime)/1000;
    x_angle += dt * (newRate - x_bias);
    //以下代码更改成白色,下载后恢复成其他颜色即可看到
    P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
    P_01 +=  - dt * P_11;
    P_10 +=  - dt * P_11;
    P_11 +=  + Q_gyro * dt;
     
    y = newAngle - x_angle;
    S = P_00 + R_angle;
    K_0 = P_00 / S;
    K_1 = P_10 / S;
     
    x_angle +=  K_0 * y;
    x_bias  +=  K_1 * y;
    P_00 -= K_0 * P_00;
    P_01 -= K_0 * P_01;
    P_10 -= K_1 * P_00;
    P_11 -= K_1 * P_01;
     
    return x_angle;

    } To get the answer, you have toset 3 parameters: Q_angle, R_angle,R_gyro.

     

     

    详细卡尔曼滤波

    /* -*- indent-tabs-mode:T;c-basic-offset:8; tab-width:8; -*- vi: set ts=8:

     *$Id: tilt.c,v 1.1 2003/07/09 18:23:29 john Exp $

     *

     * 1dimensional tilt sensor using a dual axis accelerometer

     *and single axis angular rate gyro.  Thetwo sensors are fused

     *via a two state Kalman filter, with one state being the angle

     *and the other state being the gyro bias.

     *

     *Gyro bias is automatically tracked by the filter.  This seems

     *like magic.

     *

     *Please note that there are lots of comments in the functions and

     * inblocks before the functions.  Kalmanfiltering is an already complex

     *subject, made even more so by extensive hand optimizations to the C code

     *that implements the filter.  I've triedto make an effort of explaining

     *the optimizations, but feel free to send mail to the mailing list,

     *autopilot-devel@lists.sf.net, with questions about this code.

     *

     *

     *(c) 2003 Trammell Hudson <hudson@rotomotion.com>

     *

     *************

     *

     *  Thisfile is part of the autopilot onboard code package.

     * 

     * Autopilot is free software; you can redistribute it and/or modify

     *  itunder the terms of the GNU General Public License as published by

     *  theFree Software Foundation; either version 2 of the License, or

     *  (atyour option) any later version.

     * 

     * Autopilot is distributed in the hope that it will be useful,

     *  butWITHOUT ANY WARRANTY; without even the implied warranty of

     * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

     *  GNUGeneral Public License for more details.

     * 

     *  Youshould have received a copy of the GNU General Public License

     *  alongwith Autopilot; if not, write to the Free Software

     * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA

     *

     */

    #include <math.h>

     

     

    /*

     *Our update rate.  This is how often ourstate is updated with

     *gyro rate measurements.  For now, we doit every time an

     * 8bit counter running at CLK/1024 expires. You will have to

     *change this value if you update at a different rate.

     */

    static const float     dt    = ( 1024.0 * 256.0 )/ 8000000.0;

     

     

    /*

     *Our covariance matrix.  This is updatedat every time step to

     *determine how well the sensors are tracking the actual state.

     */

    static float             P[2][2] = {

           {1, 0 },

           {0, 1 },

    };

     

     

    /*

     *Our two states, the angle and the gyro bias. As a byproduct of computing

     *the angle, we also have an unbiased angular rate available.   These are

     *read-only to the user of the module.

     */

    float               angle;

    float               q_bias;

    float               rate;

     

     

    /*

     * Rrepresents the measurement covariance noise. In this case,

     * itis a 1x1 matrix that says that we expect 0.3 rad jitter

     *from the accelerometer.

     */

    static const float     R_angle   = 0.3;

     

     

    /*

     * Qis a 2x2 matrix that represents the process covariance noise.

     * Inthis case, it indicates how much we trust the acceleromter

     *relative to the gyros.

     */

    static const float     Q_angle  = 0.001;

    static const float     Q_gyro   = 0.003;

     

     

    /*

     *state_update is called every dt with a biased gyro measurement

     * bythe user of the module.  It updates thecurrent angle and

     *rate estimate.

     *

     *The pitch gyro measurement should be scaled into real units, but

     *does not need any bias removal.  Thefilter will track the bias.

     *

     *Our state vector is:

     *

     *    X = [ angle, gyro_bias ]

     *

     * Itruns the state estimation forward via the state functions:

     *

     *    Xdot = [ angle_dot, gyro_bias_dot ]

     *

     *    angle_dot       =gyro - gyro_bias

     *    gyro_bias_dot = 0

     *

     *And updates the covariance matrix via the function:

     *

     *    Pdot = A*P + P*A' + Q

     *

     * Ais the Jacobian of Xdot with respect to the states:

     *

     *    A = [ d(angle_dot)/d(angle)     d(angle_dot)/d(gyro_bias) ]

     *        [d(gyro_bias_dot)/d(angle) d(gyro_bias_dot)/d(gyro_bias) ]

     *

     *      = [0 -1 ]

     *        [0  0 ]

     *

     *Due to the small CPU available on the microcontroller, we've

     *hand optimized the C code to only compute the terms that are

     *explicitly non-zero, as well as expanded out the matrix math

     * tobe done in as few steps as possible. This does make it harder

     * toread, debug and extend, but also allows us to do this with

     *very little CPU time.

     */

    void

    state_update(   const float             q_m /* Pitch gyro measurement */)

    {

           /*Unbias our gyro */

           constfloat             q = q_m - q_bias;

     

           /*

            * Compute the derivative of the covariancematrix

            *

            *    Pdot= A*P + P*A' + Q

            *

            * We've hand computed the expansion of A = [ 0-1, 0 0 ] multiplied

            * by P and P multiplied by A' = [ 0 0, -1, 0].  This is then added

            * to the diagonal elements of Q, which areQ_angle and Q_gyro.

            */

           constfloat             Pdot[2 * 2] = {

                  Q_angle- P[0][1] - P[1][0],  /* 0,0 */

                          - P[1][1],              /* 0,1 */

                          -P[1][1],              /* 1,0 */

                  Q_gyro                        /* 1,1 */

           };

     

           /*Store our unbias gyro estimate */

           rate= q;

     

           /*

            * Update our angle estimate

            * angle += angle_dot * dt

            *      += (gyro - gyro_bias) * dt

            *      += q * dt

            */

           angle+= q * dt;

     

           /*Update the covariance matrix */

           P[0][0]+= Pdot[0] * dt;

           P[0][1]+= Pdot[1] * dt;

           P[1][0]+= Pdot[2] * dt;

           P[1][1]+= Pdot[3] * dt;

    }

     

     

    /*

     *kalman_update is called by a user of the module when a new

     *accelerometer measurement is available. ax_m and az_m do not

     *need to be scaled into actual units, but must be zeroed and have

     *the same scale.

     *

     *This does not need to be called every time step, but can be if

     *the accelerometer data are available at the same rate as the

     *rate gyro measurement.

     *

     *For a two-axis accelerometer mounted perpendicular to the rotation

     *axis, we can compute the angle for the full 360 degree rotation

     *with no linearization errors by using the arctangent of the two

     *readings.

     *

     * Ascommented in state_update, the math here is simplified to

     *make it possible to execute on a small microcontroller with no

     *floating point unit.  It will be hard toread the actual code and

     *see what is happening, which is why there is this extensive

     *comment block.

     *

     *The C matrix is a 1x2 (measurements x states) matrix that

     * isthe Jacobian matrix of the measurement value with respect

     * tothe states.  In this case, C is:

     *

     *    C = [ d(angle_m)/d(angle)  d(angle_m)/d(gyro_bias) ]

     *      = [1 0 ]

     *

     *because the angle measurement directly corresponds to the angle

     *estimate and the angle measurement has no relation to the gyro

     *bias.

     */

    void

    kalman_update(

           constfloat             ax_m,     /* X acceleration */

           constfloat             az_m       /* Z acceleration */

    )

    {

           /*Compute our measured angle and the error in our estimate */

    //以下代码更改成白色,下载后恢复成其他颜色即可看到

           constfloat             angle_m = atan2( -az_m,ax_m );

           constfloat             angle_err = angle_m -angle;

     

           /*

            * C_0 shows how the state measurement directlyrelates to

            * the state estimate.

           *

            * The C_1 shows that the state measurement doesnot relate

            * to the gyro bias estimate.  We don't actually use this, so

            * we comment it out.

            */

           constfloat             C_0 = 1;

           /*const float          C_1 = 0; */

     

           /*

            * PCt<2,1> = P<2,2> *C'<2,1>, which we use twice.  Thismakes

            * it worthwhile to precompute and store thetwo values.

            * Note that C[0,1] = C_1 is zero, so we do notcompute that

            * term.

            */

           constfloat             PCt_0 = C_0 * P[0][0];/* + C_1 * P[0][1] = 0 */

           constfloat             PCt_1 = C_0 * P[1][0];/* + C_1 * P[1][1] = 0 */

                 

           /*

            * Compute the error estimate.  From the Kalman filter paper:

            *

            *    E =C P C' + R

            *

            * Dimensionally,

            *

            *    E<1,1>= C<1,2> P<2,2> C'<2,1> + R<1,1>

            *

            * Again, note that C_1 is zero, so we do notcompute the term.

            */

           constfloat             E =

                  R_angle

                  +C_0 * PCt_0

           /*    + C_1 * PCt_1 = 0 */

           ;

     

           /*

            * Compute the Kalman filter gains.  From the Kalman paper:

            *

            *    K =P C' inv(E)

            *

            * Dimensionally:

            *

            *    K<2,1>= P<2,2> C'<2,1> inv(E)<1,1>

            *

            * Luckilly, E is <1,1>, so the inverseof E is just 1/E.

            */

           constfloat             K_0 = PCt_0 / E;

           constfloat             K_1 = PCt_1 / E;

                 

           /*

            * Update covariance matrix.  Again, from the Kalman filter paper:

            *

            *    P =P - K C P

            *

            * Dimensionally:

            *

            *    P<2,2>-= K<2,1> C<1,2> P<2,2>

            *

            * We first compute t<1,2> = C P.  Note that:

            *

            *    t[0,0]= C[0,0] * P[0,0] + C[0,1] * P[1,0]

            *

            * But, since C_1 is zero, we have:

            *

            *    t[0,0]= C[0,0] * P[0,0] = PCt[0,0]

            *

            * This saves us a floating point multiply.

            */

           constfloat             t_0 = PCt_0; /* C_0 *P[0][0] + C_1 * P[1][0] */

           constfloat             t_1 = C_0 * P[0][1]; /*+ C_1 * P[1][1]  = 0 */

     

           P[0][0]-= K_0 * t_0;

           P[0][1]-= K_0 * t_1;

           P[1][0]-= K_1 * t_0;

           P[1][1]-= K_1 * t_1;

          

           /*

            * Update our state estimate.  Again, from the Kalman paper:

            *

            *    X +=K * err

            *

            * And, dimensionally,

            *

            *    X<2>= X<2> + K<2,1> * err<1,1>

            *

            * err is a measurement of the difference inthe measured state

            * and the estimate state.  In our case, it is just the difference

            * between the two accelerometer measured angleand our estimated

            * angle.

            */

           angle       += K_0 * angle_err;

           q_bias     += K_1 * angle_err;

    }

     

    展开全文
  • 看到有人说互补滤波跟卡尔曼滤波效果差不多,我是不相信的。不过单我自己说不行,要有结果,后来我就用下面的代码进行测试,结果如下图<ignore_js_op>结果:果然两滤波结果几乎相同。黑色曲线是互补滤波结果,...
  • 基于互补滤波算法的四轴飞行控制系统设计.pdf
  • 融合磁力计的Mahony互补滤波算法

    千次阅读 2019-04-06 15:41:45
    ...
  • 1、卡尔曼滤波函数 void Kalman_Filter_X(float Accel, float Gyro) { Angle_X_Final += (Gyro - Q_bias_x) * dt; //先验估计 Pdot[0] = Q_angle - PP[0][1] - PP[1][0]; //Pk-先验估计误差协方差的微分 Pdot[1...
  • 这是总体的滤波效果 这是放大的版的 可以看出,卡尔曼和二阶基本在重合在一起,一阶也还好,也没有差得太远。 这里是从一放在桌子上一段时间后再进行测试的,可以看出一阶的跟随性上面要比其他二个都要好。...
  • 四旋翼姿态解算——互补滤波算法及理论推导

    万次阅读 多人点赞 2017-02-24 17:37:11
    这次来介绍一些实际的姿态解算算法吧! 一般在程序中,姿态解算的方式...互补滤波算法: 顾名思义,是多组数据结合互补,并进行滤波处理稳定输出,得到姿态的算法。而我们使用的传感器就是加速度计和陀螺仪。加速度计
  • 滤波原理 滤波主要过程 预备知识 预测 校正 加速度计校正 磁力计校正 更新四元数 源码分析 主程序运行流程图 函数功能简述 源码分析 头文件 using @@@ namespace attitude_estimator_q 类定义: class...
  • 有人问我关于四元数姿态解算算法的分析,每次都解释好久,今日空闲,特发一帖,供大家参考。本分析将结合程序,分析...http://blog.csdn.net/u013608300/article/details/52459515四轴之互补滤波与四元数算法最详...
  • 滤波方法及其原理, 卡尔曼滤波,互补滤波
  • 互补滤波

    2015-05-10 13:28:53
    --这个算法在短时间内信陀螺仪(动态特性好),长时间是加速度计的加权平均值------很经典的补偿算法。 --增大第一个参数,减小第二个参数滤波效果好,但会导致收敛慢、有一定延时;反之,滤波的动态特性好,但滤波...
  • 卓大大,请问您可以针对卡尔曼滤波、互补滤波、清华滤波的原理与异同做一个推送吗?最近在查阅相关的资料学习滤波算法,感觉好难理解呀/::< ▲ 平衡双轮自行车 正好昨天结束了本学期的“信号与系统”课程的最后...
  • 卡尔曼滤波和互补滤波参考程序

    热门讨论 2012-03-13 16:51:42
    加速度传感器的角度和陀螺仪的角速度通过卡尔曼滤波或者互补滤波得到稳定的输出波形,本资源就是关于这方面的C语言代码...
  •  介绍两种算法前,先定义两个坐标系。导航坐标系(参考坐标系)n,选取东北天右手直角坐标系作为导航坐标系n、载体坐标... 互补滤波算法:通过加速度计的输出稳定性来修正陀螺仪的积分漂移误差。(既通过修正陀螺仪...
  • 二阶互补滤波

    2014-04-29 20:54:57
    二阶互补滤波
  • 互补滤波程序

    2012-03-07 23:25:07
    互 补滤 波 程 序 互补滤波源代码 亲测
  • 互补滤波

    2017-09-14 16:16:33
    最近在研究小四轴的飞行,姿态检测主要用到的传感器是MPU6050。从MPU6050读出来的加速度和角速度数据最后要转成姿态,可以转换成...这里介绍圆点博士小四轴飞控开源代码关于四元数姿态融合的算法以及代码实现,不能...
  • 3阶互补滤波

    2018-05-25 16:53:13
    讲解了ArduPilot中AP_TECS库中的用于估计高度和垂直方向上的爬升速率的三阶互补滤波的实现 参考的paper
  • 互补滤波参考程序

    2014-01-04 20:27:32
    卡尔曼滤波程序的一种,欢迎对滤波程序有要求的来参考

空空如也

1 2 3 4 5 ... 16
收藏数 301
精华内容 120
关键字:

互补滤波