[发明专利]一种基于惯导/DVL/USBL组合的惯导误差估计方法

摘要

本发明属于惯导技术，具体公开一种基于惯导/DVL/USBL组合的惯导误差估计方法，包括同步采集惯导的位置和速度、DVL的速度、USBL的位置等测试数据，并设计三个通过经典Kalman滤波实现的滤波器的配合：采用子滤波器1实现惯导/DVL组合、采用子滤波器2实现惯导/USBL组合，采用主滤波器，获得惯导误差的高精度估计；从而实现了高环境适应性，低位置累计误差的惯导误差估计方法。

主权项

一种基于惯导/DVL/USBL组合的惯导误差估计方法，其特征在于：它包括下列步骤：步骤1：同步采集惯导的位置和速度、DVL的速度、USBL的位置等测试数据；步骤2：采用子滤波器1实现惯导/DVL组合；子滤波器1的输入信息为惯导的速度与DVL的速度；子滤波器1的状态变量为17维向量：其中，φN、φU、φE分别为惯导的北向、天向、东向轴失准角；δvN、δvU、δvE分别为惯导的北向、天向、东向轴速度误差；分别为惯导的纬度、高度和经度误差；εN、εU、εE和▽N、▽U、▽E分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθs为DVL的安装误差角，δks为DVL的刻度系数误差；经过经典的Kalman滤波方程解算，获得子滤波器1的状态估计值和估计均方误差阵；取子滤波器1的状态估计值的前15个变量，记作估计均方误差阵的前15行前15列矩阵，记作P1；步骤3：采用子滤波器2实现惯导/USBL组合；子滤波器2的输入信息为惯导的位置与USBL的位置；子滤波器2的状态变量为17维向量：其中，φN、φU、φE分别为惯导的北向、天向、东向轴失准角；δvN、δvU、δvE分别为惯导的北向、天向、东向轴速度误差；分别为惯导的纬度、高度和经度误差；εN、εU、εE和▽N、▽U、▽E分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθa为USBL的安装误差角，δka为USBL的刻度系数误差；经过经典的Kalman滤波方程解算，获得子滤波器2的状态估计值和估计均方误差阵；取子滤波器2的状态估计值的前15个变量，记作估计均方误差阵的前15行前15列矩阵，记作P2；步骤4：采用主滤波器，获得惯导误差的高精度估计；主滤波器的输入信息为状态估计值估计均方误差阵P1、状态估计值和估计均方误差阵P2，计算融合估计误差方差阵，公式为：Pg=(P1-1+P2-1)-1]]>其中，为P1的逆矩阵，为P2的逆矩阵；计算主滤波器的状态估计值，公式为：X^g=Pg(P1-1X^1+P2-1X^2)]]>为主滤波器估计出的惯导误差。