姿态角速度和机体角速度，横摆角速度（Yaw Rate）估算
matlab代码
姿态角速度是相当于世界坐标系的，转换到机体角速度
function A = w2Datti( o,atti )
%输入：姿态角(pitch, roll, yaw)-Y-X-Z
%输出：姿态角速率与角速度的关系矩阵
% A = [-sin(atti(2)), 0, cos(atti(2));
% cos(atti(2))*cos(atti(1)),0,sin(atti(2))*cos(atti(1));
% sin(atti(1))*sin(atti(2)), cos(atti(1)), -sin(atti(1))*cos(atti(2))]/cos(atti(1));
sp = sin(atti(1));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）估算
机体坐标系
航迹坐标系
参考：
https://wenku.baidu.com/view/36c0fc940740be1e640e9a5d.html
名词解释
首先对这3个概念进行一下基本的解释。
1、加速计
加速计（Accelerometer、G-Sensor）也叫重力感应器，实际上是可以感知任意方向上的加速度（重力加速度则只是地表垂直方向加速度），加速计通过测量组件在某个轴向的受力情况来得到结果，表现形式为轴向的加速度大小和方向（XYZ），这一点又有点类似于陀螺仪，但陀螺仪的更多关注自身旋转情况（原位运动），加速计则主要是测量设备的受力情况，也就是三轴运动情况，尽管加速计也可能在某个小范围换算出角速度的可能，但设计原理决定似乎更适合于空间运动判断。
2、陀螺仪
陀螺仪（Gyroscope、GYRO-Sensor）也叫地感器，传统结构是内部有个陀螺，三轴陀螺仪的工作原理是通过测量三维坐标系内陀螺转子的垂直轴与设备之间的夹角，并计算角速度，通过夹角和角速度来判别物体在三维空间的运动状态。三轴陀螺仪可以同时测定上、下、左、右、前、后等6个方向（合成方向同样可分解为三轴坐标），最终可判断出设备的移动轨迹和加速度。也就是说陀螺仪通过测量自身的旋转状态，判断出设备当前运动状态，是向前、向后、向上、向下、向左还是向右呢，是加速（角速度）还是减速（角速度）呢，都可以实现，但是要判断出设备的方位（东西南北），陀螺仪就没有办法。
传统的陀螺仪属于机械式的，随技术发展，还有出现了振动式陀螺仪、激光陀螺仪、微机电机械陀螺仪等，无论是在体积微型化、测量精度和易用性上都有大大提高。
陀螺仪是测量物体旋转时的角速度，经手机中的处理器对角速度积分后就得到了手机在某一段时间内旋转的角度。其实手机里的重力传感器就可以获得手机的相对水平面的转角，但你可以试试，让手机绕垂直与地面的轴旋转，相比之下，有陀螺仪的则能感应到这个旋转，而只有重力传感器的就不行。通俗的讲，以前的手机只能感应相对水平面的转角，而陀螺仪可以感应任何方向的转角
3、磁力计
磁力计（Magnetic、M-Sensor）也叫地磁、磁感器，可用于测试磁场强度和方向，定位设备的方位，磁力计的原理跟指南针原理类似，可以测量出当前设备与东南西北四个方向上的夹角。所以，陀螺仪知道“我们转了个身”，加速计知道“我们又向前走了几米”，而磁力计则知道“我们是向西方向”的。
所以在实际应用中，由于应用、误差修正、误差补偿需要，往往会结合使用上述传感器，充分利用每种传感器的特长，让最终的运算结果更准确，比如在Android中，会同时使用磁力计和加速计来运算出Orientation（方位计），运算出的方位信息需要同时结合磁场方向和方向运动情况才能得到
————————————————
链接：https://blog.csdn.net/LEON1741/article/details/80831169
区别
1、概念对比
• 陀螺仪就是内部有一个陀螺，它的轴由于陀螺效应始终与初始方向平行，这样就可以通过与初始方向的偏差计算出实际方向。手机里陀螺仪实际上是一个结构非常精密的芯片，内部包含超微小的陀螺。
• 加速计是用来检测手机受到的加速度的大小和方向的，而手机静置的时候是只受到重力加速度（这个高中学过）的.所以很多人把加速计功能又叫做重力感应功能。
• 磁力计是测试磁场强度和方向的。
2、原理对比
• 陀螺仪测量是参考标准是内部中间在与地面垂直的方向上进行转动的陀螺。通过设备与陀螺的夹角得到结果。原理：角动量守恒，一个正在高速旋转的物体（陀螺），它的旋转轴没有受到外力影响时，旋转轴的指向是不会有任何改变的。陀螺仪就是以这个原理作为依据，用它来保持一定的方向。
• 加速计是以内部测量组件在各个方向上的受力情况来得到结果。
• 磁力计的原理就是中学物理中涉及到的那个最简单的指南针了（那记得那根被磁化的钢针么）。
3、应用对比
• 陀螺仪的强项在于测量设备自身的旋转运动。对设备自身运动更擅长。但不能确定设备的方位。
• 加速计的强项在于测量设备的受力情况。对设备相对外部参考物(比如，地面)的运动更擅长。但用来测量设备相对于地面的摆放姿势，则精确度不高。
• 磁力计的强项在于定位设备的方位。可以测量出当前设备与东南西北四个方向上的夹角。
陀螺效应：https://www.bilibili.com/video/BV1rE411W73o
所谓陀螺效应，就是旋转着的物体具有像陀螺一样的效应。陀螺有两个特点进动性和定轴性。当高速旋转的陀螺遇到外力时，它的轴的方向是不会随着外力的方向发生改变的，而是轴围绕着一个定点进动。大家如果玩过陀螺就会知道，陀螺在地上旋转时轴会不断地扭动，这就是进动。 简单来说，陀螺效应就是旋转的物体有保持其旋转方向（旋转轴的方向）的惯性。
进动性
定轴性：
横摆角速度（Yaw Rate）估算
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（也叫偏航率）。
Question
1 Yaw-G 传感器发出的Yaw-rate 真实吗？如何校验？
2 通过方向盘转角和车速，计算出的整车参考Yaw-rate能真实反映车辆应有的Yaw值吗？
Yaw-G的校验算法
推导过程：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.
通常车上的输出信息：
