由磁场数据和加速度数据计算初始姿态角_ros2 角速度和加速度计算姿态角-程序员宅基地
前言
一般惯导姿态角的初始值是由加速度数据和磁场数据计算得到的，加速度数据计算得到滚转角和俯仰角，由滚转角，俯仰角以及磁场数据经过计算再得到初始的偏航角。下面是具体的计算和推导过程。
计算过程:
1.计算滚转角和俯仰角
这里我们的导航坐标系为东北天坐标系（ENU），载体坐标系为右、前，上（x,y,z）,姿态角的旋转顺序为：z,x,y(偏航、俯仰，滚转)。
对于加速度计，有以下公式：
表示导航系到载体系的转换矩阵。设偏航，滚转、俯仰的初始值分别为z0 ，y0 , x0 。
并且满足： 
经过计算得到:
所以：
在推导过程中纯手推容易出错，可以借助matlab计算矩阵，这样不易出错，我在推导这部分时也使用matlab来计算。代码如下：
syms x0
syms y0
syms z0
syms g
x_r_matrix=[1,0,0;0,cos(x0),-sin(x0);0,sin(x0),cos(x0)];
y_r_matrix=[cos(y0),0,sin(y0);0,1,0;-sin(y0),0,cos(y0)];
z_r_matrix=[cos(z0),-sin(z0),0;sin(z0),cos(z0),0;0,0,1];
Cn_b = z_r_matrix*x_r_matrix*y_r_matrix;
Cb_n = Cn_b';
g_n=[0;0;g];
g_b=Cb_n*g_n;
2.计算偏航角
这块可以想象一下，假设一开始平稳放置磁力计，则偏航角的计算公式如下：
该平稳状态设定为a状态，即俯仰角和滚转都为0,对于磁力计放置的任意状态，有以下公式：
根据上式求出
和 之后，再根据
公式就可求出偏航角。
Matlab推导代码如下：
syms x0
syms y0
syms z0
syms mgbx
syms mgby
syms mgbz
x_r_matrix=[1,0,0;0,cos(x0),-sin(x0);0,sin(x0),cos(x0)];
y_r_matrix=[cos(y0),0,sin(y0);0,1,0;-sin(y0),0,cos(y0)];
z_r_matrix=[1,0,0;0,1,0;0,0,1];
Cn_b = z_r_matrix*x_r_matrix*y_r_matrix; m_b=[mgbx;mgby;mgbz];
m_n = Cn_b*m_b;
实际验证
我这里验证使用的IMU型号为ADI公司的ADIS16470，其载体坐标系的定义如下图所示：
和上文提到的载体坐标系是一致的，右，前，上代表载体坐标系的x,y,z轴，软件界面如下图所示：
具体的验证过程很简单，这里不再详叙，不同IMU的载体坐标系定义是不一样的，这点在使用时要注意。当逆时针绕y轴旋转，x轴数据为负， z轴向数据为正；当顺时针绕y轴旋转，x轴向数据为正，z轴数据为正；当逆时针绕x轴旋转，y轴数据为正，z轴数据为正；当顺时针绕x轴旋转，y轴数据为负，z轴数据为正。和前面公式计算得到的角度的正负号验证是一致的。
参考文献
- Euler angles, quaternions, and transformation matrices for space shuttle analysis
- A Double Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9D IMU
智能推荐
什么是内部类？成员内部类、静态内部类、局部内部类和匿名内部类的区别及作用？_成员内部类和局部内部类的区别-程序员宅基地
文章浏览阅读3.4k次，点赞8次，收藏42次。一、什么是内部类？or 内部类的概念内部类是定义在另一个类中的类；下面类TestB是类TestA的内部类。即内部类对象引用了实例化该内部对象的外围类对象。public class TestA{ class TestB {}}二、 为什么需要内部类？or 内部类有什么作用？1、 内部类方法可以访问该类定义所在的作用域中的数据，包括私有数据。2、内部类可以对同一个包中的其他类隐藏起来。3、 当想要定义一个回调函数且不想编写大量代码时，使用匿名内部类比较便捷。三、 内部类的分类成员内部_成员内部类和局部内部类的区别
分布式系统_分布式系统运维工具-程序员宅基地
文章浏览阅读118次。分布式系统要求拆分分布式思想的实质搭配要求分布式系统要求按照某些特定的规则将项目进行拆分。如果将一个项目的所有模板功能都写到一起，当某个模块出现问题时将直接导致整个服务器出现问题。拆分按照业务拆分为不同的服务器，有效的降低系统架构的耦合性在业务拆分的基础上可按照代码层级进行拆分(view、controller、service、pojo)分布式思想的实质分布式思想的实质是为了系统的..._分布式系统运维工具
用Exce分析l数据极简入门_exce l趋势分析数据量-程序员宅基地
文章浏览阅读174次。1.数据源准备2.数据处理step1：数据表处理应用函数：①VLOOKUP函数; ② CONCATENATE函数终表：step2：数据透视表统计分析（1） 透视表汇总不同渠道用户数, 金额（2）透视表汇总不同日期购买用户数，金额（3）透视表汇总不同用户购买订单数，金额step3：讲第二步结果可视化, 比如, 柱形图（1）不同渠道用户数, 金额（2）不同日期..._exce l趋势分析数据量
宁盾堡垒机双因素认证方案_horizon宁盾双因素配置-程序员宅基地
文章浏览阅读3.3k次。堡垒机可以为企业实现服务器、网络设备、数据库、安全设备等的集中管控和安全可靠运行，帮助IT运维人员提高工作效率。通俗来说，就是用来控制哪些人可以登录哪些资产（事先防范和事中控制），以及录像记录登录资产后做了什么事情（事后溯源）。由于堡垒机内部保存着企业所有的设备资产和权限关系，是企业内部信息安全的重要一环。但目前出现的以下问题产生了很大安全隐患：密码设置过于简单，容易被暴力破解；为方便记忆，设置统一的密码，一旦单点被破，极易引发全面危机。在单一的静态密码验证机制下，登录密码是堡垒机安全的唯一_horizon宁盾双因素配置
谷歌浏览器安装（Win、Linux、离线安装）_chrome linux debian离线安装依赖-程序员宅基地
文章浏览阅读7.7k次，点赞4次，收藏16次。Chrome作为一款挺不错的浏览器，其有着诸多的优良特性，并且支持跨平台。其支持（Windows、Linux、Mac OS X、BSD、Android），在绝大多数情况下，其的安装都很简单，但有时会由于网络原因，无法安装，所以在这里总结下Chrome的安装。Windows下的安装：在线安装：离线安装：Linux下的安装：在线安装：离线安装：..._chrome linux debian离线安装依赖
烤仔TVの尚书房 | 逃离北上广？不如押宝越南“北上广”-程序员宅基地
文章浏览阅读153次。中国发达城市榜单每天都在刷新，但无非是北上广轮流坐庄。北京拥有最顶尖的文化资源，上海是“摩登”的国际化大都市，广州是活力四射的千年商都。GDP和发展潜力是衡量城市的数字指...
随便推点
java spark的使用和配置_使用java调用spark注册进去的程序-程序员宅基地
文章浏览阅读3.3k次。前言spark在java使用比较少，多是scala的用法，我这里介绍一下我在项目中使用的代码配置详细算法的使用请点击我主页列表查看版本jar版本说明spark3.0.1scala2.12这个版本注意和spark版本对应，只是为了引jar包springboot版本2.3.2.RELEASEmaven<!-- spark --> <dependency> <gro_使用java调用spark注册进去的程序
汽车零部件开发工具巨头V公司全套bootloader中UDS协议栈源代码，自己完成底层外设驱动开发后，集成即可使用_uds协议栈 源代码-程序员宅基地
文章浏览阅读4.8k次。汽车零部件开发工具巨头V公司全套bootloader中UDS协议栈源代码，自己完成底层外设驱动开发后，集成即可使用，代码精简高效，大厂出品有量产保证。:139800617636213023darcy169_uds协议栈 源代码
AUTOSAR基础篇之OS(下)_autosar 定义了 5 种多核支持类型-程序员宅基地
文章浏览阅读4.6k次，点赞20次，收藏148次。AUTOSAR基础篇之OS(下)前言首先，请问大家几个小小的问题，你清楚：你知道多核OS在什么场景下使用吗？多核系统OS又是如何协同启动或者关闭的呢？AUTOSAR OS存在哪些功能安全等方面的要求呢？多核OS之间的启动关闭与单核相比又存在哪些异同呢？。。。。。。今天，我们来一起探索并回答这些问题。为了便于大家理解，以下是本文的主题大纲：[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-JCXrdI0k-1636287756923)(https://gite_autosar 定义了 5 种多核支持类型
VS报错无法打开自己写的头文件_vs2013打不开自己定义的头文件-程序员宅基地
文章浏览阅读2.2k次，点赞6次，收藏14次。原因：自己写的头文件没有被加入到方案的包含目录中去，无法被检索到，也就无法打开。将自己写的头文件都放入header files。然后在VS界面上，右键方案名，点击属性。将自己头文件夹的目录添加进去。_vs2013打不开自己定义的头文件
【Redis】Redis基础命令集详解_redis命令-程序员宅基地
文章浏览阅读3.3w次，点赞80次，收藏342次。此时，可以将系统中所有用户的 Session 数据全部保存到 Redis 中，用户在提交新的请求后，系统先从Redis 中查找相应的Session 数据，如果存在，则再进行相关操作，否则跳转到登录页面。此时，可以将系统中所有用户的 Session 数据全部保存到 Redis 中，用户在提交新的请求后，系统先从Redis 中查找相应的Session 数据，如果存在，则再进行相关操作，否则跳转到登录页面。当数据量很大时，count 的数量的指定可能会不起作用，Redis 会自动调整每次的遍历数目。_redis命令
URP渲染管线简介-程序员宅基地
文章浏览阅读449次，点赞3次，收藏3次。URP的设计目标是在保持高性能的同时，提供更多的渲染功能和自定义选项。与普通项目相比，会多出Presets文件夹，里面包含着一些设置，包括本色，声音，法线，贴图等设置。全局只有主光源和附加光源，主光源只支持平行光，附加光源数量有限制，主光源和附加光源在一次Pass中可以一起着色。URP：全局只有主光源和附加光源，主光源只支持平行光，附加光源数量有限制，一次Pass可以计算多个光源。可编程渲染管线：渲染策略是可以供程序员定制的，可以定制的有：光照计算和光源，深度测试，摄像机光照烘焙，后期处理策略等等。_urp渲染管线