本发明涉及水下滑翔装置，具体涉及一种用于水下滑翔装置的声纳高度计的数据滤波方法及装置。
背景技术：
1、水下滑翔装置是一种新型的水下机器人。其利用净浮力和姿态角调整获得推进力，能源消耗极小，满足了长时间、大范围海洋探索的需要。图1是水下滑翔装置下潜过程示意图。在实际应用中，常常需要了解水下滑翔装置下潜过程中距离海底的距离，防止发生撞击。探测离底高度的一种重要工具是声呐高度计，但是由于水下滑翔装置运行的环境复杂,水声信号的噪声大,而水声传感器普遍存在精度较差、跳变频繁的缺点,因此需要对所测数据进行近一步的处理,才能得到满意的精度。
2、卡尔曼滤波自1960年问世以来，经过50多年的发展，已经形成了一套比较完整的理论体系，并且成功应用于航空、航天、工业控制等领域。但是它要求系统模型精确，特别是状态噪声和观测噪声模型已知，这在实际应用中很难得到满足或者在系统工作过程中,模型发生变化,这些都可能导致标准卡尔曼滤波的发散或精度降低。本发明通过基于观测噪声n次迭代修正的自适应卡尔曼滤波算法解决该问题。
技术实现思路
1、有鉴于此，本发明提供了一种用于水下滑翔装置的声纳高度计的数据滤波方法及装置，能够解决水下滑翔装置声呐高度计精度较差、跳变频繁的技术问题。
2、为了解决上述技术问题，本发明是这样实现的。
3、一种用于水下滑翔装置的声纳高度计的数据滤波方法，所述方法包括：
4、步骤s1：针对水下滑翔装置在水下环境的垂直面运动过程，建立所述水下滑翔装置的状态向量；基于所述状态向量建立所述水下滑翔装置的状态方程；
5、步骤s2：基于声呐高度计采样值与水下滑翔装置离底高度的关系，建立所述水下滑翔装置的观测方程；
6、步骤s3：定义进行连续两次高度采样的采样时刻的时间间隔为一个步长，在每个采样时刻，通过步长为1、以及步长为n的卡尔曼滤波算法分别对当前状态进行计算，得到所述水下滑翔装置的当前状态的第一滤波结果以及第二滤波结果分别表示步长为1和n时，第k时刻的第i个状态；
7、步骤s4：基于预先设定的两个窗口距离矩阵和判断所述第一滤波结果与所述第二滤波结果的变化量是否大于预设阈值，若大于预设阈值，则调整所述观测方程中的观测噪声协方差矩阵r，进入步骤s3；否则，将状态方程基于此时的水下滑翔装置的状态参数求得的状态估计值作为滤波后的声纳高度计的数值。
8、优选地，所述水下滑翔装置的状态向量为xk＝[hk d′k d″k]t，其中，hk为tk时刻水下滑翔装置离底高度，dk为tk时刻水下滑翔装置深度，dk′，dk″分别为dk的一阶导数和二阶导数。
9、优选地，所述水下滑翔装置的状态方程为
10、
11、其中，t为水下滑翔装置控制系统采样周期，hk-1为tk-1时刻水下滑翔装置离底高度，dk-1′为tk-1时刻水下滑翔装置垂直方向速度，dk-1″为tk-1时刻水下滑翔装置垂直方向加速度，wk-1为tk-1时刻过程激励噪声。
12、优选地，所述观测方程为：
13、
14、其中，α为水下滑翔装置下潜过程中的倾斜角，β为声纳高度计与水下滑翔装置水平面的夹角，vk为tk时刻观测噪声，该噪声是服从正态分布n(0,r)的高斯白噪声，r表示观测噪声协方差矩阵。
15、优选地，所述基于预先设定的两个窗口距离矩阵和判断所述第一滤波结果与所述第二滤波结果的变化量是否大于预设阈值，若大于预设阈值，则调整所述观测方程中的观测噪声协方差矩阵r，包括：
16、若令r1＝λmaxr，其中λmax为大于1的固定值，从而增大观测噪声协方差的值，降低其在滤波结果中的比重，r1为调整后的观测噪声协方差矩阵r的值；
17、若令r1＝λminr，其中0<λmin<1，降低观测噪声协方差值，增大其在滤波结果中的比重。
18、本发明所提供的一种用于水下滑翔装置的声纳高度计的数据滤波装置，所述装置包括：
19、状态方程模块：配置为针对水下滑翔装置在水下环境的垂直面运动过程，建立所述水下滑翔装置的状态向量；基于所述状态向量建立所述水下滑翔装置的状态方程；
20、观测方程模块：配置为基于声呐高度计采样值与水下滑翔装置离底高度的关系，建立所述水下滑翔装置的观测方程；
21、滤波结果确定模块：配置为定义进行连续两次高度采样的采样时刻的时间间隔为一个步长，在每个采样时刻，通过步长为1、以及步长为n的卡尔曼滤波算法分别对当前状态进行计算，得到所述水下滑翔装置的当前状态的第一滤波结果以及第二滤波结果分别表示步长为1和n时，第k时刻的第i个状态；
22、调整模块：配置为基于预先设定的两个窗口距离矩阵和判断所述第一滤波结果与所述第二滤波结果的变化量是否大于预设阈值，若大于预设阈值，则调整所述观测方程中的观测噪声协方差矩阵r，进入步骤s3；否则，将状态方程基于此时的水下滑翔装置的状态参数求得的状态估计值作为滤波后的声纳高度计的数值。
23、本发明所提供的一种计算机可读存储介质，所述存储介质中存储有多条指令；所述多条指令，用于由处理器加载并执行如前所述方法。
24、本发明所提供的一种电子设备，其特征在于，所述电子设备，包括：
25、处理器，用于执行多条指令；
26、存储器，用于存储多条指令；
27、其中，所述多条指令，用于由所述存储器存储，并由所述处理器加载并执行如前所述方法。
28、本发明所带来的有益技术效果：
29、(1)本发明能大幅提高声呐高度计精度、抑制数据跳变。
30、(2)本发明考虑了水下滑翔装置下潜过程中的运动特性，自动调整观测噪声协方差的估计值，从而更好的符合实际的观测情况，最终达到有效改善水下滑翔装置声呐高度计性能的目的。
31、(3)本发明实现简单，且降低了水下滑翔装置对所携带声呐高度计探测精度的要求。
1.一种用于水下滑翔装置的声纳高度计的数据滤波方法，其特征在于，包括：
2.如权利要求1所述的方法，其特征在于，所述水下滑翔装置的状态向量为xk＝[hk d′kd″k]t，其中，hk为tk时刻水下滑翔装置离底高度，dk为tk时刻水下滑翔装置深度，dk′，dk″分别为dk的一阶导数和二阶导数。
3.如权利要求2所述的方法，其特征在于，所述水下滑翔装置的状态方程为
4.如权利要求3所述的方法，其特征在于，所述观测方程为：
5.如权利要求1-4中任一项所述的方法，其特征在于，所述基于预先设定的两个窗口距离矩阵和判断所述第一滤波结果与所述第二滤波结果的变化量是否大于预设阈值，若大于预设阈值，则调整所述观测方程中的观测噪声协方差矩阵r，包括：
6.一种用于水下滑翔装置的声纳高度计的数据滤波装置，其特征在于，包括：
7.一种计算机可读存储介质，所述存储介质中存储有多条指令；所述多条指令，用于由处理器加载并执行如权利要求1-5中任一项所述方法。
8.一种电子设备，其特征在于，所述电子设备，包括：