一种IMU与激光测距机信息融合的高度修正方法

本申请提供一种IMU与激光测距机信息融合的高度修正方法，包括以下步骤：在静止状态下启动初始对准，获取水平姿态；惯导解算，根据水平姿态和IMU原始数据，解算出系统的速度、位置和姿态；姿态至少包括水平姿态角；先根据水平姿态角将激光测距值转为高度值，再对多个高度值拟合直线，获得预测速度值和预测高度值；通过卡尔曼滤波器对惯导解算状态和拟合直线数据进行融合，得到修正后的高度值。通过将多个激光测距值拟合出预测高度和速度作为量测值进入卡尔曼滤波器；同时，还根据预测的值计算出了该测距值的不确定性，合理分配了滤波器误差，这种方法能保证高度通道的速度和高度均不会随着运行时长而发散。