利用加速度计和陀螺仪测量车辆运动

图3显示了一个轴的算法。实时对陀螺仪输出积分得到原始角度。利用加速度计测量重力方向并推断出倾斜角度。例如，如果在x轴上得到0.1G的加速度，这意味着倾斜角为arcsin（0.1）=5.7°。为了避免震动和冲击造成倾斜角计算错误，使用截止频率为100Hz或更底的低通滤波器。简单的单极RC滤波器即可。

图3 稳定的角度计算

计算两角度之差，为误差信号。它可以用来更正角度计算。这里定义了一个增益参数，k，决定着有多少误差信号用于更正角速度积分。最后将角速度原始积分与误差信号（已乘增益）相加。该过程的输出是一个角度值，短时间看角速度值起主导作用，但长时间看是由加速度值来修正的。时间尺度由k值决定。

计算增益参数k值：在设定的时间内角速度传感器计算得到的角度被重力计算的角度稳定修正。您应该选择一个比预期测试更长的时间常数。然后将它除于测量速率，就是k值了。例如，如果你选了一个5s的时间常数，而且角速度和加速度计的工作速率是200Hz。那么，k=5/200=0.025。

一旦得到稳定的倾角，就可以用它修正原始的加速度数据。这样能得到沿任一轴的真实（运动）加速度。请注意， 一旦倾斜，重力分量就会被x和y轴的加速度计测量。解决这一问题的办法之一是建立一个旋转矩阵，根据已求取的倾斜角，将加速度矢量（x，y和z轴与车辆一致）旋转到地面坐标系（对车辆坐标系进行坐标变换）。在水平系统中，重力是完全竖直的，所以水平方向上x，y轴得到的将是纯运动加速度，没有重力分量。

最终结果是得到车辆运动的完整描述，包括角速度，稳定的倾斜角度和纠正了的线性加速度。以上描述的实时系统可以使用固态传感器实现。这构成了一个可靠，廉价的车辆运动检测系统。