先mark一下，争取两周之内写完

由于快要毕业了，所以一直都很忙，并没有时间写完，所以拖到现在，不好意思。现在开始介绍四旋翼定高以及调试过程。

首先介绍下四旋翼的模型

图中为 四旋翼的升力,并且满足，。所以

其中，为螺旋桨升力系数，为螺旋桨转速，则又动力学方程知

其中X即为z轴位移距离，则推出四旋翼的模型

显然，系统输出z轴加速度与系统的输入u成正比关系，因此采用PID线性控制器来作为z轴方向上的高度控制器。

假设在没有任何外部观测传感器的情况下，仅有惯性器件估计的位置和速度书算法如下图所示。

上述传递函数为：

特征方程跟为：。因此这种纯惯性通道是不稳定的，而引起系统发散的根本原因是系统无阻尼，所以，要在惯性通道中引入观测量使得系统具有阻尼。所以博主采用经典回路反馈法设计四轴位置估计算法。

高度信息的观测器件一般有：超声波传感器、气压计。

博主第一次定高用的传感器为超声波传感器，总体感觉超声波传感器做好极值滤波（突然大范围跳动）后，定高效果相当好，只是定高的范围有限制，博主超声波定高适用范围为5cm-250cm。

第二次定高采用的传感器为MS5611气压计，效果比较好，高度范围也提升了不少，但是对气流很敏感，所以需要把气压计密封好，不要让螺旋桨的气流对其造成错误影响。

所以超声波定高导航算法采用  超声波融合惯导的算法。总体算法框图如下：

其中，分别为速度和加速度的反馈系数，所以上述的特征多项式为：

由典型二级系统的特征多项式的标准形式：，可以得到

从而选择合适的系数。

控制器由之前推导的四轴模型而选择采用PID 线性控制器作为z轴方向上的高度控制器。选取高度-速度-加速度三级PID控制器方案用于z轴高速控制器，高度控制系统框图如下，

以上即为，四旋翼定高。

只有比例控制时，由四轴模型可以等效为另一个熟悉系统：光滑平面上用力F 推小滑块来控制距离。

所以现象为：高度控制不稳定，频繁上下加减速（最理想是在给定悬停点上下等幅振荡）

比例积分控制时，有了积分来消除静差，四轴定高现象为衰减振荡。

一般不加微分。

只有比例控制时,那么四轴定高控制目标就是让四轴的z轴速度为0，不会控制四轴的高度

所以现象为：1.四轴开启定高时，若z轴速度为0，则稳定不动，定高高度准确无误差

                      2.四轴开启定高是，若z轴速度0，四轴向上飞（做一个加速度减小的减速运动），直至四轴z轴速度为0，但是最终稳定高度会比期望高度高，既高度有误差

                      3.四轴开启定高时，若z轴速度>0，那么控制器输出