0
  • 聊天消息
  • 系统消息
  • 评论与回复
登录后你可以
  • 下载海量资料
  • 学习在线课程
  • 观看威廉希尔官方网站 视频
  • 写文章/发帖/加入社区
会员中心
创作中心

完善资料让更多小伙伴认识你,还能领取20积分哦,立即完善>

3天内不再提示

理解Mahony滤波算法的实现过程

zhc135136 来源:SmartHWFW 2023-11-13 10:55 次阅读

1. 说在前面的

之前调试基于QMI8658 6轴姿态解算的时候,我对Mahony滤波的认识还比较浅薄。初次的学习和代码的移植让我对四元数、欧拉角、旋转余弦矩阵有了一定的接触。然而,直到我将地磁传感器加入到我的硬件,进行9轴姿态解算时,我深深地感受到对四元数与姿态阵之间关系的理解是非常重要的。在此之前,我检索了网络文章,并记录了《MEMS_惯性传感器09 - Mahony姿态解算算法详解》、《MEMS_惯性传感器14 - Mahony滤波算法的代码分析》两篇文章。可能对于大佬、资深工程师来说,这些文章的严谨性不够,可能会误导别人。但我必须承认,我仍然没有扎实的数学计算研究背后更深层次的理论。即使如此,我依然希望通过自己的努力在惯性导航算法上学习得更深入一些。同时,我也想多记录一些学到的知识,多记录一些自己的理解和思考。希望这些记录能帮助到一些初学者。如果我的文章有错误的引导,我非常希望大佬们能够指正,因为这对我来说是最大的收获,我非常乐意倾听和学习。我愿意自我突围,向着理想前进!

这次学习检索时发现了一个博文内容硬核、有理有据的博主:路痴导航员[1]。她的博文 《AHRS互补滤波(Mahony)算法及开源代码》对我帮助很大。

2.Mahony滤波算法的核心思想

Mahony滤波算法的核心思想是通过将角速度和角度的测量值与历史估计值进行加权平均,从而得到更加准确的姿态估计结果。同时,滤波算法还利用加速度计的测量值来纠正姿态角度的漂移。该算法也存在一些缺点,例如对于快速旋转和加速度变化较大的情况,其姿态估计结果可能会出现较大的误差。

3. 易懂的理解 Mahony 滤波算法的过程

为了更好的理解,表达式先不用矩阵的思想,在算法的实现的过程中,需要将表达式转换成矩阵形式。

① 根据陀螺仪的测量数据计算出角速度的增量,即:

Δθ = 0.5 * (gyro + bias) * Δt

其中,gyro表示陀螺仪的测量值,bias表示陀螺仪的零偏误差,Δt表示时间间隔。

② 根据加速度计的测量数据计算出重力方向的估计值,即:

g_est = q * (0, 0, -1) * q^-1

其中,q表示四元数的姿态估计值。

③ 根据加速度计的测量数据和重力方向的估计值,计算出加速度计的误差,即:

acc_err = acc_meas - g_est

其中,acc_meas表示加速度计的测量值。

④ 根据角速度的增量和加速度计的误差,更新四元数的姿态估计值,即:

q = q * exp(Δθ - K * acc_err)

其中,K为控制增益。

4. 其他的一些思考

传感器EVB板的测量数据以载体坐标系(b-框架)表示。欧拉角的直观表示描述了地理坐标系中的角度,其中包括重力的影响。地理坐标系中的重力矢量为[0,0,1]g,磁传感器将地球磁场中的强度表示为[cos(θ),0,sin(θ)]guess。因此,在算法中,加速度和磁数据被归一化,确保它们的模是一定大小的。

四元数从地理坐标系(东北天)转换到载体坐标系,就得到了旋转矩阵Cnb。

60b59c4a-7fd9-11ee-939d-92fbcf53809c.png

Cnb的最后一列取负,即将其转换为载体坐标系中的值。在理想情况下,如果没有误差,这个矢量应该与测量值相等。然而,由于这两者总会存在差异,因此我们需要利用它们之间的差值来修正陀螺仪更新的不准确性。

在修正的过程中,我们认为加速度计测量的可信度较高,还需要使用地磁传感来修正加速度传感的Z轴。磁力计的模的长度是确定的,但是两个分量的具体大小无法像重力加速度一样确定。因此,我们使用上一次的四元数得到的Cbn(从载体到地理坐标系的旋转矩阵)来计算测量的磁场强度。理论上,此刻的Cbn应该在东向上的分量是零[cos(θ),0,sin(θ)],而北向和地向会有分量。然而,由于此刻的Cbn尚未得到,我们使用的是上一次的Cbn,并加上一些测量误差,导致东向上仍会有分量存在。为了处理这种情况,我们将xy平面的分量合成到一个方向上,使得y轴上的分量为0。这样,处理后的地磁信息的地位就与重力加速度(0,0,-1)相似了。

根据余弦矩阵和欧拉角的定义,我们可以将地理坐标系的重力向量转换到载体坐标系中。具体而言,将四元数转换成方向余弦矩阵后,我们可以得到方向余弦矩阵的第三列的三个元素,即vx、vy和vz。这三个元素实际上表示了当前欧拉角(即四元数)在载体坐标系上换算得到的重力单位向量。

60cca3d6-7fd9-11ee-939d-92fbcf53809c.png

axayaz是载体坐标参照系上,加速度计测出来的重力向量。vxvyvz是陀螺积分后的姿态推算出的重力向量。它们之间的误差向量exeyez是陀螺积分后的姿态和加计测出来的姿态之间的误差。误差向量可以用向量叉积来表示。叉积向量exeyez位于载体坐标系上,且其大小与陀螺积分误差成正比,可用于纠正陀螺。由于陀螺是对机体直接积分,所以对陀螺的纠正量会直接体现在对载体坐标系的纠正。

60dae068-7fd9-11ee-939d-92fbcf53809c.png

矢量之间的叉乘公式为err = A × B = |A| × |B| × sin(β)。当两个矢量重合时,误差为零,表示没有差异。当两个矢量之间夹角为90°时,误差达到最大值,表示差异最大。然而,在夹角为90°附近的区间内,误差的变化是非线性的,这可能导致一些准确性问题。

审核编辑:汤梓红

声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。 举报投诉
  • 传感器
    +关注

    关注

    2550

    文章

    51039

    浏览量

    753092
  • mems
    +关注

    关注

    129

    文章

    3924

    浏览量

    190587
  • 加速度计
    +关注

    关注

    6

    文章

    700

    浏览量

    45891
  • 惯性传感器
    +关注

    关注

    2

    文章

    169

    浏览量

    27943
  • 滤波算法
    +关注

    关注

    2

    文章

    88

    浏览量

    13718

原文标题:MEMS_惯性传感器17-调试 Mahony 滤波算法的思考

文章出处:【微信号:SmartHWFW,微信公众号:SmartHWFW】欢迎添加关注!文章转载请注明出处。

收藏 人收藏

    评论

    相关推荐

    姿态融合算法是什么

    作者:Joy Yang1.什么是姿态融合算法简单来说,姿态融合算法就是融合多种运动传感器数据(一般需要3轴加速度, 3轴陀螺仪或者3轴地磁感应传感器),通过数字滤波算法容错补偿,
    发表于 07-19 06:47

    介绍几种嵌入式常用滤波算法的matlab实现

    计算过程,可靠性高,不存在阻抗匹配问题。尤其是数字滤波可以对频率很低的信号进行滤波,这是模拟滤波器做不到的。数字滤波使用软件
    发表于 12-21 08:32

    采用软件算法实现数字滤波

    单片机主要作用是控制外围的器件,并实现一定的通信和数据处理。但在某些特定场合,不可避免地要用到数学运算,尽管单片机并不擅长实现算法和进行复杂的运算。下面主要是介绍如何用单片机实现数字
    发表于 01-18 07:00

    软件算法实现数字滤波问题

    软件算法实现数字滤波问题:解决方法:数字滤波算法优点:限幅滤波
    发表于 01-20 06:58

    基于分布式算法的FIR滤波器的设计与实现

    本文介绍了能高效实现固定常数乘法的分布式算法原理,给出了在FPGA 中用查找表实现FIR滤波器的算法设计,并以一个16 阶低通
    发表于 09-02 10:10 10次下载

    数字图像空域滤波算法的FPGA设计与实现

    本文采用的图像是256×256大小的灰度图像,滤波模板3×3大小。如何设计硬件电路来完成上述空域滤波算法,分析上述算法实现
    发表于 01-18 12:12 954次阅读
    数字图像空域<b class='flag-5'>滤波</b><b class='flag-5'>算法</b>的FPGA设计与<b class='flag-5'>实现</b>

    QT+Opencv粒子滤波算法实现视频目标跟踪——(二)滤波算法分析及测试

    在前一个博客中已经对机器视觉及粒子滤波算法的相关原理进行了介绍,在基于410c平台实现粒子滤波算法的前期,我们在上位机上应用QT和Openc
    发表于 02-21 09:53 3856次阅读
    QT+Opencv粒子<b class='flag-5'>滤波</b><b class='flag-5'>算法</b><b class='flag-5'>实现</b>视频目标跟踪——(二)<b class='flag-5'>滤波</b><b class='flag-5'>算法</b>分析及测试

    matlab实现的自适应滤波算法

    本课题的主要内容是在MATLAB上实现自适应滤波器的设计。课题的具体内容为利用GUI界面设计出滤波器的选择界面、参数的输入界面、结果分析图等。其中滤波器的设计将会用两种
    发表于 12-14 16:24 3.8w次阅读
    matlab<b class='flag-5'>实现</b>的自适应<b class='flag-5'>滤波</b><b class='flag-5'>算法</b>

    如何使用FPGA实现图像的中值滤波算法

    图像滤波是图像预处理过程中苇要的组成部分,而基于FPGA的滤波算法相对软件算法而言具有高度的并行性。能满足实时图像处理的要求.同时也具有灵活
    发表于 04-01 11:21 42次下载
    如何使用FPGA<b class='flag-5'>实现</b>图像的中值<b class='flag-5'>滤波</b><b class='flag-5'>算法</b>

    IMU姿态滤波算法——Mahony算法:原理与代码

    首先要指出的是,Mahony算法假设加速度计测量的加速度完全由重力提供,即物体本体运动产生的加速度可忽略不计。在这一假设下,我们假设当前时刻机体的姿态为,则将重力向量的表示转到机体坐标系下,应该为:,这里表示四元数对应的旋转矩阵
    的头像 发表于 05-22 09:18 4475次阅读
    IMU姿态<b class='flag-5'>滤波</b><b class='flag-5'>算法</b>——<b class='flag-5'>Mahony</b><b class='flag-5'>算法</b>:原理与代码

    Mahony算法常见的姿态融合算法

    首先要指出的是,Mahony算法假设加速度计测量的加速度完全由重力提供,即物体本体运动产生的加速度可忽略不计。在这一假设下,我们假设当前时刻机体的姿态为,则将重力向量的表示转到机体坐标系下,应该为:,这里表示四元数对应的旋转矩阵:
    的头像 发表于 05-22 09:22 2514次阅读
    <b class='flag-5'>Mahony</b><b class='flag-5'>算法</b>常见的姿态融合<b class='flag-5'>算法</b>

    经典的滤波算法:Madgwick滤波算法

    Madgwick算法Mahony算法相比,最大的不同之处是如何对待加速度计估计的误差。Mahony是利用叉乘,Madgwick是利用优化;
    的头像 发表于 05-31 11:12 2956次阅读
    经典的<b class='flag-5'>滤波</b><b class='flag-5'>算法</b>:Madgwick<b class='flag-5'>滤波</b><b class='flag-5'>算法</b>

    Mahony滤波器的原理和公式推导

      1. 概述 在进行代码分析之前,了解Mahony滤波器的原理和公式推导是必要的。Mahony滤波器是一种基于四元数的姿态估计滤波器,其主
    的头像 发表于 06-17 11:18 3709次阅读
    <b class='flag-5'>Mahony</b><b class='flag-5'>滤波</b>器的原理和公式推导

    Mahony滤波算法参数自动调节方法介绍

    Mahony滤波算法参数自动调节方法是一种用于姿态估计的滤波算法
    的头像 发表于 12-06 09:45 1128次阅读

    详解从均值滤波到非局部均值滤波算法的原理及实现方式

    图像降噪处理主要分为2D(空域)与3D降噪(时域/多帧),而2D降噪由于相关的实现 算法 丰富,效果各异,初学入门有着丰富的研究价值。理解2D降噪算法的流程,也对其他的增强
    的头像 发表于 12-19 16:30 1248次阅读
    详解从均值<b class='flag-5'>滤波</b>到非局部均值<b class='flag-5'>滤波</b><b class='flag-5'>算法</b>的原理及<b class='flag-5'>实现</b>方式