大神对飞控精准高度估计算法解读

飞控的高度信息主要由气压计来提供，也可包含GPS、超声波等。

跟姿态解算类似，单一的传感器所提供的信息并不能满足实际应用要求，所以同样需要多传感器数据融合。

气压计高度问题主要有以下几点，如图所示：

1、噪声相对较大，数据不够平滑，不过这个问题不大，可以使用一阶低通滤波来处理；

2、数据从上电开始会有漂移，尤其受温度影响，这里图中反应不明显；

3、最大的问题在于，受外界气流影响太大，图中50-90s左右，设定的高度并未改变，但是由于飞行器剧烈运动，导致高度变化较大，波动范围达到5m左右。

所以显然，单用气压计是无法获得精准的高度的。

GPS的高度信息：

主要与GPS性能密切相关了，如果不计成本的，使用价格高昂的GPS设备，自然高度信息是比较理想的。但是在消费级飞行器领域，这个显然不适合，普通GPS（M8N等）的高度同样误差较大，与搜星质量也有关系。

加速度积分：

前面讲过，MEMS的加速度计数据质量更差了，单纯靠加速度积分得到的高度，会在很短的时间内漂移很大。

注：这里的数据是用pixhawk飞控，去掉GPS的情况下进行的飞行测试。主要是测试在无GPS情况下，大机动定高飞行的精度。这里的pixhawk高度估计不作为精确的参考。

下面，主要描述三种高度估计（不含GPS数据）的算法，并用matlab计算结果，观察。

一、互补滤波

很简单的思路，就是将两个高度信息，按照一个权重进行融合。

算法步骤：

1.相关变量赋值，记录当前高度（气压计、GPS所得的是绝对高度，而控制器需要的是相对高度）；

2.将机体测得的加速度转换到NED坐标系下；

3.加速度数据进行一阶低通滤波，并计算加速度偏移；

4.将去除偏移的加速度进行双重积分，分别得到速度、位置；

5.与气压计的数据进行融合；

最终，较为理想的参数是barokp为0.2，即还是基本相信加速度计的数据。

二、pixhawk的高度估计算法

具体思路参考:

做定高控制时，不可避免的就要涉及到如何解算出高度信息，那高度信息又是如何获取的？参考pixhawk源码。这里只介绍只有气压计和加速度计的情况。

概述：

首先要明白，所需的高度信息是地理坐标系下的相对高度，整个算法的核心思想是由地理坐标系下的加速度通过积分，来获得速度、位置信息，而这个数据的精确程度是由机体测量的加速度通过减去偏差，再转换到地理坐标系求得的。这里气压计的作用就是计算一个校正系数来对加速度偏移量进行校正。

算法代码详解：

1. 变量初始化。

float z_est[2] = { 0.0f, 0.0f }; // z轴的高度、速度

float acc[] = { 0.0f, 0.0f, 0.0f }; //地理坐标系（NED）的加速度数据

float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // 机体坐标系下的加速度偏移量

float corr_baro = 0.0f; // 气压计校正系数

2. 计算气压计高度的零点偏移，主要是取200个数据求平均。

baro_offset += sensor.baro_alt_meter;

baro_offset /= (float) baro_init_cnt;

3. 将传感器获取的机体加速度数据转换到地理坐标系下。

加速度数据要先去除偏移量；

sensor.accelerometer_m_s2[0] -= acc_bias[0];

sensor.accelerometer_m_s2[1] -= acc_bias[1];

sensor.accelerometer_m_s2[2] -= acc_bias[2];

然后转换坐标系；

acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];

地理坐标系下的z轴加速度是有重力加速度的，因此补偿上去。

acc[2] += CONSTANTS_ONE_G;

4. 计算气压计的校正系数

corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];

5. 加速度偏移向量校正

accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;

6. 将偏移向量转换到机体坐标系

c += PX4_R(att.R, j, i) * accel_bias_corr[j];

acc_bias[i] += c * params.w_acc_bias * dt;

7. 加速度推算高度

inertial_filter_predict(dt, z_est, acc[2]);

8. 气压计校正系数进行校正

inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);

预测-校正函数：

仿真测试：

通过实际飞行，取出一份飞行数据，在matlab下编程进行数据分析。

仿真结果并未完全与飞控自身计算的高度吻合，猜测由于平台编辑器的问题，还有数据本身受到控制环的影响。至少表明该算法是有效。

三、卡尔曼滤波

建立以下模型，进行卡尔曼估计。

状态量设为：Z轴高度、Z轴速度、Z轴加速度、Z轴加速度偏移，观测量为气压计的高度和NED系Z轴加速度。理论推导相对简单，得到：

其中：

对比三种算法的结果如下：

互补滤波，由于对气压计的权重设的较大0.9，而数据噪声大，所以对应的速度估计噪声很大，不能使用。如果对气压计数据进行低通滤波，同时，气压计的权重为0.05，也就是主要相信加速度数据。

可以看到噪声问题有所好转，但是速度估计静态效果不好，因为只有检测到加速度运动，相应的速度估计才会更新值，由于气压计的权重小了，如果缓慢的高度变化并不能准确测量。所以互补滤波的参数，需要根据实际的飞行情况不断调整。而另外的卡尔曼滤波与pixhawk的算法效果相对较好。

小结：

1、 气压计、GPS的高度是绝对高度，因此为了计算，需要初始化记录当前高度，从而求得相对高度；

2、 本文只是提出几种算法，并不绝对说明哪种算法一定好，与前面的文章一样，没有参考数据的比较没有意义；

3、 算法不是万能的，如果传感器的数据质量差，再牛逼的估计算法也很难得到精准的信息，所以最好对传感器做好结构、硬件上的处理，比如加速度减震、气压计的密封、遮光等等；

4、 调参怎么调？可以先取数据，分别画出各个中间数据的波形，调整得到个大概可用的值。然后根据实际的飞行情况进行调整，主要就是决定更相信哪个数据。

部分代码（kalman M代码）：