[发明专利]一种基于5阶SSRCKF的SINS/DVL组合导航滤波方法

权利要求书

1.一种基于5阶SSRCKF的SINS/DVL组合导航滤波方法，其特征在于，包括如下步骤：

1)建立SINS非线性误差模型：在SINS中选取东北天地理坐标系作为导航坐标系，以水下航行器自身建立载体坐标系，并给定实际计算导航坐标系，建立惯性器件陀螺仪和加速度计的误差模型，并求解系统的姿态误差方程、位置误差方程；

2)建立DVL非线性误差模型：针对DVL误差中的刻度误差以及随机测量误差，以一阶马尔科夫过程来拟合误差方程；

3)根据步骤1)和步骤2)分别得到的SINS非线性误差模型和DVL非线性误差模型建立组合导航滤波模型：选取SINS的三维姿态角误差、速度误差的前两维、位置误差的前两维，三维陀螺常值漂移和加速度计的水平方向的两维常值误差的12维状态变量，选取DVL的刻度误差和随机测量误差作为状态量；然后根据步骤1)和步骤2)分别得到的SINS非线性误差模型和DVL非线性误差模型共同建立组合导航的状态方程；以DVL和SINS的东向、北向速度分量的差值作为观测量，建立SINS/DVL组合导航系统的观测方程；

4)建立5阶SSRCKF滤波模型，输出导航系统参数：利用5阶SSRCKF对SINS和DVL输出的信息进行滤波、融合，得到导航参数的最优估计并修正速度、位置和姿态矩阵，得到精确的导航信息，具体包括以下步骤：

41)基于SSR容积采样规则：高斯滤波积分归纳成其中

采用基于SSR容积采样规则，求解球面积分S(r)，对于拥有n维状态量的系统，求解过程如下：取一组n维向量a_i＝[a_i1，a_i2，a_i3，…，a_in]^T，i＝1，2，…，n+1，并将向量a_i的中点投影到球面U_n得到序列则有

选用[ξ_i，ω_i]表示滤波器的采样容积点，其中则得到基于SSR规则下5阶非线性多维积分为：

其中：

F(ξ_i)＝f(ξ_i)+f(-ξ_i)

F(ω_i)＝f(ω_i)+f(-ω_i)；

42)时间更新过程：估计方差P_k-1/k-1通过乔列斯基(Cholesky)分解得：根据基于SSR容积规则选取容积点集合[ξ_i，ω_i]，计算容积点：通过状态方程来传导容积点：因此k时刻的一步状态预测和一步预测方差为：

43)观测过程更新流程：通过观测方程传导容积点：Z_i，k/k-1＝H(X_i，k/k-1)，k时刻的观测量Z的预测以及滤波增益矩阵为：

因此k时刻的状态估计和均方差估计为：