Madgwick AHRS算法笔记

最新推荐文章于 2024-06-22 16:32:32 发布
最新推荐文章于 2024-06-22 16:32:32 发布
阅读量1w 收藏 115
点赞数 11
分类专栏： 惯性导航 文章标签： Madgwick AHRS 梯度法
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/m0_37142194/article/details/81784761
版权

Madgwick AHRS算法笔记

引言

最近项目需要搞AHRS，重新读了一下Madgwick的AHRS算法论文[1]，在这里把笔记整理如下，如有错误，欢迎指正。

坐标系

1. 地理坐标系

地理坐标系原点位于载体质心，通常选取东北天坐标系，即其 x x 轴指向水平东方，其 y 轴指向水平北方，其 z z 轴沿地垂线向上。

2. 载体坐标系

载体坐标系原点位于载体质心，通常选取右前上坐标系，即其 x 轴沿载体横轴向右，其 y y 轴沿载体纵轴向前， 其 z 轴沿载体竖轴向上。

四元数

1. 四元数基本理论

1.1 定义

四元数是一种四维超复数，由一个实数单位加上三个虚数单位 i i ， j， k k 组成，其数学表达式如下：

(1)q^=q1+q2i+q3j+q4k=[q1q2q3q4],i2=j2=k2=ijk=−1

其中实数部分为四元数中的标量部分，虚数部分为四元数的矢量部分。

1.2 运算

  • 模值

    ∣∣q̂ ∣∣=q21+q22+q23+q24(2) (2) | q ^ | = q 1 2 + q 2 2 + q 3 2 + q 4 2

  • 共轭

    q̂ ∗=[q1−q2−q3−q4](3) (3) q ^ ∗ = [ q 1 − q 2 − q 3 − q 4 ]

  • 叉乘

    â ⨂b̂ =⎡⎣⎢⎢⎢⎢a1b1−a2b2−a3b3−a4b4a1b2+a2b1+a3b4−a4b3a1b3−a2b4+a3b1+a4b2a1b4+a2b3−a3b2+a4b1⎤⎦⎥⎥⎥⎥T(4) (4) a ^ ⨂ b ^ = [ a 1 b 1 − a 2 b 2 − a 3 b 3 − a 4 b 4 a 1 b 2 + a 2 b 1 + a 3 b 4 − a 4 b 3 a 1 b 3 − a 2 b 4 + a 3 b 1 + a 4 b 2 a 1 b 4 + a 2 b 3 − a 3 b 2 + a 4 b 1 ] T

    四元数叉乘并不满足交换律，但当 â  a ^ ， b̂  b ^ 为纯矢量，即 a1=0 a 1 = 0 ， b1=0 b 1 = 0 时，四元数叉乘等价于矢量叉乘，满足反交换律。

2. 四元数表示旋转

归一化的四元数可以用于表示三维空间中刚体或坐标系的旋转。假设空间任意向量 v̂  v ^ 在坐标系 A A 和坐标系 B B 中的投影分别为 [vAvxvAvyvAvz] 和 [BvxBvyBvz] [ v B v x v B v y v B v z ] ，在坐标系 A A 中可以找到这样的转轴 Ar^ 和转角 θ θ ，使得向量 v̂  v ^ 绕转轴旋转后的向量 v̂ ′ v ^ ′ 在坐标系 A A 中的投影为 [vBvxvBvyvBvz] 。这种旋转过程可以使用四元数 ABq̂  q ^ B A q ^ 表示，其数学表达式如下：

ABq̂ =[cosθ2−rxsinθ2−rysinθ2−rzsinθ2](5) (5) q ^ B A q ^ = [ c o s θ 2 − r x s i n θ 2 − r y s i n θ 2 − r z s i n θ 2 ]

其中上标 A A 表示参考坐标系，下标 B 表示目标坐标系， rx r x ， ry r y ， rz r z 表示转轴 Ar̂  A r ^ 在坐标系 A A 中的投影。对于逆向旋转，可用四元数的共轭表示：

(6)q^ABq^=q^BAq^∗=[q1−q2−q3−q4]

向量 v̂  v ^ 由坐标系 A A 转换到坐标系 B 的投影公式如下：

Bv̂ =ABq̂ ⨂Av̂ ⨂ABq̂ ∗(7) (7) v ^ B v ^ = q ^ B A q ^ ⁡ ⨂ v ^ A v ^ ⁡ ⨂ q ^ B A q ^ ∗

四元数 ABq̂  q ^ B A q ^ 所描述的旋转过程可以用旋转矩阵 ABR R B A R 表示，其数学公式如下：

Bv̂ =Av̂ ⋅ABRT=[Av̂ xAv̂ yAv̂ z]⎡⎣⎢⎢⎢2q21−1+2q222(q2q3−q1q4)2(q2q4+q1q3)2(q2q3+q1q4)2q21−1+2q232(q3q4−q1q2)2(q2q4−q1q3)2(q3q4+q1q2)2q21−1+2q24⎤⎦⎥⎥⎥T(8) (8) v ^ B v ^ = v ^ A v ^ ⋅ R T B A R T = [ v ^ A v ^ x v ^ A v ^ y v ^ A v ^ z ] [ 2 q 1 2 − 1 + 2 q 2 2 2 ( q 2 q 3 + q 1 q 4 ) 2 ( q 2 q 4 − q 1 q 3 ) 2 ( q 2 q 3 − q 1 q 4 ) 2 q 1 2 − 1 + 2 q 3 2 2 ( q 3 q 4 + q 1 q 2 ) 2 ( q 2 q 4 + q 1 q 3 ) 2 ( q 3 q 4 − q 1 q 2 ) 2 q 1 2 − 1 + 2 q 4 2 ] T

3. 四元数表示姿态

四元数可以表示刚体姿态，这里的姿态是指载体坐标系相对于地理坐标系的旋转。这种旋转过程在本文中使用四元数 SEq̂  q ^ E S q ^ 表示，其中上标 S S 表示载体坐标系即参考坐标系，下标 E 表示地理坐标系即目标坐标系。

姿态解算

1. 姿态的角速度更新方法

1.1 利用姿态求解线速度

考虑载体坐标系下的空间任意一定点 SR R S R ，将其映射到地理坐标系中，其数学表达如下：

ER=SEq̂ ⨂SR⨂SEq̂ ∗(9) (9) R E R = q ^ E S q ^ ⁡ ⨂ R S R ⁡ ⨂ q ^ ∗ E S q ^ ∗

对时间 t t 求导，可得点 RER 在地理坐标系中的线速度 Ev v E v ，其数学表达式如下：

Ev=ER˙=SEq˙⨂SR⨂SEq̂ ∗+SEq̂ ⨂SR⨂SEq˙∗(10) (10) v E v = R ˙ E R ˙ = q ˙ E S q ˙ ⁡ ⨂ R S R ⁡ ⨂ q ^ ∗ E S q ^ ∗ + q ^ E S q ^ ⁡ ⨂ R S R ⁡ ⨂ q ˙ ∗ E S q ˙ ∗

其中上标一点表示对时间 t t 求微分。

由式 (9) 可得：

SEq̂ ∗⨂ER=SR⨂SEq̂ ∗(11) (11) q ^ ∗ E S q ^ ∗ ⁡ ⨂ R E R = R S R ⁡ ⨂ q ^ ∗ E S q ^ ∗

ER⨂SEq̂ =SEq̂ ⨂SR(12) (12) R E R ⁡ ⨂ q ^ E S q ^ = q ^ E S q ^ ⁡ ⨂ R S R

将式 (11)(12) ( 11 ) ( 12 ) 带入式 (10) ( 10 ) 可得：

Ev=SEq˙⨂SEq̂ ∗⨂ER+ER⨂SEq̂ ⨂SEq˙∗(13) (13) v E v = q ˙ E S q ˙ ⁡ ⨂ q ^ ∗ E S q ^ ∗ ⁡ ⨂ R E R + R E R ⁡ ⨂ q ^ E S q ^ ⁡ ⨂ q ˙ ∗ E S q ˙ ∗

考查 SEq̂ ⨂SEq̂ ∗ q ^ E S q ^ ⁡ ⨂ q ^ ∗ E S q ^ ∗ 可得：

SEq̂ ⨂SEq̂ ∗=[1000](14) (14) q ^ E S q ^ ⁡ ⨂ q ^ ∗ E S q ^ ∗ = [ 1 0 0 0 ]

由式 (14) ( 14 ) 可得：

SEq˙⨂SEq̂ ∗+SEq̂ ⨂SEq˙∗=0(15) (15) q ˙ E S q ˙ ⁡ ⨂ q ^ ∗ E S q ^ ∗ + q ^ E S q ^ ⁡ ⨂ q ˙ ∗ E S q ˙ ∗ = 0

将式 (15) ( 15 ) 带入式 (13) ( 13 ) 可得：

Ev=SEq˙⨂SEq̂ ∗⨂ER−ER⨂SEq̂ ˙⨂SEq∗(16) (16) v E v = q ˙ E S q ˙ ⁡ ⨂ q ^ ∗ E S q ^ ∗ ⁡ ⨂ R E R − R E R ⁡ ⨂ q ^ ˙ E S q ^ ˙ ⁡ ⨂ q ∗ E S q ∗

考查 SEq˙⨂SEq̂ ∗ q ˙ E S q ˙ ⁡ ⨂ q ^ ∗ E S q ^ ∗ 标量部分可得：

Scale(SEq˙⨂SEq̂ ∗)=q0˙q0+q1˙q1+q2˙q2+q3˙q3=12ddt(q20+q21+q22+q23)=0(17) (17) S c a l e ( q ˙ E S q ˙ ⁡ ⨂ q ^ ∗ E S q ^ ∗ ) = q 0 ˙ q 0 + q 1 ˙ q 1 + q 2 ˙ q 2 + q 3 ˙ q 3 = 1 2 d d t ( q 0 2 + q 1 2 + q 2 2 + q 3 2 ) = 0

由式 (17) ( 17 ) 可知 SEq˙⨂SEq̂ ∗ q ˙ E S q ˙ ⁡ ⨂ q ^ E S q ^ ∗ 为纯矢量， Ev v E v 为矢量叉乘相减，而矢量叉乘又满足反交换律，因此式 (16) ( 16 ) 可重写为：

Ev=2SEq˙⨂SEq̂ ∗⨂ER(18) (18) v E v = 2 q ˙ E S q ˙ ⁡ ⨂ q ^ ∗ E S q ^ ∗ ⁡ ⨂ R E R

1.2 利用角速度求解线速度

另一方面，线速度可以由角速度叉乘位移得到，其数学表达如下：

Ev=Eω̂ ⨂ER(19) (19) v E v = ω ^ E ω ^ ⁡ ⨂ R E R

考虑到载体坐标系下的角速度 Sω ω S ω ，式 (19) ( 19 ) 可重写为：

Ev=SEq̂ ⨂Sω̂ ⨂SEq̂ ∗⨂ER(20) (20) v E v = q ^ E S q ^ ⁡ ⨂ ω ^ S ω ^ ⁡ ⨂ q ^ ∗ E S q ^ ∗ ⁡ ⨂ R E R

1.3 姿态更新方程

对比式 (18) ( 18 ) 和 (20) ( 20 ) 可得：

SEq˙=12SEq̂ ⨂Sω̂ (21) (21) q ˙ E S q ˙ = 1 2 q ^ E S q ^ ⁡ ⨂ ω ^ S ω ^

因此结合角速度，通过数值积分，可以求解角速度姿态四元数 SEqω,t q ω , t E S q ω , t ：

SEqω,t=SEq̂ est,t−1+12SEq̂ est,t−1⨂Sω̂ Δt=SEq̂ est,t−1+SEq˙ω,tΔt(22) (22) q ω , t E S q ω , t ⁡ = q ^ e s t , t − 1 E S q ^ e s t , t − 1 + 1 2 q ^ e s t , t − 1 E S q ^ e s t , t − 1 ⁡ ⨂ ω ^ S ω ^ ⁡ Δ t = q ^ e s t , t − 1 E S q ^ e s t , t − 1 + q ˙ ω , t E S q ˙ ω , t ⁡ Δ t

其中下标 est e s t 表示估计值，是融合角速度和向量观测求解的四元数，下标 t t 表示采样序号。

2. 姿态的向量观测更新方法

2.1 姿态最优化问题

假设 d^Ed^ 是大地坐标系下的某一参考矢量， Sŝ  s ^ S s ^ 是该向量在载体坐标系下的测量矢量。参考矢量 Ed̂  d ^ E d ^ 可以通过姿态四元数 SEq̂  q ^ E S q ^ 投影到载体坐标系下，其数学表达如下：

Sd̂ =SEq̂ ∗⨂Ed̂ ⨂SEq̂ (23) (23) d ^ S d ^ = q ^ ∗ E S q ^ ∗ ⁡ ⨂ d ^ E d ^ ⁡ ⨂ q ^ E S q ^

四元数共轭表示逆向旋转

投影矢量 Sd̂  d ^ S d ^ 与测量矢量 Sŝ  s ^ S s ^ 理论上相一致，因此姿态四元数 SEq̂  q ^ E S q ^ 应该满足如下条件：

minSEq̂ ∈ℜ4f(SEq̂ ,Ed̂ ,Sŝ )(24) (24) m i n q ^ E S q ^ ∈ R 4 f ( q ^ E S q ^ , d ^ E d ^ , s ^ S s ^ )

f(SEq̂ ,Ed̂ ,Sŝ )=Sd̂ −Sŝ =SEq̂ ∗⨂Ed̂ ⨂SEq̂ −Sŝ (25) (25) f ( q ^ E S q ^ , d ^ E d ^ , s ^ S s ^ ) = d ^ S d ^ − s ^ S s ^ = q ^ ∗ E S q ^ ∗ ⁡ ⨂ d ^ E d ^ ⁡ ⨂ q ^ E S q ^ − s ^ S s ^

笔者不太明白在上述最优化问题中，为什么目标函数 f(SEq̂ ,Ed̂ ,Sŝ ) f ( q ^ E S q ^ , d ^ E d ^ , s ^ S s ^ ) 是矢量而非标量。另外，由于由参考矢量 Ed̂  d ^ E d ^ 到投影矢量 Sŝ  s ^ S s ^ 到旋转过程不具有唯一性，因此用这种最优化方式求解的四元数并不一定是实际的姿态四元数。

式 (25) ( 25 ) 被称为目标函数。由式 (24) ( 24 ) 可知，姿态四元数求解被转化为了最优化问题中的极小值求解，利用梯度法求解极小值：

SEqk+1=SEq̂ k−μ∇f(SEq̂ ,Ed̂ ,Sŝ )||∇f(SEq̂ ,Ed̂ ,Sŝ )||,k=1,2,⋯,n(26) (26) q k + 1 E S q k + 1 = q ^ k E S q ^ k − μ ∇ f ( q ^ E S q ^ , d ^ E d ^ , s ^ S s ^ ) | | ∇ f ( q ^ E S q ^ , d ^ E d ^ , s ^ S s ^ ) | | , k = 1 , 2 , ⋯ , n

关于梯度 ∇f(SEq̂ ,Ed̂ ,Sŝ ) ∇ f ( q ^ E S q ^ , d ^ E d ^ , s ^ S s ^ ) 的具体数学表达式在Madgwick原文[1]的式 (20)∼(22) ( 20 ) ∼ ( 22 ) 中有详尽描述。

其中 μ μ 表示迭代步进。

2.2 姿态更新方程

为了使最优化方式求解的四元数唯一，我们需要引入两个参考矢量。在这里我们选取大地坐标系下的重力矢量 Eĝ =[0001] g ^ E g ^ = [ 0 0 0 1 ] 和地磁矢量在 XOZ X O Z 面的投影矢量 Eb̂ =[0bx0bz] b ^ E b ^ = [ 0 b x 0 b z ] 作为参考矢量。因此，目标函数可重写为：

f(SEq̂ ,Sâ ,Eb̂ ,Sm̂ )=[SEq̂ ∗⨂Eĝ ⨂SEq̂ −Sâ SEq̂ ∗⨂Eb̂ ⨂SEq̂ −Sm̂ ](27) (27) f ( q ^ E S q ^ , a ^ S a ^ , b ^ E b ^ , m ^ S m ^ ) = [ q ^ ∗ E S q ^ ∗ ⁡ ⨂ g ^ E g ^ ⁡ ⨂ q ^ E S q ^ − a ^ S a ^ q ^ ∗ E S q ^ ∗ ⁡ ⨂ b ^ E b ^ ⁡ ⨂ q ^ E S q ^ − m ^ S m ^ ]

其中 Sâ  a ^ S a ^ 表示重力矢量的测量值， Sm̂  m ^ S m ^ 表示磁力矢量的测量值。

bx≠0 b x ≠ 0 的情况下，四元数才有唯一解。

我们在每次采样时刻 t t 进行四元数迭代，可以求解向量观测姿态四元数 q∇,tESq∇,t：

SEq∇,t=SEq̂ est,t−1−μt∇f(SEq̂ ,Sâ ,Eb̂ ,Sm̂ )||∇f(SEq̂ ,Sâ ,Eb̂ ,Sm̂ )||(28) (28) q ∇ , t E S q ∇ , t = q ^ e s t , t − 1 E S q ^ e s t , t − 1 − μ t ∇ f ( q ^ E S q ^ , a ^ S a ^ , b ^ E b ^ , m ^ S m ^ ) | | ∇ f ( q ^ E S q ^ , a ^ S a ^ , b ^ E b ^ , m ^ S m ^ ) | |

μt=α||SEq˙ω,t||Δt,α>1(29) (29) μ t = α | | q ˙ ω , t E S q ˙ ω , t ⁡ | | Δ t , α > 1

其中 Δt Δ t 表示采样间隔， α α 表示增强因子。

3. 融合算法

融合上文中计算出的角速度姿态四元数 SEqω,t q ω , t E S q ω , t 和向量观测姿态四元数 SEq∇,t q ∇ , t E S q ∇ , t ，我们可以得到估计姿态四元数 SEqest,t q e s t , t E S q e s t , t ：

SEqest,t=γtSEq∇,t+(1−γt)SEqω,t,0≤γt≤1(30) (30) q e s t , t E S q e s t , t = γ t q ∇ , t E S q ∇ , t + ( 1 − γ t ) q ω , t E S q ω , t , 0 ≤ γ t ≤ 1

其中 γt γ t 表示融合加权系数。

对于融合加权系数 γt γ t 而言，一种比较好的取值策略定义如下：取值 γt γ t 使得 SEq∇,t q ∇ , t E S q ∇ , t 的收敛与 SEqω,t q ω , t E S q ω , t 的发散相当。

笔者不太明白这里的收敛和发散是否有具体的数学定义，个人将收敛理解为式 (28) ( 28 ) 中的减法运算，发散理解为式 (22) ( 22 ) 中的加法运算。

根据上述策略，可以推导出融合加权系数 γt γ t 的计算公式如下：

γtμtΔt=(1−γt)β(31) (31) γ t μ t Δ t = ( 1 − γ t ) β

γt=βμtΔt+β(32) (32) γ t = β μ t Δ t + β

其中 β β 是由陀螺仪测量误差估算出来一个增益系数。

β β 在3.3节会有说明。

现在将 μt μ t 中的增强因子 α α 取一个非常大的值，那么向量观测姿态四元数 SEq∇,t q ∇ , t E S q ∇ , t 可重写为：

SEq∇,t≈−μt∇f(SEq̂ ,Sâ ,Eb̂ ,Sm̂ )||∇f(SEq̂ ,Sâ ,Eb̂ ,Sm̂ )||(33) (33) q ∇ , t E S q ∇ , t ≈ − μ t ∇ f ( q ^ E S q ^ , a ^ S a ^ , b ^ E b ^ , m ^ S m ^ ) | | ∇ f ( q ^ E S q ^ , a ^ S a ^ , b ^ E b ^ , m ^ S m ^ ) | |

融合加权系数 γt γ t 可重写为：

γt≈βΔtμt(34) (34) γ t ≈ β Δ t μ t

将式 (22)(33)(34) ( 22 ) ( 33 ) ( 34 ) 带入式 (30) ( 30 ) 可得：

SEqest,t=βΔtμt(−μt∇f(SEq̂ ,Sâ ,Eb̂ ,Sm̂ )||∇f(SEq̂ ,Sâ ,Eb̂ ,Sm̂ )||)+(1−0)(SEq̂ est,t−1+SEq˙ω,tΔt)=SEq̂ est,t−1+SEq˙est,tΔt(35) (35) q e s t , t E S q e s t , t ⁡ = β Δ t μ t ( − μ t ∇ f ( q ^ E S q ^ , a ^ S a ^ , b ^ E b ^ , m ^ S m ^ ) | | ∇ f ( q ^ E S q ^ , a ^ S a ^ , b ^ E b ^ , m ^ S m ^ ) | | ) + ( 1 − 0 ) ( q ^ e s t , t − 1 E S q ^ e s t , t − 1 + q ˙ ω , t E S q ˙ ω , t ⁡ Δ t ) = q ^ e s t , t − 1 E S q ^ e s t , t − 1 + q ˙ e s t , t E S q ˙ e s t , t ⁡ Δ t

SEq˙est,t=SEq˙ω,t−βSEq̂ ˙ϵ,t(36) (36) q ˙ e s t , t E S q ˙ e s t , t = q ˙ ω , t E S q ˙ ω , t − β q ^ ˙ ϵ , t E S q ^ ˙ ϵ , t

SEq̂ ˙ϵ,t=∇f(SEq̂ ,Sâ ,Eb̂ ,Sm̂ )||∇f(SEq̂ ,Sâ ,Eb̂ ,Sm̂ )||(37) (37) q ^ ˙ ϵ , t E S q ^ ˙ ϵ , t = ∇ f ( q ^ E S q ^ , a ^ S a ^ , b ^ E b ^ , m ^ S m ^ ) | | ∇ f ( q ^ E S q ^ , a ^ S a ^ , b ^ E b ^ , m ^ S m ^ ) | |

理论上式 (35) ( 35 ) 第二项矢量权重应为 (1−βΔtμt) ( 1 − β Δ t μ t ) ，但由于 μt μ t 是一个非常大的值，为了简化运算这里取权重为 (1−0) ( 1 − 0 ) 。

其中 SEq˙est,t q ˙ e s t , t E S q ˙ e s t , t 表示估算姿态四元数导数， SEq̂ ˙ϵ,t q ^ ˙ ϵ , t E S q ^ ˙ ϵ , t 表示由陀螺仪测量误差引入的单位四元数导数。

3.1 磁场畸变补偿

磁力计易受外部环境干扰，其测量值可能会存在偏差。假设 Eĥ t h ^ t E h ^ t 大地坐标系下 t t 时刻的地磁测量矢量，其数学表达如下：

(38)h^Eh^t=q^est,t−1ESq^est,t−1⁡⨂m^Sm^t⁡⨂q^ESq^est,t−1∗

其中 SEq̂ est,t−1 q ^ e s t , t − 1 E S q ^ e s t , t − 1 表示 t−1 t − 1 时刻的姿态四元数， Sm̂ t m ^ t S m ^ t 表示载体坐标系下 t t 时刻的地磁测量值。我们使用下式对 t 时刻的地磁参考矢量进行补偿：

Eb̂ t=[0h2x+h2y‾‾‾‾‾‾‾√0hz](39) (39) b ^ E b ^ t = [ 0 h x 2 + h y 2 0 h z ]

这种方式可以补偿地磁矢量在倾斜角方向的畸变，使得地磁畸变仅影响姿态航向角。

3.2 陀螺仪偏置漂移补偿

由于温度和运动的影响，陀螺仪的偏置会存在漂移现象。Mahony的AHRS算法[2]中，陀螺仪偏置漂移的补偿利用误差积分消除。这里，我们采用相似的方法，其补偿公式如下：

Sωϵ,t=2SEq̂ ∗est,t−1⨂SEq̂ ˙ϵ,t(40) (40) ω S ω ϵ , t = 2 q ^ e s t , t − 1 ∗ E S q ^ e s t , t − 1 ∗ ⁡ ⨂ q ^ ˙ E S q ^ ˙ ϵ , t

Sωb,t=ζ∑Sωϵ,tΔt(41) (41) ω S ω b , t = ζ ∑ ω ϵ , t S ω ϵ , t ⁡ Δ t

Sωc,t=Sωt−Sωb,t(42) (42) ω S ω c , t = ω t S ω t − ω b , t S ω b , t

以重力矢量和地磁矢量解算的姿态为真实姿态 SEqreal,t q r e a l , t E S q r e a l , t ， SEq̂ ˙ϵ,t q ^ ˙ ϵ , t E S q ^ ˙ ϵ , t 近似于真实姿态四元数 SEqreal,t q r e a l , t E S q r e a l , t 与角速度姿态四元数 SEqω,t−1 q ω , t − 1 E S q ω , t − 1 之差，由此推导的 Sωϵ,t ω S ω ϵ , t 可以视作真实角度 Sωreal,t ω S ω r e a l , t 与测量角速度 Sωt ω S ω t 之差，通过积分负反馈便可以消除补偿角速度 Sωc,t ω S ω c , t 的静态误差（偏置漂移）。

3.3 滤波器增益系数

系数 β β 由陀螺仪零均值测量误差估算，零均值测量误差包括：传感器噪声，信号混叠，量化误差，校准误差，传感器失准，传感器轴非对称和一些频率响应特性。系数 ζ ζ 由陀螺仪非零均值测量误差估算，这种误差主要是指偏置漂移。

参考文献

[1] An efficient orientation filter for inertial and inertial/magnetic sensor arrays

[2] A complementary filter for attitude estimation of a fixed-wing UAV

会飞的鱼chelmx
关注
  • 11
    点赞
  • 踩
  • 115
    收藏
    觉得还不错? 一键收藏
  • 7
    评论
专栏目录
【运动传感器】Madgwick算法（上）
shenxiaolu1984的专栏
02-17 2万+
传感器融合姿态估计算法Madgwick中，推导单个传感器的使用方法。
madgwickAHRS算法
09-08
madgwick算法解算姿态，利用c语言解算，初学者可以看看
【51单片机快速入门指南】4.4.3：Madgwick AHRS 九轴姿态融合获取四元数、欧拉角
乙酸氧铍的博客
12-03 1949
目录传感器的方向源码Madgwick_9.cMadgwick_9.h使用方法测试main.c效果 STC15F2K60S2 22.1184MHz Keil uVision V5.29.0.0 PK51 Prof.Developers Kit Version:9.60.0.0 上位机：Vofa+ 1.3.10 传感器的方向 源码        所用MCU为STC15F2K60S2 使用内部RC时钟，22.1184MHz &nbsp
推荐开源项目：madgwick_py —— Python实现的Madgwick IMU和AHRS算法
gitblog_00010的博客
06-03 330
推荐开源项目：madgwick_py —— Python实现的Madgwick IMU和AHRS算法 madgwick_pyA Python implementation of Madgwick's IMU and AHRS algorithm.项目地址:https://gitcode.com/gh_mirrors/ma/madgwick_py 1、项目介绍 madgwick_py是一个Pyth...
【STM32】利用姿态传感器实现倾斜报警器
最新发布
jmqsPH的博客
06-22 498
以上代码是针对STM32F4系列微控制器的嵌入式C代码。它包括了一系列头文件的引用，定义了一些全局变量和中断服务函数。在main函数中，初始化了一些硬件设备如LED、MPU6050等，并进入一个主循环。主要功能是通过IMU6轴融合计算姿态角数据，并监测pitch角和roll角是否超过±60度，如果超过则触发蜂鸣器报警。定时通过串口打印姿态角数据。（说实话其实很多代码的编写都是复制粘贴的，但学会看懂别人的代码和在此基础上进行调用和改写，培养编程逻辑思维就算是学有所悟了吧。
AHRS开源融合算法
qq_39581383的博客
11-10 989
//===================================================================================================== // MadgwickAHRS.c //===================================================================================================== // // Implementation of Madgwi
【51单片机快速入门指南】4.3.4： MPU6050使用Madgwick AHRS算法实现六轴姿态融合获取四元数、欧拉角
乙酸氧铍的博客
11-26 1989
目录源码Madgwick_6.cMadgwick_6.h测试程序main.c效果 STC89C516 32MHz Keil uVision V5.29.0.0 PK51 Prof.Developers Kit Version:9.60.0.0 上位机：Vofa+ 1.3.10 移植自AHRS —— LOXO，算法作者：SOH Madgwick 源码        为了避免所用RAM超标，部分变量设为idata类型，移植时需注意。
AHRS 原理算法+代码实现（好文记录）
qq_40957243的博客
11-18 2637
MPU9250数据处理的过程 IMU误差模型和校准 AHRS关键算法 IMU矫正和姿态融合 加速度计和磁力计椭球拟合矫正 加速度计和磁力计求解初始姿态角度 KF 、EKF、ESKF的区别，前两个属于直接法滤波，最后一个属于间接法滤波 AHRS的EKF滤波算法 参考文献：A Double-Stage Kalman Filter for Orientation Tracking With An Integrated Processor in 9-D IMU（双阶段卡尔曼滤波器） AHRS 的三种算法对比，算法.
AHRS--APM飞控基于DCM算法的姿态及航向解算核心代码
10-27
AHRS--APM飞控基于DCM算法的姿态及航向解算核心代码
Madgwick 的 AHRS算法的Rust端口_rust_代码_下载
06-11
标题中的“Madgwick 的 AHRS 算法的 Rust 端口”指的是 Sebastian Madgwick 开发的 Attitude and Heading Reference System (AHRS) 算法在 Rust 编程语言中的实现。AHRS 算法是一种用于估计三维空间中物体的姿态（如...
madgwick_py:Madgwick的IMU和AHRS算法的Python实现
05-25
madgwick_py：Madgwick的IMU和AHRS算法的Python实现。 有关该算法的更多信息，请参见 。 该实现是在卡尔斯鲁厄技术学院的认知系统实验室（CSL）上完成的： ： 要求Python 3.x（经过Python 3.4测试） NumPy执照版权...
Madgwick算法代码.zip
10-21
采用Madgwick算法实现姿态解算，加速度计和磁强计采用梯度下降法用来补偿陀螺仪的误差。
PyMadgwickAHRS
05-16
PyMadgwickAHRS Madgwick的IMU和AHRS算法的实现。 请参阅： ： 感谢您提供如此精彩的信息。 该源代码分叉了用C编写的对象，并将其移植到Python。 北见洋 除非在源代码中提供了替代许可证，否则本网站上可用的开源资源是根据GNU通用公共许可证提供的。
madgwick_传感器融合算法_matlab
03-29
madgwick的传感器融合算法，matlab版本的
MadgwickAHRS:MadgwickAHRS算法的Arduino实现
05-14
麦格威克图书馆 该库包装了MadgwickAHRS算法的官方实现，以基于加速度计和陀螺仪读数获取对象的方向 执照 版权所有（c）Arduino LLC。 保留所有权利。 该库是免费软件。 您可以根据自由软件基金会（Free Software Foundation）发布的GNU通用公共许可协议（GNU Lesser General Public License）的条款重新分发和/或修改它； 许可的2.1版，或（由您选择）任何更高的版本。 分发该库是希望它会有用，但没有任何担保； 甚至没有对适销性或特定用途适用性的暗示保证。 有关更多详细信息，请参见GNU较宽松通用公共许可证。 您应该已经与该库一起收到了GNU通用公共通用许可证的副本； 如果没有，请写信给自由软件基金会公司，美国马萨诸塞州波士顿，五楼富兰克林街51号，美国02110-1301 Madgwick的IMU和AHRS算法的
AHRS姿态融合代码
04-20
国外飞控有关姿态融合的一个函数，AHRS，带地磁纠正偏航角
algo_AHRS_dll:SOH Madgwick的AHRS算法封装在.dll文件中-开源
04-29
这是SOH Madgwick的AHRS算法的Windows DLL。 原始代码可以在以下位置找到：http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
Madgwick 和 Mahony算法源码
12-10
标题中的“Madgwick 和 Mahony算法”是两种在惯性测量单元（IMU）姿态解算中广泛使用的算法，主要应用于无人机、机器人以及其他需要实时三维姿态估计的系统。这两种算法都是为了处理从IMU传感器获取的数据，通过...
Mahony算法 AHRS系统
wesky95的博客
01-12 1669
Mahony算法 AHRS系统简介1、陀螺仪1.1 零点漂移1.2 白噪声1.3 温度/加速度计影响1.4 积分误差2、加速度计3、坐标系定义4、算法流程4.1 初始状态确定4.2 加速度计修正4.3 磁力计修正4.4 对陀螺仪进行修正参考资料 简介 AHRS(Attitude and heading reference system，也就是航姿参考系统。在互补滤波算法中传感器主要采用了IMU（陀螺仪、加速度计）和磁力计。 AHRS的基本思想是，在载体没有平移运动的情况下，通过加速度感知重力分量，可以计算出
Madgwick AHRS MATLAB应用程序
03-30
Madgwick AHRS是一种基于四元数的姿态估计算法，可以估计设备的姿态（即方向）和角速度，广泛应用于无人机、机器人、虚拟现实等领域。在MATLAB中，可以使用Madgwick AHRS MATLAB应用程序来实现姿态估计。 Madgwick AHRS MATLAB应用程序的使用方法如下： 1. 下载Madgwick AHRS MATLAB应用程序，解压缩后将所有文件保存在同一目录下。 2. 打开MATLAB软件，进入该目录，并运行madgwick_ahrs.m文件。 3. 在命令窗口中输入以下命令： ```matlab [data,t] = getIMUdata(); ``` 该命令将获取IMU数据，其中data为3轴加速度计和3轴陀螺仪的数据，t为时间戳。 4. 在命令窗口中输入以下命令： ```matlab [quaternion, euler_angles] = MadgwickAHRS(data, t); ``` 该命令将使用Madgwick AHRS算法计算姿态估计，其中quaternion为四元数，euler_angles为欧拉角（即俯仰角、翻滚角和偏航角）。 5. 可以通过以下命令将姿态估计结果可视化： ```matlab plotEulerAngles(euler_angles, t); ``` 该命令将绘制欧拉角随时间变化的曲线。 Madgwick AHRS MATLAB应用程序可以和各种IMU传感器（如MPU6050、MPU9250等）配合使用，实现实时姿态估计。
评论 7
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值