by luoshi006
上接【互补滤波器】,继续学习互补滤波。。。。
参考: Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs. PX4/Pixhawk—uORB深入理解和应用
应用场景
本文中 mahony 的应用场景为 多旋翼无人机 的姿态估计 。
陀螺仪、加速度计、MPU6050 详述,请参考:传送门
#名词解释
陀螺仪
陀螺仪,测量角速度,具有高动态特性,它是一个间接测量角度的器件。它测量的是角度的导数,即角速度,要将角速度对时间积分才能得到角度。由于噪声等误差影响,在积分作用下不断积累,最终导致陀螺仪的低频干扰和漂移。
加速度计
输出当前加速度(包含重力加速度
g
g
g )的方向【也叫重力感应器】,在悬停时,输出为
g
g
g 。由其测量原理导致的高频信号敏感,使得加速度计在振动环境中高频干扰较大。
磁力计
输出为当前机体与地磁场的夹角。测量原理与指南针相似。低频特性较好,易受周围磁场干扰。 磁力计的工作原理参考:WalkAnt的博客
坐标系
导航坐标系 :在多旋翼中,又叫地球坐标系、地理坐标系。通常,采用北东地(NED)构成坐标系的
X
,
Y
,
Z
X, Y, Z
X , Y , Z 轴。
机体坐标系 :固联在多旋翼飞行器上,即,坐标系原点定位于飞行器中心点(假设中心点与重心点重合)。
关于航空导航用到的坐标系,请参考 AIAA 系列丛书。在多旋翼中,因为只在低空飞行,且时间较短,只需要以上两个。
姿态表示
欧拉角 :较直观,描述刚体在三维欧式空间中的姿态。此时,坐标系为机体坐标系,随着刚体的旋转而旋转。缺点:万向节锁。 详情参加:Wikipedia 传送门
四元数 :由一组四维向量,表示刚体的三维旋转。适合用于计算机计算。 详情参加:Wikipedia 传送门
方向余弦矩阵DCM :用欧拉角余弦值或四元数,表示的坐标系的旋转。
mahony 滤波原理
互补滤波要求两个信号的干扰噪声处在不同的频率,通过设置两个滤波器的截止频率,确保融合后的信号能够覆盖需求频率。 在 IMU 的姿态估计中,互补滤波器对陀螺仪(低频噪声)使用高通滤波;对加速度/磁力计(高频噪声)使用低频滤波。 (此处尚未对传感器数据实测,噪声和有用频率未知。。。。待后期补足)
互补滤波中,两个滤波器的截止频率一致,此时就需要根据有用频率的值对其进行取舍。
机体水平时,加速度计无法测量绕 Z 轴的旋转量,即偏航角。磁力计也有同样问题,无法测得要磁轴的旋转量。故,需要加速度计和磁力计同时对陀螺仪进行校正。
mahony 滤波主要过程
q
^
˙
=
1
2
q
^
⊗
P
(
Ω
‾
+
δ
)
−
−
−
−
−
−
−
−
(
a
)
\dot{\hat{q}}=\frac{1}{2} \hat{q} \otimes \mathbf P(\overline \Omega +\delta)--------(a)
q ^ ˙ = 2 1 q ^ ⊗ P ( Ω + δ ) − − − − − − − − ( a )
δ
=
k
P
⋅
e
+
k
I
⋅
∫
e
−
−
−
−
−
−
−
−
(
b
)
\delta = k_P \cdot e+k_I \cdot \int e --------(b)
δ = k P ⋅ e + k I ⋅ ∫ e − − − − − − − − ( b )
e
=
v
‾
×
v
^
−
−
−
−
−
−
−
−
−
−
−
−
(
c
)
e=\overline v \times \hat v------------(c)
e = v × v ^ − − − − − − − − − − − − ( c )
式中,
q
^
\hat q
q ^ 表示系统姿态估计的四元数表示;
δ
\delta
δ 是经过 PI 调节器产生的新息;
e
e
e 表示实测的惯性向量
v
‾
\overline v
v 和预测的向量
v
^
\hat v
v ^ 之间的相对旋转(误差)。
P
(
⋅
)
\mathbf P(\cdot)
P ( ⋅ ) —— pure quaternion operator(四元数实部为0),表示只有旋转。
PI 调节器中 :[20160901更新]
参数
k
p
k_p
k p 用于控制加速度计和陀螺仪之间的交叉频率 ; 参数
k
I
k_I
k I 用于校正陀螺仪误差 。
预备知识
主要是公式,不包含推导过程。。。。
欧拉角与机体角速度的关系:
Θ
˙
=
W
b
ω
\dot \Theta = W {}^b \omega
Θ ˙ = W b ω
=
[
1
t
a
n
θ
s
i
n
ϕ
t
a
n
θ
c
o
s
ϕ
0
c
o
s
ϕ
−
s
i
n
ϕ
0
s
i
n
ϕ
/
c
o
s
θ
c
o
s
ϕ
/
c
o
s
θ
]
b
ω
=\begin{bmatrix}1 & tan\theta sin\phi & tan\theta cos\phi \\ 0 & cos \phi &-sin\phi \\ 0 & sin\phi /cos \theta & cos \phi /cos \theta\end{bmatrix}{}^b \omega
= ⎣ ⎡ 1 0 0 t a n θ s i n ϕ c o s ϕ s i n ϕ / c o s θ t a n θ c o s ϕ − s i n ϕ c o s ϕ / c o s θ ⎦ ⎤ b ω
旋转矩阵与机体角速度的关系:
R
b
e
˙
=
R
b
e
[
b
ω
]
×
\dot{R^e_b}=R^e_b[{}^b \omega]_×
R b e ˙ = R b e [ b ω ] ×
四元数与机体角速度的关系 :
q
˙
e
b
(
t
)
=
1
2
[
0
−
ω
x
−
ω
y
−
ω
z
ω
x
0
ω
z
−
ω
y
ω
y
−
ω
z
0
ω
x
ω
z
ω
y
−
ω
x
0
]
q
e
b
(
t
)
−
−
−
−
−
−
−
(
1
)
\dot q^b_e(t)=\frac{1}{2} \begin{bmatrix}0 & -\omega _x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix}q^b_e(t) -------(1)
q ˙ e b ( t ) = 2 1 ⎣ ⎢ ⎢ ⎡ 0 ω x ω y ω z − ω x 0 − ω z ω y − ω y ω z 0 − ω x − ω z − ω y ω x 0 ⎦ ⎥ ⎥ ⎤ q e b ( t ) − − − − − − − ( 1 )
参考:北航全权老师课件 第五章内容、惯性导航(秦永元)第九章内容。
预测
与卡尔曼滤波相似,mahony 滤波也分为预测-校正。 在预测环节,由三轴陀螺仪 测得的角速度,通过式(1)计算出四元数姿态预测。
q
e
b
q^b_e
q e b 表示从地球坐标系到机体坐标系,或机体坐标系姿态在地球坐标系下的表示。
q
e
b
(
k
)
=
q
e
b
(
k
−
1
)
+
q
˙
e
b
(
k
)
Δ
t
q^b_e(k)=q^b_e(k-1)+\dot q^b_e(k)\Delta t
q e b ( k ) = q e b ( k − 1 ) + q ˙ e b ( k ) Δ t
校正
在预测环节得到的四元数
q
e
b
(
k
)
q^b_e(k)
q e b ( k ) ,通过加速度计和磁力计的值进行校正。该环节通常分为两部分:
通过加速度计得到
Δ
q
a
c
c
^
\widehat{\Delta q_{acc}}
Δ q a c c
,然后校正四元数中的横滚(roll)和俯仰(pitch)分量。 当磁力计可读时,通过
Δ
q
m
a
g
^
\widehat{\Delta q_{mag}}
Δ q m a g
校正四元数中的偏航(yaw)分量。
加速度计校正
加速度计信号首先经过低通滤波器(消除高频噪声):
y
(
k
)
=
R
C
T
+
R
C
y
(
k
−
1
)
+
T
T
+
R
C
x
(
k
)
y(k)=\frac{RC}{T+RC}y(k-1)+\frac{T}{T+RC}x(k)
y ( k ) = T + R C R C y ( k − 1 ) + T + R C T x ( k )
然后,对得到的结果进行归一化(normalized)
Δ
q
a
c
c
^
=
Δ
q
a
c
c
‾
∣
∣
Δ
q
a
c
c
‾
∣
∣
\widehat{\Delta q_{acc}}=\frac{\overline{\Delta q_{acc}}}{||\overline{\Delta q_{acc}}||}
Δ q a c c
= ∣ ∣ Δ q a c c ∣ ∣ Δ q a c c
计算偏差:
e
=
Δ
q
a
c
c
^
×
v
e=\widehat{\Delta q_{acc}} \times v
e = Δ q a c c
× v
式中,
v
v
v 表示重力向量在机体坐标系的向量。
[
v
x
v
y
v
z
]
=
C
n
b
[
0
0
1
]
\begin{bmatrix}v_x\\ v_y \\v_z\end{bmatrix}=C^b_n\begin{bmatrix}0\\ 0 \\1\end{bmatrix}
⎣ ⎡ v x v y v z ⎦ ⎤ = C n b ⎣ ⎡ 0 0 1 ⎦ ⎤
=
[
2
(
q
1
q
3
−
q
0
q
2
)
2
(
q
2
q
3
+
q
0
q
1
)
q
0
2
−
q
1
2
−
q
2
2
+
q
3
2
]
=\begin{bmatrix}2(q_1q_3-q_0q_2)\\ 2(q_2q_3+q_0q_1) \\q^2_0-q^2_1-q^2_2+q^2_3\end{bmatrix}
= ⎣ ⎡ 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 ⎦ ⎤
此时,由
v
v
v 与加速度计向量垂直分量叉乘,得到误差值。【两个物理意义相同的向量,理论上叉乘为零】
磁力计校正
数据预处理与加速度计相同,先滤波,然后归一化,得到
Δ
q
m
a
g
^
\widehat{\Delta q_{mag}}
Δ q m a g
。
1. 无 GPS 校准时:
计算误差:
e
=
Δ
q
m
a
g
^
×
w
e=\widehat{\Delta q_{mag}} \times w
e = Δ q m a g
× w
式中,
w
w
w 计算过程如下:
磁力计的输出(
m
m
m )在机体坐标系下,将其转换到导航坐标系:
[
h
x
h
y
h
z
]
=
C
b
n
[
m
x
m
y
m
z
]
\begin{bmatrix}h_x\\ h_y \\h_z\end{bmatrix}=C^n_b\begin{bmatrix}m_x\\ m_y \\m_z\end{bmatrix}
⎣ ⎡ h x h y h z ⎦ ⎤ = C b n ⎣ ⎡ m x m y m z ⎦ ⎤
导航坐标系的
x
x
x 轴与正北对齐,故,可以将磁力计在
x
o
y
xoy
x o y 平面的投影折算到
x
x
x 轴。
b
x
=
h
x
2
+
h
y
2
b_x=\sqrt{h^2_x+h^2_y}
b x = h x 2 + h y 2
b
z
=
h
z
b_z=h_z
b z = h z
再次变换到机体坐标系:
[
w
x
w
y
w
z
]
=
C
n
b
[
b
x
0
b
z
]
\begin{bmatrix}w_x\\ w_y \\w_z\end{bmatrix}=C^b_n\begin{bmatrix}b_x\\ 0\\b_z\end{bmatrix}
⎣ ⎡ w x w y w z ⎦ ⎤ = C n b ⎣ ⎡ b x 0 b z ⎦ ⎤
2. 有 GPS 校准时:
在 px4 中,磁力计使用 GPS 信息
[
0
,
0
,
m
a
g
]
[0, 0, mag]
[ 0 , 0 , m a g ] 进行校准,故,公式与加速度计相同。
[
w
x
w
y
w
z
]
=
C
n
b
[
0
0
m
a
g
]
\begin{bmatrix}w_x\\ w_y \\w_z\end{bmatrix}=C^b_n\begin{bmatrix}0\\ 0 \\mag\end{bmatrix}
⎣ ⎡ w x w y w z ⎦ ⎤ = C n b ⎣ ⎡ 0 0 m a g ⎦ ⎤
此时,由
w
w
w 与磁力计向量叉乘,得到误差值。【两个物理意义相同的向量,理论上叉乘为零】
##更新四元数
由加速度计和磁力计校准得到的误差值:
e
=
e
a
c
c
+
e
m
a
g
e=e_{acc}+e_{mag}
e = e a c c + e m a g
由该误差值得到修正值:(只有 ki 修正bias)
δ
=
K
I
⋅
∫
e
d
t
\delta =K_I \cdot \int e dt
δ = K I ⋅ ∫ e d t
修正后的角速度值:
ω
=
ω
g
y
r
o
+
δ
\omega = \omega_{gyro}+\delta
ω = ω g y r o + δ
根据一阶龙格库塔方法求解一阶微分方程:
q
˙
=
f
(
q
,
ω
)
\dot q = f(q,\omega)
q ˙ = f ( q , ω )
q
(
t
+
T
)
=
q
(
t
)
+
T
⋅
f
(
q
,
ω
)
q(t+T)=q(t)+T \cdot f(q,\omega)
q ( t + T ) = q ( t ) + T ⋅ f ( q , ω )
可以求出四元数微分方程的差分形式:
q
0
(
t
+
T
)
=
q
0
(
t
)
+
T
2
[
−
ω
x
q
1
(
t
)
−
ω
y
q
2
(
t
)
−
ω
z
q
3
(
t
)
]
q_0(t+T)=q_0(t)+\frac{T}{2}[-\omega _xq_1(t)-\omega _yq_2(t)-\omega_zq_3(t)]
q 0 ( t + T ) = q 0 ( t ) + 2 T [ − ω x q 1 ( t ) − ω y q 2 ( t ) − ω z q 3 ( t ) ]
四元数规范化:
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
q
0
2
+
q
1
2
+
q
2
2
+
q
3
2
q=\frac{q_0+q_1i+q_2j+q_3k}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}}
q = q 0 2 + q 1 2 + q 2 2 + q 3 2
q 0 + q 1 i + q 2 j + q 3 k
源码分析
该部分源码直接截取 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 <anton.babushkin@me.com>
*/
头文件
(l42~ l76)
#include <px4_config.h>
#include <px4_posix.h>
#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>
#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>
using @@@
(l78~ l82)
extern "C" __EXPORT int attitude_estimator_q_main ( int argc, char * argv[ ] ) ;
using math:: Vector;
using math:: Matrix;
using math:: Quaternion;
此处,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;
}
###类定义: class AttitudeEstimatorQ
( l92~ l210)
class AttitudeEstimatorQ
{
public :
AttitudeEstimatorQ ( ) ;
~ AttitudeEstimatorQ ( ) ;
int start ( ) ;
static void task_main_trampoline ( int argc, char * argv[ ] ) ;
void task_main ( ) ;
void print ( ) ;
private :
static constexpr float _dt_max = 0.02 ;
bool _task_should_exit = false ;
int _control_task = - 1 ;
int _sensors_sub = - 1 ;
int _params_sub = - 1 ;
int _vision_sub = - 1 ;
int _mocap_sub = - 1 ;
int _airspeed_sub = - 1 ;
int _global_pos_sub = - 1 ;
orb_advert_t _att_pub = nullptr ;
orb_advert_t _ctrl_state_pub = nullptr ;
orb_advert_t _est_state_pub = nullptr ;
struct {
param_t w_acc;
param_t w_mag;
param_t w_ext_hdg;
param_t w_gyro_bias;
param_t mag_decl;
param_t mag_decl_auto;
param_t acc_comp;
param_t bias_max;
param_t vibe_thresh;
param_t ext_hdg_mode;
} _params_handles;
float _w_accel = 0.0f ;
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 = { } ;
Vector< 3 > _vision_hdg;
att_pos_mocap_s _mocap = { } ;
Vector< 3 > _mocap_hdg;
airspeed_s _airspeed = { } ;
Quaternion _q;
Vector< 3 > _rates;
Vector< 3 > _gyro_bias;
vehicle_global_position_s _gpos = { } ;
Vector< 3 > _vel_prev;
Vector< 3 > _pos_acc;
DataValidatorGroup _voter_gyro;
DataValidatorGroup _voter_accel;
DataValidatorGroup _voter_mag;
math:: LowPassFilter2p _lp_roll_rate;
math:: LowPassFilter2p _lp_pitch_rate;
math:: LowPassFilter2p _lp_yaw_rate;
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 ;
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) ;
} ;
构造函数
(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");
}
析构函数
l240~ l262
AttitudeEstimatorQ:: ~ AttitudeEstimatorQ ( )
{
if ( _control_task != - 1 ) {
_task_should_exit = true ;
unsigned i = 0 ;
do {
usleep ( 20000 ) ;
if ( ++ i > 50 ) {
px4_task_delete ( _control_task) ;
break ;
}
} while ( _control_task != - 1 ) ;
}
attitude_estimator_q:: instance = nullptr ;
}
start();
l264~ l282
int AttitudeEstimatorQ:: start ( )
{
ASSERT ( _control_task == - 1 ) ;
_control_task = px4_task_spawn_cmd ( "attitude_estimator_q" ,
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;
}
print();
l284~ l292
void AttitudeEstimatorQ:: print ( )
{
warnx ( "gyro status:" ) ;
_voter_gyro. print ( ) ;
warnx ( "accel status:" ) ;
_voter_accel. print ( ) ;
warnx ( "mag status:" ) ;
_voter_mag. print ( ) ;
}
task_main_trampoline();
l294~ l297
void AttitudeEstimatorQ:: task_main_trampoline ( int argc, char * argv[ ] )
{
attitude_estimator_q:: instance- > task_main ( ) ;
}
task_main();
l299~ l655
void AttitudeEstimatorQ:: task_main ( )
{
#ifdef __PX4_POSIX
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
_sensors_sub = orb_subscribe ( ORB_ID ( sensor_combined) ) ;
_vision_sub = orb_subscribe ( ORB_ID ( vision_position_estimate) ) ;
_mocap_sub = orb_subscribe ( ORB_ID ( att_pos_mocap) ) ;
_airspeed_sub = orb_subscribe ( ORB_ID ( airspeed) ) ;
_params_sub = orb_subscribe ( ORB_ID ( parameter_update) ) ;
_global_pos_sub = orb_subscribe ( ORB_ID ( vehicle_global_position) ) ;
update_parameters ( true ) ;
hrt_abstime last_time = 0 ;
px4_pollfd_struct_t fds[ 1 ] = { } ;
fds[ 0 ] . fd = _sensors_sub;
fds[ 0 ] . events = POLLIN;
while ( ! _task_should_exit) {
int ret = px4_poll ( fds, 1 , 1000 ) ;
if ( ret < 0 ) {
usleep ( 10000 ) ;
PX4_WARN ( "Q POLL ERROR" ) ;
continue ;
} else if ( ret == 0 ) {
PX4_WARN ( "Q POLL TIMEOUT" ) ;
continue ;
}
update_parameters ( false ) ;
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) ) {
for ( unsigned i = 0 ; i < ( sizeof ( sensors. gyro_timestamp) / sizeof ( sensors. gyro_timestamp[ 0 ] ) ) ; i++ ) {
if ( sensors. gyro_timestamp[ i] > 0 ) {
float gyro[ 3 ] ;
for ( unsigned j = 0 ; j < 3 ; j++ ) {
if ( sensors. gyro_integral_dt[ i] > 0 ) {
gyro[ j] = ( double ) sensors. gyro_integral_rad[ i * 3 + j] / ( sensors. gyro_integral_dt[ i] / 1e6 ) ;
} else {
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] ) ;
}
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] ) ;
}
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] ) ;
}
}
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 ;
}
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 ) {
_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" ) ;
}
}
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 ;
}
}
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) ;
math:: Quaternion q ( _vision. q) ;
math:: Matrix< 3 , 3 > Rvis = q. to_dcm ( ) ;
math:: Vector< 3 > v ( 1.0f , 0.0f , 0.4f ) ;
_vision_hdg = Rvis. transposed ( ) * 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 ) ;
_mocap_hdg = Rmoc. transposed ( ) * v;
}
bool airspeed_updated = false ;
orb_check ( _airspeed_sub, & airspeed_updated) ;
if ( airspeed_updated) {
orb_copy ( ORB_ID ( airspeed) , _airspeed_sub, & _airspeed) ;
}
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 ) {
update_mag_declination ( math:: radians ( get_mag_declination ( _gpos. lat, _gpos. lon) ) ) ;
}
}
if ( _acc_comp && _gpos. timestamp != 0 && hrt_absolute_time ( ) < _gpos. timestamp + 20000 && _gpos. eph < 5.0f && _inited) {
if ( gpos_updated) {
Vector< 3 > vel ( _gpos. vel_n, _gpos. vel_e, _gpos. vel_d) ;
if ( _vel_prev_t != 0 && _gpos. timestamp != _vel_prev_t) {
float vel_dt = ( _gpos. timestamp - _vel_prev_t) / 1000000.0f ;
_pos_acc = _q. conjugate_inversed ( ( vel - _vel_prev) / vel_dt) ;
}
_vel_prev_t = _gpos. timestamp;
_vel_prev = vel;
}
} else {
_pos_acc. zero ( ) ;
_vel_prev. zero ( ) ;
_vel_prev_t = 0 ;
}
hrt_abstime now = hrt_absolute_time ( ) ;
float dt = ( last_time > 0 ) ? ( ( now - last_time) / 1000000.0f ) : 0.00001f ;
last_time = now;
if ( dt > _dt_max) {
dt = _dt_max;
}
if ( ! update ( dt) ) {
continue ;
}
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) ;
}
memcpy ( & att. rate_offsets, _gyro_bias. data, sizeof ( att. rate_offsets) ) ;
Matrix< 3 , 3 > R = _q. to_dcm ( ) ;
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 ;
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 ( ) ) ;
int att_inst;
orb_publish_auto ( ORB_ID ( vehicle_attitude) , & _att_pub, & att, & att_inst, ORB_PRIO_HIGH) ;
{
struct control_state_s ctrl_state = { } ;
ctrl_state. timestamp = sensors. timestamp;
ctrl_state. q[ 0 ] = _q ( 0 ) ;
ctrl_state. q[ 1 ] = _q ( 1 ) ;
ctrl_state. q[ 2 ] = _q ( 2 ) ;
ctrl_state. q[ 3 ] = _q ( 3 ) ;
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 ) ) ;
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 ;
}
int ctrl_inst;
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 ) ;
int est_inst;
orb_publish_auto ( ORB_ID ( estimator_status) , & _est_state_pub, & est, & est_inst, ORB_PRIO_HIGH) ;
}
}
}
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) ;
}
}
init();
k
k
k 为导航坐标系(NED)的
z
z
z 轴(D)在机体坐标系中的表示;
导航系中,D正方向朝下;
i
i
i 为导航坐标系(NED)的
x
x
x 轴(N)在机体坐标系;
i = (_mag - k * (_mag * k)); //施密特正交化;
β
2
=
α
2
−
(
α
2
,
β
1
)
(
β
1
,
β
1
)
⋅
β
1
\beta_2=\alpha_2-\frac{(\alpha_2,\beta_1)}{(\beta_1,\beta_1)} \cdot \beta_1
β 2 = α 2 − ( β 1 , β 1 ) ( α 2 , β 1 ) ⋅ β 1
//因 向量 k 已归一化,故分母等于1;
j
j
j 为导航坐标系(NED)的
y
y
y 轴(E)在机体坐标系;
j = k % i //叉乘求正交向量;
∣
i
j
k
k
[
0
]
k
[
1
]
k
[
2
]
i
[
0
]
i
[
1
]
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
]
]
\begin{vmatrix}i&j&k \\ k[0]&k[1]&k[2]\\ i[0]&i[1]&i[2] \end{vmatrix}=\begin{bmatrix}k[1]i[2]-i[1]k[2]\\ i[0]k[2]- k[0]i[2] \\ k[0]i[1]-i[0]k[1]\end{bmatrix}
∣ ∣ ∣ ∣ ∣ ∣ i k [ 0 ] i [ 0 ] j k [ 1 ] i [ 1 ] k k [ 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
R
R.set_row(0, i); R.set_row(1, j); R.set_row(2, k);
R
=
[
i
j
k
]
R=\begin{bmatrix}i\\j\\k\end{bmatrix}
R = ⎣ ⎡ i j k ⎦ ⎤
转换为四元数
q
q
q ,根据设定校正磁偏,归一化;
l688~ l728
bool AttitudeEstimatorQ:: init ( )
{
Vector< 3 > k = - _accel;
k. normalize ( ) ;
Vector< 3 > i = ( _mag - k * ( _mag * k) ) ;
i. normalize ( ) ;
Vector< 3 > j = k % i;
Matrix< 3 , 3 > R;
R. set_row ( 0 , i) ;
R. set_row ( 1 , j) ;
R. set_row ( 2 , k) ;
_q. from_dcm ( R) ;
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;
}
update();
init();//执行一次;
由加速度计和磁力计初始化 旋转矩阵和四元数 ;
mag_earth = _q.conjugate(_mag);
不使用外部航向,或外部航向异常时,采用磁力计校准; 将磁力计读数从机体坐标系转换到导航坐标系;
R
b
n
∗
_
m
a
g
R^n_b*\_mag
R b n ∗ _ m a g
mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
将磁力计的读数由向量换算到角度; 该角度减去磁偏,得到磁场偏差; _mag_decl 由GPS得到; **atan2f: 2 表示两个输入参数; 支持四象限内角度换算; 输出弧度值; **_wrap_pi: 将(02pi)的角度映射到(-pi pi);
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
将磁场偏差转换到机体坐标系,并乘以其对应权重;
R
n
b
[
0
0
−
m
a
g
_
e
r
r
]
⋅
_
w
m
a
g
R^b_n \begin{bmatrix}0 \\0 \\ -mag\_err \end{bmatrix} \cdot\_w_mag
R n b ⎣ ⎡ 0 0 − m a g _ e r r ⎦ ⎤ ⋅ _ w m a g
_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
n
b
[
0
0
1
]
k=R^b_n \begin{bmatrix}0\\0\\1 \end{bmatrix}
k = R n b ⎣ ⎡ 0 0 1 ⎦ ⎤
e
=
k
×
[
机
体
测
量
重
力
加
速
度
]
e=k \times[机体测量重力加速度]
e = k × [ 机 体 测 量 重 力 加 速 度 ]
_gyro_bias += corr * (_w_gyro_bias * dt);
得到陀螺仪修正值 此处修正值为 mahony 滤波的 pi 控制器的 积分部分; 因为 _gyro_bias 不归零,故不断累积;
g
y
r
o
_
b
i
a
s
+
=
[
M
a
g
∗
w
m
a
g
+
A
c
c
∗
w
a
c
c
]
∗
w
g
y
r
o
∗
d
t
gyro\_bias += [Mag * w_{mag}+Acc*w_{acc}]*w_{gyro}*dt
g y r o _ b i a s + = [ M a g ∗ w m a g + A c c ∗ w a c c ] ∗ w g y r o ∗ d t
_rates = _gyro + _gyro_bias;
_rates 表示角速度;
ω
=
ω
g
+
δ
\omega = \omega_g + \delta
ω = ω g + δ
corr += _rates;
这里的 corr = (Ea+Em) + (Ea+Em)*dt + gyro; 个人认为这里的 corr 才是更新后的角速度;
_q += _q.derivative(corr) * dt;
更新四元数,derivative为求导函数,使用一阶龙格库塔求导。
q
˙
=
1
2
q
ω
\dot q=\frac{1}{2} q \omega
q ˙ = 2 1 q ω
l730~ l817
bool AttitudeEstimatorQ:: update ( float dt)
{
if ( ! _inited) {
if ( ! _data_good) {
return false ;
}
return init ( ) ;
}
Quaternion q_last = _q;
Vector< 3 > corr;
if ( _ext_hdg_mode > 0 && _ext_hdg_good) {
if ( _ext_hdg_mode == 1 ) {
Vector< 3 > vision_hdg_earth = _q. conjugate ( _vision_hdg) ;
float vision_hdg_err = _wrap_pi ( atan2f ( vision_hdg_earth ( 1 ) , vision_hdg_earth ( 0 ) ) ) ;
corr + = _q. conjugate_inversed ( Vector< 3 > ( 0.0f , 0.0f , - vision_hdg_err) ) * _w_ext_hdg;
}
if ( _ext_hdg_mode == 2 ) {
Vector< 3 > mocap_hdg_earth = _q. conjugate ( _mocap_hdg) ;
float mocap_hdg_err = _wrap_pi ( atan2f ( mocap_hdg_earth ( 1 ) , mocap_hdg_earth ( 0 ) ) ) ;
corr + = _q. conjugate_inversed ( Vector< 3 > ( 0.0f , 0.0f , - mocap_hdg_err) ) * _w_ext_hdg;
}
}
if ( _ext_hdg_mode == 0 || ! _ext_hdg_good) {
Vector< 3 > mag_earth = _q. conjugate ( _mag) ;
float mag_err = _wrap_pi ( atan2f ( mag_earth ( 1 ) , mag_earth ( 0 ) ) - _mag_decl) ;
corr + = _q. conjugate_inversed ( Vector< 3 > ( 0.0f , 0.0f , - mag_err) ) * _w_mag;
}
_q. normalize ( ) ;
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 + = 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;
corr + = _rates;
_q + = _q. derivative ( corr) * dt;
_q. normalize ( ) ;
if ( ! ( PX4_ISFINITE ( _q ( 0 ) ) && PX4_ISFINITE ( _q ( 1 ) ) &&
PX4_ISFINITE ( _q ( 2 ) ) && PX4_ISFINITE ( _q ( 3 ) ) ) ) {
_q = q_last;
_rates. zero ( ) ;
_gyro_bias. zero ( ) ;
return false ;
}
return true ;
}
update_mag_declination();
l819~ l832
void AttitudeEstimatorQ:: update_mag_declination ( float new_declination)
{
if ( ! _inited || fabsf ( new_declination - _mag_decl) < 0.0001f ) {
_mag_decl = new_declination;
} else {
Quaternion decl_rotation;
decl_rotation. from_yaw ( new_declination - _mag_decl) ;
_q = decl_rotation * _q;
_mag_decl = new_declination;
}
}
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 ;
}
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 ;
}
文章目录
应用场景 **陀螺仪** **加速度计** **磁力计** **坐标系** **姿态表示**
mahony 滤波原理 mahony 滤波主要过程 预备知识 预测 校正 加速度计校正 磁力计校正 1. 无 GPS 校准时: 2. 有 GPS 校准时:
源码分析 这里写图片描述 函数功能简述 源码分析 头文件 using @@@ namespace attitude_estimator_q 构造函数 析构函数 start(); print(); task_main_trampoline(); task_main(); update_parameters(); init(); update(); update_mag_declination(); attitude_estimator_q_main();
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)