[发明专利]基于ISTSSRCKF的惯性导航初始对准方法

权利要求书

1.一种基于ISTSSRCKF的惯性导航初始对准方法，其特征在于，包括以下步骤：

步骤1：建立大失准角状态下的SINS晃动基座对准模型，所述对准模型包括SINS的非线性误差模型、非线性滤波状态模型和非线性滤波量测模型：

其中，所述建立SINS非线性误差模型过程如下：

步骤1.1：以东北天地理坐标系作为导航坐标系n，以载体右前上方向矢量右手定则构成坐标系作为载体坐标系b，真实姿态角为真实速度为真实地理坐标为p＝[L λ H]^T；SINS实际解算出的姿态为速度为地理坐标为记由SINS解算的地理位置建立的坐标系为计算导航坐标系n′，定义SINS姿态角和速度误差分别为则φ、δνⁿ的微分方程如下：

其中，φ＝[φ_e φ_n φ_u]^T为姿态角误差，所述姿态角误差具体为纵摇角、横摇角和航向角误差，δvⁿ＝[δv_e δv_n δv_u]^T为东向、北向和天向速度误差， 为b系下三轴陀螺的常值误差，为b系下三轴陀螺的随机误差，为b系下三轴加速度计的常值误差，为b系下三轴加速度计的随机误差，为加速度计的实际输出，为SINS解算的速度，为计算的导航坐标系下的旋转角速度，为计算的地球旋转角速度，为计算的导航坐标系相对地球的旋转角速度，为对应的计算误差，为n系依次旋转角度φ_e、φ_n、φ_u得到n′系所形成的方向余弦矩阵，为b系到n′系状态转移矩阵，即计算的姿态矩阵，C_w为欧拉角微分方程的系数矩阵，上标T表示转置，矩阵和C_w具体为：

所述非线性滤波状态模型建立过程如下：

步骤1.2：取SINS的欧拉平台误差角φ_e、φ_n、φ_u，速度误差δv_e、δv_n，b系下的陀螺常值误差b系下的加速度计常值误差构成状态向量在晃动基座下和 近似为零，则非线性滤波的状态方程可简化为：

其中，只取前两维状态，ω(t)＝[ω_g ω_a 0_1×3 0_1×2]^T为零均值高斯白噪声过程,将该非线性滤波状态方程简记为：

所述非线性量测方程的建立如下：

步骤1.3：记在b系下的真实速度为v^b，在b系下的实际速度为利用SINS解算的姿态矩阵把转换成以和中的东向和北向速度分量作为匹配信息源，则非线性滤波的量测方程为：

其中，取z的前两维为观测值，v为零均值高斯白噪声过程，并将该连续非线性滤波量测方程简记为：

z(t)＝h(x,t)+v(t)

步骤2：以采样周期T_s作为滤波周期，并以T_s为步长进行离散化，将 离散化为x_k＝x_k-1+[f(x_k-1,t_k-1)+ω(t_k-1)]T_s，可得状态方程：

x_k＝f(x_k-1)+ω_k-1

将z(t)＝h(x,t)+v(t)离散化为z_k＝h(x_k,t_k)+v(t_k)，可得量测方程：

z_k＝h(x_k)+v_k

步骤2.1：由状态方程式和量测方程式组成如下离散非线性系统：

式中，x_k为k时刻系统的状态向量；z_k为k时刻系统状态的量测值；随机系统噪声ω_k～N(0,Q_k)；随机观测噪声v_k～N(0,R_k)；

步骤2.2：设置系统状态初始值初始误差协方差阵：

P₀＝diag{(1°)²(1°)²(10°)²(0.1)²(0.1)²

(0.01°/h)²(0.01°/h)²(0.01°/h)²(100μg)²(100μg)²}*10

并对P₀进行Cholesky分解，得到初始误差协方差阵的特征平方根S₀；

步骤2.3：采用SSR规则选取采样点，取如下一组向量：a_j＝[a_j,1 a_j,2 … a_j,n]^T,j＝1,2,...,n+1，其中n为状态量的个数，则

选用ξ_i表示第i个容积点，m＝2(n+1)个容积点为：

式中n为状态量的个数，本文中n＝10；简记非线性多维积分 为Q(f)，SSR规则下

步骤2.4：计算状态一步预测值和一步预测误差协方差阵P_k/k-1：通过Cholesky分解误差协方差阵P_k-1/k-1得到S_k-1/k-1：

P_k-1/k-1＝S_k-1/k-1S^T_k-1/k-1

计算Cubature点(i＝1,2,…,n+1；m＝2(n+1))：

通过状态方程传播Cubature点：

X^*_i,k/k-1＝f(X_i,k-1/k-1)

状态一步预测值：

一步预测误差协方差阵：

步骤3：利用当前量测值z_k减去相同时刻的量测预测值得到当前时刻的残差ε_k，依据强跟踪原理计算次优渐消因子λ_k，然后利用λ_k修正滤波时间更新过程；

步骤4：以状态一步预测值和一步预测误差协方差阵P_k/k-1为初始值进行迭代过程，进行最大迭代次数N_max次迭代，完成量测更新；

步骤5：利用滤波器滤波获得的欧拉平台角估计值和速度估计值修正SINS解算的姿态矩阵和速度将修正之后的值作为下一次捷联解算的初始值，利用当前获得的陀螺常值误差估计值和加速度计的常值误差估计值修正下一时刻的陀螺输出和加速度计的输出具体修正公式按下式进行计算：

若精度达到预设的初始对准要求则结束，否则继续循环执行步骤2～5，直至初始对准结束。