矩阵代码
姿势角速度相当于世界坐标系，变换为机体的角速度
functiona=w2datti(o，atti )输入：姿态角(pitch，roll，yaw )-Y-X-Z % )输出：姿态角速度和角速度关系矩阵(a=[-sin ] atti ) )，0 %sin(atti(1) *sin ) atti )2)、cos ) atti )1)、-sin ) atti )1)、-sin )1) *cos ) * atti )2) ]/cos ) 1sp=sin CP=cos(atti(1)； Sr=sin(atti(2)； Cr=cos(atti(2)； A=[cp*cr，0，sr*cp； sp*sr，cp，-cr*sp； -sr，0，cr]/cp； end修订： 20200403横摆角速度(Yaw Rate )估计
机体坐标系
航迹坐标系
参考：
3359 wenku.Baidu.com/view/36 c 0f c 940740 be1 e 640 e9a5d.html
名词解释首先对这三个概念进行基本解释。
1、加速度计
加速计(Accelerometer，G-Sensor )也称为重力感应器，实际上能够感知任意方向的加速度(重力加速度仅为地表垂直方向的加速度)，加速计是组件所在轴向上的受力http://www.Sina.Cina但陀螺仪往往关注自身的旋转情况(原位运动)，加速计主要测量装置的受力情况，即三轴运动情况。 加速计虽然可以在某个小范围内换算角速度，但设计原理的确定似乎适用于表现形式为轴向的加速度大小和方向（XYZ）。
2、陀螺仪
陀螺仪(Gyroscope，GYRO-Sensor )又称地磁传感器，传统结构为内有陀螺，三轴陀螺的工作原理是测量三维坐标系中陀螺转子的垂直轴与设备之间的夹角，计算角速度，通过角和角速度在物体的三维空间中三轴陀螺仪可以同时测量上、下、左、右、前、后6个方向，合成方向也可以同样分解为三轴坐标，最终判断设备的移动轨迹和加速度。 也就是说，陀螺仪通过测量自身的旋转状态，可以判断设备现在的运动状态，是前方、后方、上方、下方、左还是右，是加速(角速度)还是减速(角速度)，为空间运动判断。
传统陀螺是机械式的，随着技术的发展，出现了振动陀螺、激光陀螺、微机电等，无论是体积的微型化、测量精度还是易用性都有了很大的提高。
陀螺仪测量物体要判断出设备的方位（东西南北），陀螺仪就没有办法，用手机中的处理器对角速度进行积分后，得到了手机在一定时间内旋转的角度。 其实手机里的重力传感器可以得到手机相对水平面的旋转角，但是可以试试。 让手机绕着垂直和地面的轴旋转，而有陀螺仪的东西可以感知到这个旋转，但是只有重力传感器是不行的。 一般来说，以前的手机只能感知相对于水平面的旋转角，而陀螺仪可以感知任何方向的旋转角
3、磁力仪
磁力仪(Magnetic，M-Sensor )又称地磁、磁传感器，可用于磁场强度和方向测试，确定设备方位。 磁力仪的原理与指南针的原理相似，可以测量现有设备和东南西北四个方向的角度。 因此，请参阅旋转时的角速度
因此，在实际应用中，出于应用、误差校正、误差补偿的需要，往往将这些传感器组合使用，需要充分利用各个传感器的特点，使最终的运算结果更加准确。 例如，在Android中，磁强计和加速度计并用运算Orientation，运算出的方位信息需要同时组合磁场的方向和方向的运动状况来获得
————————————————
链接： https://blog.csdn.net/Leon 1741/article/details/80831169
差异1、概念比较
陀螺仪在内部有陀螺仪。陀螺仪知道“我们转了个身”，加速计知道“我们又向前走了几米”，而磁力计则知道“我们是向西方向”的。手机里的陀螺仪其实是一种结构非常精密的芯片，里面含有超小型的陀螺仪。
加速度计是为了检测手机受到的加速度的大小和方向，手机静置的时候只受到重力加速度(在这所高中学过)，所以很多人都在做加速度计的工作
能又叫做重力感应功能。• 磁力计是测试磁场强度和方向的。
2、原理对比
• 陀螺仪测量是参考标准是内部中间在与地面垂直的方向上进行转动的陀螺。通过设备与陀螺的夹角得到结果。原理：角动量守恒，一个正在高速旋转的物体（陀螺），它的旋转轴没有受到外力影响时，旋转轴的指向是不会有任何改变的。陀螺仪就是以这个原理作为依据，用它来保持一定的方向。
• 加速计是以内部测量组件在各个方向上的受力情况来得到结果。
• 磁力计的原理就是中学物理中涉及到的那个最简单的指南针了（那记得那根被磁化的钢针么）。
3、应用对比
• 陀螺仪的强项在于测量设备自身的旋转运动。对设备自身运动更擅长。但不能确定设备的方位。
• 加速计的强项在于测量设备的受力情况。对设备相对外部参考物(比如，地面)的运动更擅长。但用来测量设备相对于地面的摆放姿势，则精确度不高。
• 磁力计的强项在于定位设备的方位。可以测量出当前设备与东南西北四个方向上的夹角。
所谓陀螺效应，就是旋转着的物体具有像陀螺一样的效应。陀螺有两个特点进动性和定轴性。当高速旋转的陀螺遇到外力时，它的轴的方向是不会随着外力的方向发生改变的，而是轴围绕着一个定点进动。大家如果玩过陀螺就会知道，陀螺在地上旋转时轴会不断地扭动，这就是进动。 简单来说，陀螺效应就是旋转的物体有保持其旋转方向（旋转轴的方向）的惯性。
进动性
定轴性：
https://www.sohu.com/a/352265247_467757
如果单纯解释ESP 系统的工作逻辑，那很简单。
ESP 系统：
input：
方向盘转角传感器的角度信号StrAngle，结合车速信号，
output：
算出该车在该车速、方向盘转角下应有的车身yaw值（中心转向）。
Proccess：
将该Yaw值作为参考，与车身Yaw-G传感器检测出的车身真实Yaw值进行对比，判断是过度还是不足进而决定哪一侧的轮胎加压以纠正。
Yaw 是横摆角，而横摆角对时间微分，我们就得到了横摆角速度 Yaw-Rate
一般Yaw-G 传感器输出就是两个加速度（纵向、侧向）还有一个横摆角速度Yaw-rate（也叫偏航率）。
Question1 Yaw-G 传感器发出的Yaw-rate 真实吗？如何校验？
2 通过方向盘转角和车速，计算出的整车参考Yaw-rate能真实反映车辆应有的Yaw值吗？
推导过程：https://zhuanlan.zhihu.com/p/122254225
Yaw-G的校验算法，我们知道，车辆在弯道内前后轮的外侧车轮转速都大于内侧车轮，所以利用轮速差我们可以估算出车横摆角速度（偏航率）。
如果我们套入前轮，估算Yaw-rate就是
Yaw-rate1=（wf1-wf2）Rr/(Acos(a))
相同的操作，我们也可以带入后轮，因为后轮没有转向角，所以cos(a) 等于1 ，但通常情况下，更愿意取驱动轮的轮速差来估算。
用这种方法可以使用最少传感器的情况下估算出车辆的横摆角，但这里我们不得不面对一个问题，比如在高G 的工况，比如沥青路上急转向，由于侧向加速度引起重量转移会导致内外侧车轮的滚动半径差异（外侧受压变小，内侧载荷减小而变大），这样的结果使得通过轮速估算的横摆角存在了偏差，所以通常情况下还要根据侧向加速的值对上面的横摆角进行修正。
Yaw-rate1=Yaw-rate1 X K
2）侧向加速度-车速法，由于通过四个轮速能计算出车辆前进速度，而侧向加速度则是通过yaw-G 传感器获得，通过两个物理量，我们也可以估算出横摆角速度。
Yaw-rate2=侧向加速度/纵向车速
3）第三种就是Yaw-G 传感器通过惯性测量而发出的车辆质心的Yaw-rate。
具体到Yaw 测量模块，结构如下图所示，中间部分为一个三轴浮动连接的振动盘. 外壳与车身固定，当车身产生横摆时，由于振动盘的惯性，壳体与盘之间会有相速度、加速度（科氏加速度）。
很多人估计已经疑惑了，为什么传感器只输出一个Yaw（横摆），难道不能输出Pitch（俯仰）,Rolling（侧倾）吗？ 答案是肯定的，相同的陀螺仪模块，用上三个就能测量三个方向的转角。
问题又来了，那既然这么方便，为何车上很少见通过质心处的传感器来测量Pitch和rolling 的呢？具体原因有两点.
1 车辆的俯仰、侧倾都是在惯性力的作用下，绕其纵倾中心、侧倾中心来转动，真正换算到绕质心的俯仰、侧倾角度非常非常的小. 对于陀螺系统这样一个模糊测量系统来说，太困难了！更重要的是，Pitch/Rolling 对于车辆的操控、纠偏并不必要!
2 由于车身高度传感器的应用（当然并不是为了测量Pitch/Rolling存在）通过换算得到Pitch/Rolling 变成了一件非常简单的事情.
比如，前后高度传感器的输出差异，就能换算出Pitch，而左右高度传感器的输出差异就能换算出Rolling.
通常车上的输出信息：