程序员经验分享-hwhale

基于RflySim平台的mahony(含磁罗盘)互补滤波在pixhawk仿真及实物实验（带实验数据）

2023-05-16

写在前面

本案例实验采用RflySim平台，该平台可以高效快速编写代码，使用simulink模型搭建，可以见代码直接生成对应的C代码，并一键将代码烧录Pixhawk中，是一种快速开发平台，RflySim获取更多可以参考网站

开发需要

matlab2018b及以上版本
simulink可能需要对应版本才可以打开，我用的2018b的matlab
pixhawk2.4.8硬件
PX4 1.7.3stable版本软件代码

mahony思想

mahony互补滤波是一种高效的姿态解算方法。
mahony核心思想： 机体水平时，加速度计无法测量绕 Z 轴的旋转量，即偏航角。磁力计也有同样问题，无法测得绕磁轴的旋转量。故需要加速度计和磁力计同时对陀螺仪进行校正。

mahony算法步骤

1. 加速度计与磁力计测量值进行归一化。

对应代码：在这里插入图片描述

2. 磁力计的输出在机体坐标系下，将其转换到导航坐标系。

在这里插入图片描述
对应代码：在这里插入图片描述

3. 假定理想情况下，让导航坐标系n中x轴指向正北方向，并设这个新的导航坐标系下磁力计数据为[bx,by,bz]’， Z轴方向分量（即重力方向分量）相等，水平方向分量相等，可以将磁力计在xoy平面的投影折算到 x轴则容易得到：

在这里插入图片描述
by = 0；对应代码：在这里插入图片描述

4. 计算理论载体坐标系b下的三轴加速度大小。

在这里插入图片描述
对应代码：在这里插入图片描述

5. 将磁力计在导航坐标系n中的理想输出[bx,by,bz] 再次旋转到载体坐标系b中，得到在b系中的理想输出。

在这里插入图片描述
对应代码：在这里插入图片描述

6. 将理想输出[bx,by,bz]与原始输出[mx,my,mz]叉乘得到误差emag，将其作为补偿项送给陀螺仪进行矫正，这部分只是用来修正yaw的还要加上加速度计补偿量，加速计测量值与理论值进行叉乘得到误差eacc，两者相加得到总误差e。

对应代码：在这里插入图片描述

7. 误差e使用PI修正

对应代码：在这里插入图片描述

8. 将PI修正量补偿到陀螺仪中

对应代码：在这里插入图片描述

9. 采用一阶龙格库塔法求解四元数，并作归一化处理

对应代码：在这里插入图片描述

mahony仿真实验

1. 如果有硬件设备可以打开（e4.4\mahony仿真实验\实物采集加速度计、陀螺仪、磁力计数据）目录下的cf.slx文件。

在这里插入图片描述
上图是搭建的一个简单的读取Pixhawk的陀螺、加计、磁罗盘数据的simulink模型通过编译烧录两个步骤就可以实现将搭建的模型以一个topic形式加入到源代码中。相当于在源代码的基础上添加了一个进程，并不改变源代码。（具体PSP工具箱下载安装等操作参考这个网站）

编译模型后下载烧录代码，通过遥控器5通道控制是否开始采集数据，并通过指示灯显示当前系统是否正常运转。
写卡期间可以晃动飞控，结束操作后，先通过遥控器5通道到关闭写卡操作，再关闭遥控器，最后给飞控断电。取出SD卡，使用读卡器读出SD数据。
采集完成数据后会生成一个.bin文件，文件名字可以有数据采集模型的datalog模块更改。

2. 打开e4.4\mahony仿真实验\实物采集加速度计、陀螺仪、磁力计数据\plot_filter.m文件

在这里插入图片描述
将数二进制数据读取解包并存住在名为da.mat的文件中。
第二行读取文件的名字，这个名字是我在datalog模块中设置的名字，也可以自行更改。

3. 将第2步得到的da.mat文件复制到e4.4\mahony仿真实验，并在文件夹同目录下打开Attitude_estimator.m文件，将第二行loda 后面改成第2步生成发文件名字。

在这里插入图片描述

4. 运行

红色线是加速度计直接解算结果
蓝色线是陀螺仪直接积分解算结果
黑色线是互补滤波解算结果
Roll解算结果
Roll解算结果
在这里插入图片描述
Pitch解算结果

红色线是磁力计直接解算结果
蓝色线是陀螺仪直接积分解算结果
黑色线是互补滤波解算结果
在这里插入图片描述
Yaw解算结果
因为yaw我没有做角度限制变化，所以看到上图是±90变化，这个可以自行更改。

以上是有硬件设备的，可以自行采集数据，将数据打包进行实验。

###没有硬件设备的可以直接使用e4.4\mahony仿真实验\da.mat文件作为数据源。

mahony实物实验

1. 连接好飞控并打开e4.5\mahony实物实验\cf.slx文件

在这里插入图片描述
cf1模块是设计的mahony互补滤波器其最终输出的是已经解算好的四元数，同时vehicle_altitude模块是PX4自带的姿态解算角度输出模块，可以作为解算对比。
在这里插入图片描述
上图是CF1模块，输出是mahony解算的四元数。以上用到的所有资料、数据、详细代码下载

下载烧录代码后，通过遥控器5通道指定是否进行写卡操作，并通过指示灯判断系统是否正常运行。

2. 写卡期间可以晃动飞控，结束操作后，先通过遥控器5通道到关闭写卡操作，再关闭遥控器，最后给飞控断电。取出SD卡，使用读卡器读出SD数据（一个.bin文件）
3. 打开e4.5\mahony实物实验\plot_filter.m文件进行数据读取。

通过解算写入的四元数数据得到三轴姿态角。
在这里插入图片描述
实物解算的Roll
在这里插入图片描述
实物解算的Pitch
在这里插入图片描述
实物解算的Yaw
上图可以看出互补滤波相对于PX4内部自带的解算机制还是差一些，解算结果有毛刺。

上述用到的数据、代码下载

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

基于RflySim平台的mahony(含磁罗盘)互补滤波在pixhawk仿真及实物实验（带实验数据） 的相关文章

  • 通过ROS控制真实机械臂(18) --- 构建 octocmap 用于碰撞检测和避障

    师兄和同门在做SLAM的时候 xff0c 经常会用到的 octomap xff0c 也就是八叉树地图 octomap相比于点云地图来说大大减小了地图的存储空间 既然octomap可以用于导航和避障 xff0c 那么自然也可以导入moveit

  • 【实用版】卡尔曼滤波及其扩展方法的区别与定位系统中的应用

    卡尔曼滤波及其扩展方法的区别与定位系统中的应用 卡尔曼滤波卡尔曼滤波的扩展EKFSPKFEnKF 定位系统中的应用 源自于学校课题 xff0c 主要用卡尔曼滤波KF及其扩展方法 xff08 包括扩展卡尔曼滤波EKF Sigma点卡尔曼滤波S

  • CP2102 USB to UART Bridge Controller 驱动安装（windows or Ubuntu）

    CP2102是一款USB转TTL电平的USB转串口芯片 xff0c 使用时发现诸多小网站上的驱动不是病毒就是安装后无效 xff0c 经同事推荐去官网下载后成功连接 官网地址 xff1a https www silabs com produc

  • 解决罗技GHUB 安装 一直初始化 下载不了问题

    罗技的GHUB在安装时一直处于初始化状态 xff0c 可以通过修改时间解决 原地址 罗技 GHub 解决初始化无法安装教程 侵删

  • 基于Cartographer的建图与导航

    一 RoboSense 16线雷达驱动安装 2 二 Cartographer的安装 2 三 配置文件结构说明 4 四 配置文件详解 4 2D lua文件的配置 5 2D launch文件的配置 6 2D pure location lua文

  • Ubuntu18.04安装autoware.ai

    前言 Autoware AI是世界上第一个用于自动驾驶技术的 All in One 开源软件 xff0c 关于它的介绍就不再赘述了 xff0c 感兴趣的可以去看一下知乎文章 xff0c 这里主要说明一下autoware ai的安装配置 之前

  • Baxter工作站建立及简单使用

    1 硬件 1个 Baxter Research Robot1台PC机或笔记本 xff0c 安装好Ubuntu系统 xff08 推荐Ubuntu16 04安装ROS kinetic xff09 1个无线路由器 xff08 千兆网口 xff09

  • selenium网页自动登录、打卡（二）

    文章目录 前言一 Python 43 Selenium二 通信环境 xff08 服务器 客户端 xff09 1 selenium借助cookie网页登录csdn2 私信发送消息流程3 将新增的逻辑融入原本的程序 前言 前面做了一个自动打卡的

  • 解决浏览器“由你的组织管理”或“由贵单位管理”

    寒假在家使用自己电脑时 xff0c 浏览器出现意外关闭 xff0c 一次以为没什么 xff0c 这两天出现好几次 xff0c 时不时地来一下 xff0c 太搞心态了 xff0c 必须给他解决了 一番检查后 xff0c 是浏览器自动安装了一款

  • Ubuntu18.04屏幕分辨率问题

    Ubuntu18 04屏幕分辨率问题 起因 本来昨天还好好的 xff0c 过了一夜 xff0c 就变了 xff0c 像极了咳咳 自行脑补 redwallbot 2小车上固定的屏幕 xff0c 屏幕分辨率本来应该是1920x1080的 xff

  • 使用frp实现内网穿透

    使用frp实现内网穿透 引言 一打五师兄走之前留了一块树莓派给我 xff0c 暑假闲来无事拿出来玩玩 如果每次都连接显示屏和键盘使用有点麻烦而且低级 正常笔记本和树莓派都连着实验室的WIFI xff0c 网段一样 xff0c 是可以ssh远

  • ffmpeg+nginx-rtmp转发视频流

    nginx与nginx rtmp module安装 画了好几天图 xff0c 实在有些乏力 xff0c 找点有意思的事情做做 觉得视频流传输挺有意思 xff0c B站找了些视频 xff0c 但感觉有些大同小异 xff0c 讲得不是很清楚 F

  • 2013 google校园招聘笔试题

    2013 google校园招聘笔试题 回忆版 xff0c 难免有错误 xff0c 欢迎指正 同时欢迎大家在评论中讨论答案 1 单项选择题 1 1如果把传输速率定义为单位时间内传送的信息量 xff08 以字节计算 xff09 多少 关于一下几

  • 调用微信公众号接口给女票每天打卡

    文章目录 前言一 微信公众号接口二 使用步骤 前言 最近网上很火的给女朋友做公众号推送 xff0c 作者 小红书 64 猪咪不是猪 且做作者 大方的共享成品 源码 43 教学 43 自动版本 起初 xff0c 人们认为这仅仅只是个简单且幼稚

  • js获取整个页面源码

    通过 outerHTML document documentElement outerHTML 通过异步请求 get window location href function res console log res 通过jQuery 34

  • 查看自己电脑被别人U盘拷贝文件

    windows 43 R reg query HKLM System currentcontrolset enum usbstor s gt c usb txt FriendlyName 第二种是使用USBViewer USBViewer

  • 普罗米修斯-docker安装

    1 只有一台服务器 xff0c 所以使用docker来进行试验 安装docker curl fsSL https get docker com bash s docker mirror Aliyun 查看安装版本 docker versio

  • netty参数设置

    1 通用参数 CONNECT TIMEOUT MILLIS Netty参数 xff0c 连接超时毫秒数 xff0c 默认值30000毫秒即30秒 MAX MESSAGES PER READ Netty参数 xff0c 一次Loop读取的最大

  • git 删除远程文件目录

    git rm r cached 文件夹名称 git add git commit m 34 aa 34 git push origin master

  • git删除远程分支

    branch 1 列出分支 xff0c a参数是列出所有分支 xff0c 包括远程分支 git branch a 2 创建一个本地分支 git branch branchname 3 创建一个分支 xff0c 并切换到该分支 git che

随机推荐

  • maven命令上传第三方包

    mvn deploy deploy file Dmaven test skip 61 true DgroupId 61 sdk的groupId DartifactId 61 包的名称 Dversion 61 版本号 如 xff1a 0 0

  • 解决图片上传权限问题

    linux默认umask为022 xff0c 对应权限为755 xff0c 其它用户可读可执行 可以vim etc profile xff0c 搜索umusk关键字查看 if UID gt 199 amp amp 34 96 usr bin

  • 微信小程序 解决 wx.request同步问题 方便开发 Promise方式

  • Python经典书籍有哪些?这份书单送给你_黑马程序员

    文章目录 一 Python 基础 01 Python编程 xff1a 从入门到实践 xff08 第2版 xff09 02 Python编程快速上手 xff08 第2版 xff09 03 Python编程初学者指南 04 笨方法 学Pytho

  • 记忆的方法

    1 第一招 xff0c 在背书的时候 xff0c 用双手捂住你的耳朵 xff0c 并且大声的朗读 研究表明 xff0c 用手捂着耳朵来朗读的话 xff0c 声音是直接通过骨骼来传输到内耳 xff0c 对大脑刺激会更加强烈 xff0c 记忆也

  • ssh登录服务器缓慢问题

    问题描述 问题刚开始是由pod启动失败 xff0c 报错unable to ensure pod container exists failed to create container for kubepods burstable pod8

  • UCOSIII学习-任务管理

    UCOSIII学习 任务管理 1 UCOSIII 任务组成2 UCOSIII 默认系统任务3 UCOSIII 任务状态4 任务堆栈1 任务堆栈的创建2 任务堆栈初始化 5 任务控制块1 任务控制块创建2 任务控制块初始化 6 任务就绪表1

  • ubuntu（Linux）配置允许远程登陆

    安装ubuntu后默认不可以以root方式登录系统 xff0c 需要做以下配置 1 使用sudo i 命令可以让用户切换到root用户 xff0c guo用户是安装ubuntu时配置的用户 xff0c 因人而异 xff1b 2 配置root

  • python带下划线的变量和函数

    参考文献 xff1a https blog csdn net AI S YE article details 44685139

  • ADD，COPY，ENTRYPOINT和cmd

    Dockerfile中有关信息 xff1a ADD与COPY区别 add 1 对压缩包进行解压2 可以在后面直接跟文件地址 copy xff1a 把本地的文件copy到容器里面 ENTRYPOINT与CMD区别 1 第一种解释 xff08

  • docker实例操作

    很多东西都是借鉴各位大神的 xff0c 也不知道具体是谁或是哪个链接 xff0c 很抱歉 两者同为目前版本中个人和小团队常用的服务级操作系统 xff0c 在线提供的软件库中可以很方便的安装到很多开源的软件及库 两者都使用bash作为基础sh

  • 三、FreeRTOS任务管理--常用函数

    任务的基本概念 FreeRTOS 的任务可认为是一系列独立任务的集合 每个任务在自己的环境中运行 在任何时刻 xff0c 只有一个任务得到运行 xff0c FreeRTOS 调度器决定运行哪个任务 调度器会不断的启动 停止每一个任务 xff

  • 七、FreeRTOS事件和常用函数接口

    基本概念 事件是一种实现任务间通信的机制 xff0c 主要用于实现多任务间的同步 xff0c 但事件通信只能是事件类型的通信 xff0c 无数据传输 与信号量不同的是 xff0c 它可以实现一对多 xff0c 多对多的同步 即一个任务可以等

  • PX4姿态估计源码分析

    写在前面 今天入坑PX4开始学习PX4代码 xff0c pixhawk硬件是可以支持PX4 ardupilot两套固件 我用的是1 6 0rc1版本代码 代码位置 xff1a Firmware1 6 0rc1 src modules att

  • PX4位置估计源码分析

    写在前面 源码版本 xff1a 1 6 0rc1 源码位置 xff1a Firmware 1 6 0rc1 src modules position estimator inav position estimator inav main c

  • ROS入门笔记（二）：ROS安装与环境配置及卸载（重点）

    ROS入门笔记 xff08 二 xff09 xff1a ROS安装与环境配置及卸载 xff08 重点 xff09 文章目录 1 ROS安装步骤1 1 ROS版本1 2 确定Ubuntu版本号1 3 安装ROS1 3 1 Ubuntu初始环境

  • windows安装Java环境及Flightplot分析PX4飞行日志

    写在前面 用Flightplot分析PX4飞行日志 xff0c 不管是windows还是Ubuntu都需要安装对应的Java环境 xff08 我用的是win10家庭版 xff09 1 下载安装java环境 xff1a a 下载 点击地址进行

  • PX4_ECL_EKF代码分析1

    写在前面 源码版本 xff1a 1 6 0rc1 源码位置1 xff1a Firmware 1 6 0rc1 src modules ekf2 main cpp 源码位置2 xff1a Firmware 1 6 0rc1 src lib e

  • PX4_ECL_EKF代码分析2

    写在前面 源码版本 xff1a 1 6 0rc1 源码位置1 xff1a Firmware 1 6 0rc1 src modules ekf2 main cpp 源码位置2 xff1a Firmware 1 6 0rc1 src lib e

  • 基于RflySim平台的mahony(含磁罗盘)互补滤波在pixhawk仿真及实物实验（带实验数据）

    写在前面 本案例实验采用RflySim平台 xff0c 该平台可以高效快速编写代码 xff0c 使用simulink模型搭建 xff0c 可以见代码直接生成对应的C代码 xff0c 并一键将代码烧录Pixhawk中 xff0c 是一种快速开

热门标签