飞行器悬停控制的问题

小憨不傻

本人菜鸟一枚！
最近在研究INS/GPS，在研究GPS输出的数据的时候有一个疑问，就是GPS测得的航向角在不移动GPS接收机的情况下是没有输出的(PS输出为0)，因此在没有移动的情况下是不能够知道飞行器当前的速度。
原因如下
velNED[0] = gpsGndSpd*cos(gpsCourse);     //velNED[0] 向北方向的速度  velNED[1] 向东方向的速度 gpsCourse GPS航向角
velNED[1] = gpsGndSpd*sin(gpsCourse);

对于固定翼飞行器来说因为飞行器一直前进，没有悬停状态，所以航向角一直有输出，因此速度可以知道。
但是对于多旋翼飞行器来说，有一个悬停状态，这是速度便不可知。没有速度输出的话，感觉很难对飞行器的位置进行PID控制。
因为位置的微分(速度)，一直为零，因为悬停位置一直不变。这是如果没有速度项 PID的D项就失去效果。
那么这样的话，就只能进行P控制。
请问论坛大神是如何解决悬停控制的问题的

tanto

九轴和角速度传感器有硬件，需要购买定制姿态结果的软件，只需要结果，不需要源码，请问哪位高手有的，有的请联系我
主板上焊接了两组传感器， 每组分别为：
LIS35DE（三轴加速度传感器）
MPU9255（九轴传感器）
分别布局在板子的两端，见图（选用两组传感器的目的是为了计算角度更精确）；

所有传感器和一个STM32 CPU（STM32F030F4，ST的，16K的Flash空间），以及系统主CPU（ARM8127）一起，挂在同一条I2C总线上，I2C总线已经拉出板外来，要求最后的姿态信息必须由这个STM32 CPU来计算得出后，实时地送给系统主CPU（ARM8127）；

要求：
手持设备，慢速移动（类似手持粗笔写字的速度），通过STM32的姿态算法，得出本设备的实时姿态信息；

1.        实时的角度和角速度信息（必须）
2.        实时的位移方向信息（必须）
3.        实时的位移值信息（最好要求实现）

提供环境： 一套可以正常上电和供电的办卡， 开发者不需要任何的其他硬件