姿态传感器

先占个坑，把好不容易整明白的加速度计和陀螺仪记录一下。 以及四元数解算，感谢各位大佬的指导。 本文中所说六轴☞三轴加速度+三轴陀螺仪 1、姿态检测 （1）姿态说明 三个角，偏航角（Yaw），横滚角（Roll），俯仰角（Pitch）。 pitch、yaw、roll三个角如下图所示： （1）翻滚角roll是围绕x轴旋转。 （2）俯仰角pitch是围绕y轴的旋转，可视为飞机的上下俯仰。 （3）偏航角yaw是围绕z轴旋转，可视为控制飞机的偏航。 关于这三个角的解释，更详细的可以查看 在 https://blog.csdn.net/yuzhongchun/article/details/22749521 中有更详细的解释。 （2）姿态计算 由于加速度计可以获得三个轴向上的加速度，而地球重力是长期存在且永远竖直向下的，因此我们可以根据重力加速度相对于芯片的指向为参考算得当前姿态。（即三个轴上的加速度经过计算换算为有意义的值之后，通过矢量求和，最终所得结果应该为竖直向下的g——即重力加速度） 当以三个轴上的加速度为分量，可以构成加速度向量a(x,y,z)。假设当前芯片处于匀速直线运动状态，那么a应垂直于地面，即指向z轴方向，模长|a| = g = sqrt( x^2 + y^2 + z^2)。若芯片发生旋转，则a方向不与z轴重合，此时（有空再写） 四元数姿态解算： https://www

最近在做一个类似VR照片的demo，跟全景图片也很像，只是VR照片与全景720度显示，我只做了180度。但我发现他们实现的原理有一丝相似，希望可以给一些想入行AR、VR的朋友一些提示吧。 要想根据用户摇晃手机的行为轨迹展示相应的场景，那必须要使用移动端的陀螺仪、加速器等传感器来做相应的协调。现在的移动端已经提供了很多传感器，你可以根据自己的需要获取相应的数据。 刚开始的时候，我使用传感器提供的姿态角，也称为欧拉角：pitch yaw roll 来显示。这种 姿态角/欧拉角 比较常用在航空上，无人机技术应该也使用到了这个技术点。我用飞机的模型来展示一下这三个角，就一目了然了（不同作者使用不同的坐标系，使用哪种坐标系，个人而定。）： 图一 姿态角/欧拉角 在数学上理解起来会有点抽象，我也是稍理解一点。在维基百科上，欧拉角定义为：用来描述刚体在三维欧几里得空间的取向，对于任何参考系，一个刚体的取向，是依照顺序，从这参考系，做三个欧拉角的旋转而设定的。有兴趣了解得深入一点，可以参考(需翻墙)： https://zh.wikipedia.org/wiki/%E6%AC%A7%E6%8B%89%E8%A7%92 我们也可以简单理解这三个角代表什么意思： 1、俯仰角θ（pitch）：围绕Y轴旋转的。 图二 2、偏航角ψ（yaw）：围绕Z轴旋转的角度。 图三 3、滚转角Φ（roll）

飞控中使用加速度计，陀螺仪，磁罗盘进行姿态估计算法流程。 step1： 获取陀螺仪，加速度计，磁罗盘的原始数值。 step2： 陀螺仪，加速度计减去固定的偏移后得到校准数值，磁罗盘通过偏移和缩放后得到校准数值。（都是在载体坐标系下的测量值)。 step3： 首先根据上次的飞控姿态四元数计算出载体坐标系在世界坐标系中的旋转矩阵。 公式：从四元数 q ( w , x , y , z ) q(w,x,y,z) q ( w , x , y , z ) 到旋转矩阵 [ 1 − 2 y 2 − 2 z 2 2 x y + 2 w z 2 x z − 2 w y 2 x y − 2 w z 1 − 2 x 2 − 2 z 2 2 y z + 2 w x 2 x z + 2 w y 2 y z − 2 w x 1 − 2 x 2 − 2 y 2 ] \begin{bmatrix} {1 - 2{y^2} - 2{z^2}} & {2xy + 2wz} & {2xz - 2wy} \\ {2xy - 2wz} & {1 - 2{x^2} - 2{z^2}} & {2yz + 2wx} \\ {2xz + 2wy} & {2yz - 2wx} & {1 - 2{x^2} - 2{y^2}} \\ \end{bmatrix} ⎣ ⎡ ​ 1 − 2 y 2 − 2 z 2 2 x y − 2 w z 2

代码框架篇 代码的主要结构由ekf2_main.cpp, estimator_interface.cpp, 和ekf.cpp，ekf_helper.cpp 互相交互，在加上底层的的一系列处理文件。 事实上，之前花费了一周的时间，画的流程图意义和价值很大，这让我在程序代码分析的时候更加直观 流程图的连接见： https://download.csdn.net/download/weixin_39350416/11546415 接下来我该做什么呢，主要分析一下，updated（）函数中的五个步骤 1。 预测状态 2。预测协方差 3。 控制融合模式 4。 运行地形估计 5。计算输出状态（输出状态的论文已经梳理过了，但是程序还需要再仔细琢磨） 然后我们现在从预测状态开始 predictState(); predictCovariance(); 这两个函数的主要任务就是执行主过滤器的状态和协方差预测； controlFusionModes(); 这个函数控制融合观测数据 刚刚把第一个状态预测函数过了下，这个函数在ekf.cpp中 确实是根据δ角度和δ速度，校正当前的角度和速度，然后校正垂直方向的速度，和位置信息。 现在过预测协方差，这个函数在协方差的大文件里，covariance.cpp中 这个预测协方差主要做协方差的计算，但是这个内部协方差是计算还是置位，都是有条件的