本发明属于导航技术领域，尤其涉及一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法。
背景技术：
近年来，海洋事业发展迅速，对船用捷联惯导系统的性能的要求越来越高。惯性测量组件误差约占系统误差的90％左右，即只有保证惯性测量组件的高精度，才能保证惯导系统的高精度。惯性系统的性能指标下降，导致不能满足导航的精度要求，因此必须通过各种方法提高精度。
目前，提高惯性器件精度主要从硬件和软件两个方面入手。(1)硬件层面：一是对现有的陀螺仪和加速度计进行深加工；二是当科学技术更加成熟的时候，有新型的材料能够取代现有的材料，并且新材料的性能非常卓越。(2)软件层面：深入研究惯性导航系统的算法，建立更加精准的系统模型，通过软件对系统进行标定，得到精度较高的误差数据，补偿进入惯性导航系统中，从而提高系统的输出精度。在惯性器件的精度达到一定程度的时候，通过软件的补偿继续提高精度成为首要选择，也能够有效的降低投入成本。
当前针对光纤陀螺等惯性测量组件的误差，一般都在实验室通过角增量实验、位置实验和零位误差修正实验进行分立式标定。但是惯性组件安装完成之后，环境的改变和时间的迁移会使误差参数与分立式标定的结果不一致，这会导致捷联惯导系统的精度降低。而频繁拆卸捷联惯导系统进行分立式标定会带来巨大的经济损失和工程实用负担。
技术实现要素：
发明目的：为解决现有技术存在的问题，本发明提出一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法，避免了光纤陀螺捷联惯性导航系统定期拆卸标定，有效提高光纤捷联惯导系统精度。
技术方案：一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法，根据船用的主导航系统提供的位置、速度和航向信息对船用光纤捷联的子惯导系统的惯性测量组件误差进行在线标定；其包括如下具体步骤：
步骤1：建立光纤陀螺、加速度计的输出误差模型；
步骤2：建立子惯导系统的位置、速度、姿态误差方程；
步骤3：通过子惯导系统解算的位置、速度、姿态信息和陀螺加速度计常值漂移、刻度因数误差、安装误差这些惯性测量组件误差参数作为滤波器状态量，应用模型预测滤波和自适应扩展卡尔曼滤波组合滤波的方法建立状态方程；
步骤4：以子惯导系统解算的位置、速度、航向信息和主导航系统提供的位置、速度、航向信息的差值作为滤波器的量测量，建立量测方程；
步骤5：在步骤3建立的状态方程的基础上，根据模型预测滤波计算模型误差从而修正一步预测状态量实现在线实时修正系统模型；
步骤6：采用Sage-Husa自适应扩展卡尔曼滤波，通过步骤4建立的量测方程和观测量中的航向、速度和位置信息对设定的量测噪声方差阵R进行估计和修正，得到估计状态的最优值；
步骤7：根据步骤5推导出的一步预测状态量结合步骤6中采用的Sage-Husa自适应扩展卡尔曼滤波，实现时间更新和量测更新，对步骤3中的滤波器状态量进行估计，用模型预测滤波修正自适应扩展卡尔曼滤波的状态一步预测值，得到子惯导系统中系统状态各项参数值，实现误差的估计与校正。
所述步骤1中建立光纤陀螺、加速度计的输出误差模型具体为：
将光纤陀螺的安装误差、刻度系数误差和常值漂移建入光纤陀螺误差模型，得：
式中，n为导航坐标系；b为载体坐标系；i为惯性坐标系；εn为载体坐标系下常值漂移；为从载体坐标系至导航坐标系的变换矩阵；为陀螺的输出；[δKG]为陀螺的刻度系数误差，[δG]为安装误差，
将加速度计的安装误差、刻度系数误差和常值漂移建入加速度计误差模型，得：
式中，为导航坐标系下常值漂移；fb为加速度计的输出；[δKA]为加速度计的刻度系数误差，[δA]为安装误差，
所述步骤2中建立子惯导系统的位置、速度、姿态误差方程的过程为：
建立姿态误差方程为：
式中，εn、δKGi和δGi(i＝x,y,z)分别为光纤陀螺的常值漂移、刻度系数误差和安装误差；φ为姿态误差角矢量；为导航坐标系相对惯性坐标系的角速度在导航坐标系下的投影；为导航坐标系相对惯性坐标系转动角速度计算误差；
建立速度误差方程为：
式中，φn为导航坐标系下姿态误差角；fb为载体坐标系下加速度计的输出；fn为导航坐标系下加速度计的输出；δKAi和δAi(i＝x,y,z)分别为加速度计的常值漂移、刻度系数误差和安装误差；
建立位置误差方程为：
式中，δVE、δVN为东北向速度误差；RN为沿卯酉圈的曲率半径；RM沿子午圈的曲率半径；λ为经度；L为纬度。
所述步骤3中通过子惯导系统解算的位置、速度、姿态信息和陀螺加速度计常值漂移、刻度因数误差、安装误差这些惯性测量组件误差参数作为滤波器状态量，应用模型预测滤波和自适应扩展卡尔曼滤波组合滤波的方法建立状态方程具体为：
式中，
D＝[δGx δGy δGz δKGx δKGy δKGz δAx δAy δKAx δKAy]。
式中：
φE、φN、φU——东北天失准角
δVE、δVN——东北向速度误差
δL、δλ——经纬度误差
εi(i＝x,y,z)——光纤陀螺的陀螺零偏
——X、Y的加速度计常值漂移
δKGi(i＝x,y,z)——光纤陀螺的刻度系数误差
δGi(i＝x,y,z)——光纤陀螺的安装误差
δKAi(i＝x,y)——加速度计的刻度系数误差
δAi(i＝x,y)——加速度计的安装误差
式中，A为系统向量矩阵；W为系统噪声矩阵；G1是模型误差分布阵；G2是系统噪声驱动阵。式中：
式中：
cij(i,j＝1,2,3)——姿态矩阵对应元素
VE、VN、VU——东北天向速度
L、λ——经纬度
R——WGS-84坐标系中的地球半径
ωie——地球自转角速度
ωi(i＝x,y,z)——载体运动角速度
fi(i＝x,y)——加速度计输出值。
在所述步骤4中应用以子惯导系统解算的位置、速度、航向信息和主导航系统提供的位置、速度、航向信息的差值作为滤波器的量测量，建立量测方程具体为：
Z＝HX+V
式中，Z为量测量；H为系统量测矩阵；V为系统量测噪声；
其中：
式中：
VME、VMN——主导航系统输出的东向和北向速度
VSE、VSN——子惯导导航系统输出的东向和北向速度
λM、LM——主导航系统的纬度和经度
λS、LS——子惯导导航输出的经度和纬度
ΨM——主导航系统的航向角
ΨS——子惯导导航输出的航向角
则系统的量测矩阵为：
所述步骤5中在步骤3建立的状态方程的基础上，根据模型预测滤波计算模型误差从而修正一步预测状态量实现在线实时修正系统模型具体为：
模型误差向量估计：
式中，Λ(Δt)∈Rm×m是对角阵，m是量测量的维数，其对角元素为：
是灵敏度矩阵，表示为：
式中，pi(i＝1,2,…,m)是D出现在H的第i个分量Hi的微分中的最低阶数；是k阶李导数；gi(i＝1,2,…,q)是G1的第i列；
T∈Rn×n是模型误差加权矩阵；如果模型误差相对而言比较小，T的值应该要取大；如果误差比较大，那么T的值应该取小；
是m维列向量，表示为：
综上：
由此推得自适应卡尔曼滤波(AEKF)的一步预测为：
所述步骤6中采用Sage-Husa自适应扩展卡尔曼滤波，通过步骤4建立的量测方程和观测量中的航向、速度和位置信息对设定的量测噪声方差阵R进行估计和修正，得到估计状态的最优值具体为：
利用观测量带来的信息，在估计子惯导系统的系统状态的同时估计量测噪声方差阵；
mk+1＝Zk+1-Hk+1Xk+1/k
Rk+1＝(1-dk+1)Rk+dk+1[mk+1mk+1T-Hk+1Pk/kHk+1T]
式中，dk+1＝(1-b)(1-bk+1)，b为遗忘因子，在[0.95,0.995]间取值；当b较小时，Rk+1对当前时刻量测信息依赖的权重较大；当b较大时，Rk+1对历史的量测信息依赖的权重较大。
所述步骤7根据步骤5推导出的一步预测状态量结合步骤6中采用的Sage-Husa自适应扩展卡尔曼滤波，实现时间更新和量测更新，对步骤3中的滤波器状态量进行估计，用模型预测滤波修正自适应扩展卡尔曼滤波的状态一步预测值，得到子惯导系统中系统状态各项参数值，实现误差的估计与校正具体为：
计算模型误差：
自适应扩展卡尔曼滤波滤波结合模型预测滤波得到系统状态的一步预测状态量为：
系统状态的状态转移阵为：
最优滤波值为：
偏差为：
一步预测误差方差阵为：
Pk+1/k＝Φk+1,kPk/kΦk+1,kT+G2QkWT
新的信息为：
采用Sage-Husa自适应扩展卡尔曼滤波方法对量测噪声方差阵进行动态估计：
卡尔曼滤波增益阵为：
系统的估计误差方差阵为：
有益效果：本发明根据船用主导航系统的位置、速度和航向信息，采用“位置+速度+航向角”的匹配模式，对光纤捷联惯导系统进行在线标定。本发明采用模型预测滤波修正状态一步预测值，估计惯性器件各项参数误差值，并采用简化的Sage-Husa自适应扩展卡尔曼滤波对量测噪声方差阵不断进行估计和修正，提高了滤波精度，实现了在线标定。相对于现有技术，本发明利用主导航系统输出信息，使用相应的滤波方法在线估测出惯性测量组件的各标定参数，从而解决惯性测量组件的在线标定问题，避免了光纤陀螺捷联惯性导航系统定期拆卸标定，有效提高光纤捷联惯导系统精度，具有突出的应用价值。
附图说明
图1为本发明流程图。
具体实施方式
下面将结合附图，对本发明的实施案例进行详细的描述：
如图1所示，一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法，根据船用主导航系统提供的位置、速度和航向信息对船用光纤捷联子惯导系统的惯性测量组件误差进行在线标定，其包括如下具体步骤：
步骤1：建立光纤陀螺、加速度计的输出误差模型；
将光纤陀螺的安装误差、刻度系数误差和常值漂移建入光纤陀螺误差模型，得：
式中，n为导航坐标系；b为载体坐标系；i为惯性坐标系；εn为载体坐标系下常值漂移；为从载体坐标系至导航坐标系的变换矩阵；为陀螺的输出；[δKG]为陀螺的刻度系数误差，[δG]为安装误差，
将加速度计的安装误差、刻度系数误差和常值漂移建入加速度计误差模型，得：
式中，为导航坐标系下常值漂移；fb为加速度计的输出；[δKA]为加速度计的刻度系数误差，[δA]为安装误差，
步骤2：建立子惯导系统的位置、速度、姿态误差方程；
建立姿态误差方程为：
式中，εn、δKGi和δGi(i＝x,y,z)分别为光纤陀螺的常值漂移、刻度系数误差和安装误差；φ为姿态误差角矢量；为导航坐标系相对惯性坐标系的角速度在导航坐标系下的投影；为导航坐标系相对惯性坐标系转动角速度计算误差。
建立速度误差方程为：
式中，φn为导航坐标系下姿态误差角；fb为载体坐标系下加速度计的输出；fn为导航坐标系下加速度计的输出；δKAi和δAi(i＝x,y,z)分别为加速度计的常值漂移、刻度系数误差和安装误差；
建立位置误差方程为：
式中，δVE、δVN为东北向速度误差；RN为沿卯酉圈的曲率半径；RM沿子午圈的曲率半径；λ为经度；L为纬度。
步骤3：通过子惯导系统解算的位置、速度、姿态信息和陀螺加速度计常值漂移、刻度因数误差、安装误差等惯性测量组件误差参数作为滤波器状态量，应用模型预测滤波(Model Predictive Filter)和自适应扩展卡尔曼滤波(Adaptive Extended Kalman Filter)组合滤波的方法建立状态方程；
式中，
D＝[δGx δGy δGz δKGx δKGy δKGz δAx δAy δKAx δKAy]。
式中：
φE、φN、φU——东北天失准角
δVE、δVN——东北向速度误差
δL、δλ——经纬度误差
εi(i＝x,y,z)——光纤陀螺的陀螺零偏
——X、Y的加速度计常值漂移
δKGi(i＝x,y,z)——光纤陀螺的刻度系数误差
δGi(i＝x,y,z)——光纤陀螺的安装误差
δKAi(i＝x,y)——加速度计的刻度系数误差
δAi(i＝x,y)——加速度计的安装误差
式中，A为系统向量矩阵；W为系统噪声矩阵；G1是模型误差分布阵；G2是系统噪声驱动阵。式中：
式中：
cij(i,j＝1,2,3)——姿态矩阵对应元素
VE、VN、VU——东北天向速度
L、λ——经纬度
R——WGS-84坐标系中的地球半径
ωie——地球自转角速度
ωi(i＝x,y,z)——载体运动角速度
fi(i＝x,y)——加速度计输出值。
步骤4：以子惯导系统解算的位置、速度、航向信息和主导航系统提供的位置、速度、航向信息的差值作为滤波器的量测量，建立量测方程；
Z＝HX+V
式中，Z为量测量；H为系统量测矩阵；V为系统量测噪声。
其中：
式中：
VME、VMN——主导航系统输出的东向和北向速度
VSE、VSN——子惯导导航系统输出的东向和北向速度
λM、LM——主导航系统的纬度和经度
λS、LS——子惯导导航输出的经度和纬度
ΨM——主导航系统的航向角
Ψs——子惯导导航输出的航向角
则系统的量测矩阵为：
步骤5：在步骤3建立的状态方程的基础上，根据模型预测滤波计算模型误差从而修正一步预测状态量实现在线实时修正系统模型；
模型误差向量估计：
式中，Λ(Δt)∈Rm×m是对角阵，m是量测量的维数，其对角元素为：
是灵敏度矩阵，表示为：
式中，pi(i＝1,2,…,m)是D出现在H的第i个分量Hi的微分中的最低阶数；是k阶李导数；gi(i＝1,2,…,q)是G1的第i列；
T∈Rn×n是模型误差加权矩阵；如果模型误差相对而言比较小，T的值应该要取大；如果误差比较大，那么T的值应该取小；
是m维列向量，可表示为：
综上：
由此推得自适应卡尔曼滤波(AEKF)的一步预测为：
步骤6：采用改进的Sage-Husa自适应扩展卡尔曼滤波，通过步骤4建立的量测方程和观测量中的航向、速度和位置信息对设定的量测噪声方差阵R进行估计和修正，得到估计状态的最优值；
利用观测量带来的信息，在估计子惯导系统的系统状态的同时估计量测噪声方差阵：
mk+1＝Zk+1-Hk+1Xk+1/k
Rk+1＝(1-dk+1)Rk+dk+1[mk+1mk+1T-Hk+1Pk/kHk+1T]
式中，dk+1＝(1-b)(1-bk+1)，b为遗忘因子，在[0.95,0.995]间取值；当b较小时，Rk+1对当前时刻量测信息依赖的权重较大；当b较大时，Rk+1对历史的量测信息依赖的权重较大。
步骤7：根据步骤5推导出的一步预测状态量结合步骤6中采用的Sage-Husa自适应扩展卡尔曼滤波，实现时间更新和量测更新，对步骤3中的滤波器状态量进行估计，用模型预测滤波修正自适应扩展卡尔曼滤波的状态一步预测值，得到子惯导系统中系统状态各项参数值，实现误差的估计与校正。
计算模型误差：
自适应扩展卡尔曼滤波滤波结合模型预测滤波滤波可得到系统状态的一步预测状态量为：
系统状态的状态转移阵为：
最优滤波值为：
偏差为：
一步预测误差方差阵为：
Pk+1/k＝Φk+1,kPk/kΦk+1,kT+G2QkWT
新息为：
采用Sage-Husa自适应扩展卡尔曼滤波方法对量测噪声方差阵进行动态估计：
卡尔曼滤波增益阵为：
系统的估计误差方差阵为：