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

摘要

本文件公开声明了一种基于迭代强跟踪球面最简相径容积卡尔曼滤波(Iterated Strong Tracking Spherical Simplex‑Radial Cubature Kalman Filter,ISTSSRCKF)的初始对准方法，采用SSR规则选取CKF的容积点；将强跟踪滤波中的渐消因子引入到CKF的时间和量测更新方程之中；再将Gauss‑Newton迭代算法引入并改进迭代过程中相应的新息方差和协方差。对于在大失准角和晃动基座条件下的初始对准问题，本发明采用SSR规则采样减少了高阶CKF中过多的采样点，引入强跟踪算法克服了模型不准确时CKF性能下降的问题，使用迭代过程充分利用最新量测信息，从而有效地降低状态估计误差，获得优于标准CKF的初始对准精度。

主权项

一种基于ISTSSRCKF的惯性导航初始对准方法，其特征在于，包括以下步骤：步骤1：建立大失准角状态下的SINS晃动基座对准模型，所述对准模型包括SINS的非线性误差模型、非线性滤波状态模型和非线性滤波量测模型：其中，所述建立SINS非线性误差模型过程如下：步骤1.1：以东北天地理坐标系作为导航坐标系n，以载体右前上方向矢量右手定则构成坐标系作为载体坐标系b，真实姿态角为真实速度为真实地理坐标为p＝[L λ H]T；SINS实际解算出的姿态为速度为地理坐标为记由SINS解算的地理位置建立的坐标系为计算导航坐标系n′，定义SINS姿态角和速度误差分别为则φ、δνn的微分方程如下：其中，φ＝[φe φn φu]T为姿态角误差，所述姿态角误差具体为纵摇角、横摇角和航向角误差，δvn＝[δve δvn δvu]T为东向、北向和天向速度误差，为b系下三轴陀螺的常值误差，为b系下三轴陀螺的随机误差，为b系下三轴加速度计的常值误差，为b系下三轴加速度计的随机误差，为加速度计的实际输出，为SINS解算的速度，为计算的导航坐标系下的旋转角速度，为计算的地球旋转角速度，为计算的导航坐标系相对地球的旋转角速度，为对应的计算误差，为n系依次旋转角度φe、φn、φu得到n′系所形成的方向余弦矩阵，为b系到n′系状态转移矩阵，即计算的姿态矩阵，Cw为欧拉角微分方程的系数矩阵，上标T表示转置，矩阵和Cw具体为：所述非线性滤波状态模型建立过程如下：步骤1.2：取SINS的欧拉平台误差角φe、φn、φu，速度误差δve、δvn，b系下的陀螺常值误差b系下的加速度计常值误差构成状态向量在晃动基座下和近似为零，则非线性滤波的状态方程可简化为：其中，只取前两维状态，ω(t)＝[ωg ωa 01×3 01×2]T为零均值高斯白噪声过程,将该非线性滤波状态方程简记为：所述非线性量测方程的建立如下：步骤1.3：记在b系下的真实速度为vb，在b系下的实际速度为利用SINS解算的姿态矩阵把转换成以和中的东向和北向速度分量作为匹配信息源，则非线性滤波的量测方程为：其中，取z的前两维为观测值，v为零均值高斯白噪声过程，并将该连续非线性滤波量测方程简记为：z(t)＝h(x,t)+v(t)步骤2：以采样周期Ts作为滤波周期，并以Ts为步长进行离散化，将离散化为xk＝xk‑1+[f(xk‑1,tk‑1)+ω(tk‑1)]Ts，可得状态方程：xk＝f(xk‑1)+ωk‑1将z(t)＝h(x,t)+v(t)离散化为zk＝h(xk,tk)+v(tk)，可得量测方程：zk＝h(xk)+vk步骤2.1：由状态方程式和量测方程式组成如下离散非线性系统：式中，xk为k时刻系统的状态向量；zk为k时刻系统状态的量测值；随机系统噪声ωk～N(0,Qk)；随机观测噪声vk～N(0,Rk)；步骤2.2：设置系统状态初始值初始误差协方差阵：P0＝diag{(1°)2(1°)2(10°)2(0.1)2(0.1)2(0.01°/h)2(0.01°/h)2(0.01°/h)2(100μg)2(100μg)2}*10并对P0进行Cholesky分解，得到初始误差协方差阵的特征平方根S0；步骤2.3：采用SSR规则选取采样点，取如下一组向量：aj＝[aj,1 aj,2 … aj,n]T,j＝1,2,...,n+1，其中n为状态量的个数，则选用ξi表示第i个容积点，m＝2(n+1)个容积点为：式中n为状态量的个数，本文中n＝10；简记非线性多维积分为Q(f)，SSR规则下步骤2.4：计算状态一步预测值和一步预测误差协方差阵Pk/k‑1：通过Cholesky分解误差协方差阵Pk‑1/k‑1得到Sk‑1/k‑1：Pk‑1/k‑1＝Sk‑1/k‑1STk‑1/k‑1计算Cubature点(i＝1,2,…,n+1；m＝2(n+1))：通过状态方程传播Cubature点：X*i,k/k‑1＝f(Xi,k‑1/k‑1)状态一步预测值：一步预测误差协方差阵：步骤3：利用当前量测值zk减去相同时刻的量测预测值得到当前时刻的残差εk，依据强跟踪原理计算次优渐消因子λk，然后利用λk修正滤波时间更新过程；步骤4：以状态一步预测值和一步预测误差协方差阵Pk/k‑1为初始值进行迭代过程，进行最大迭代次数Nmax次迭代，完成量测更新；步骤5：利用滤波器滤波获得的欧拉平台角估计值和速度估计值修正SINS解算的姿态矩阵和速度将修正之后的值作为下一次捷联解算的初始值，利用当前获得的陀螺常值误差估计值和加速度计的常值误差估计值修正下一时刻的陀螺输出和加速度计的输出具体修正公式按下式进行计算：若精度达到预设的初始对准要求则结束，否则继续循环执行步骤2～5，直至初始对准结束。