Mahony 互补滤波算法
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 的应用场景为 多旋翼无人机的姿态估计。
陀螺仪、加速度计、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 (k−1)+q ˙ b e (k)Δt
校正
在预测环节得到的四元数 q b e (k) ，通过加速度计和磁力计的值进行校正。该环节通常分为两部分：
- 通过加速度计得到 Δq acc ˆ ，然后校正四元数中的横滚（roll）和俯仰（pitch）分量。
- 当磁力计可读时，通过 Δq mag ˆ 校正四元数中的偏航（yaw）分量。
加速度计校正
加速度计信号首先经过低通滤波器（消除高频噪声）：
y(k)=RCT+RC y(k−1)+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)+T⋅f(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 | 振动水平报警阈值 |
主程序运行流程图：
函数功能简述
-
AttitudeEstimatorQ::AttitudeEstimatorQ();
构造函数，初始化参数;
-
AttitudeEstimatorQ::~AttitudeEstimatorQ();
析构函数，杀掉所有任务;
-
int AttitudeEstimatorQ::start();
启动【attitude_estimator_q】进程，主函数入口： task_main_trampoline
-
void AttitudeEstimatorQ::print();
打印姿态信息；
-
void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
{
attitude_estimator_q::instance->task_main();
}
-
void AttitudeEstimatorQ::task_main()；
主任务进程;
-
void AttitudeEstimatorQ::update_parameters(bool force);
false: 查看参数是否更新；
true: 获取新参数, 并由磁偏角更新四元数；
-
bool AttitudeEstimatorQ::init();
由加速度计和磁力计初始化旋转矩阵，有GPS时，校正磁偏角。
-
bool AttitudeEstimatorQ::update(float dt);
调用init(); 互补滤波
-
void AttitudeEstimatorQ::update_mag_declination(float new_declination);
使用磁偏角更新四元数
-
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 @me.com>
*/
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 1
- 2
- 3
- 4
- 5
- 6
- 7
头文件
（l42~l76）
#include //add missing check;
#include //视觉位置估计, 未找到文件【待查】;
#include //mocap-->vicon;
#include
- 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, ¶m_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();
-
k 为导航坐标系（NED）的 z 轴（D）在机体坐标系中的表示;
导航系中，D正方向朝下;
-
i 为导航坐标系（NED）的 x 轴（N）在机体坐标系;
i = (_mag - k * (_mag * k)); //施密特正交化;
β 2 =α 2 −(α 2 ,β 1 )(β 1 ,β 1 ) ⋅β 1//因 向量 k 已归一化，故分母等于1;
-
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] ⎤ ⎦ ⎥ -
构造旋转矩阵 R
R.set_row(0, i); R.set_row(1, j); R.set_row(2, k);
R=⎡ ⎣ ⎢ ijk ⎤ ⎦ ⎥ -
转换为四元数 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();
-
init();//执行一次;
由加速度计和磁力计初始化旋转矩阵和四元数;
-
mag_earth = _q.conjugate(_mag);
不使用外部航向，或外部航向异常时，采用磁力计校准;
将磁力计读数从机体坐标系转换到导航坐标系;
R n b ∗_mag -
mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
将磁力计的读数由向量换算到角度; 该角度减去磁偏，得到磁场偏差;
_mag_decl 由GPS得到;
**atan2f: 2 表示两个输入参数; 支持四象限内角度换算; 输出弧度值;
**_wrap_pi: 将(0~2pi)的角度映射到(-pi~pi); -
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
将磁场偏差转换到机体坐标系，并乘以其对应权重;
R b n ⎡ ⎣ ⎢ 00−mag_err ⎤ ⎦ ⎥ ⋅_w m ag -
_q.normalize();
四元数归一化;
这里的归一化用于校正update_mag_declination后的偏差。 -
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×[机体测量重力加速度] -
_gyro_bias += corr * (_w_gyro_bias * dt);
得到陀螺仪修正值
此处修正值为 mahony 滤波的 pi 控制器的 积分部分;
因为 _gyro_bias 不归零，故不断累积;
gyro_bias+=[Mag∗w mag +Acc∗w acc ]∗w gyro ∗dt -
_rates = _gyro + _gyro_bias;
_rates 表示角速度;
ω=ω g +δ -
corr += _rates;
这里的 corr = (Ea+Em) + (Ea+Em)*dt + gyro;
个人认为这里的 corr 才是更新后的角速度; -
_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
- 应用场景
- 名词解释
- 陀螺仪
- 加速度计
- 磁力计
- 坐标系
- 姿态表示
- mahony 滤波原理
- mahony 滤波主要过程
- 预备知识
- 预测
- 校正
- 加速度计校正
- 磁力计校正
- 无 GPS 校准时
- 有 GPS 校准时
- 更新四元数
- 源码分析
- 主程序运行流程图
- 函数功能简述
- 源码分析
- 头文件
- using
- namespace attitude_estimator_q
- 类定义 class AttitudeEstimatorQ
- 构造函数
- 析构函数
- start
- task_main_trampoline
- task_main
- update_parameters
- init
- update
- update_mag_declination
- attitude_estimator_q_main