pixhawk的高度解算算法解读

原地址：http://blog.sina.com.cn/s/blog_8fe4f2f40102wo50.html

做定高控制时，不可避免的就要涉及到如何解算出高度信息，那高度信息又是如何获取的？参考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下编程进行数据分析。

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

以上所述：如有错误，欢迎指出。

另：1.预测函数很好理解，但是校正函数是根据什么理论推导出来的？目前尚不清楚。
2.该算法用来气压计和加速度数据，因此减震需要做好，气压计还受温飘影响，数据在开始阶段会漂移。