一种捷联惯性/卫星组合导航滤波方法。首先，建立包含模型误差的捷联惯性导航系统(SINS)/全球导航卫星系统(GNSS)组合导航滤波非线性数学模型；其次，对传统的二阶插值滤波进行改进，并将改进后的二阶插值滤波与预测滤波相结合，利用预测滤波在线估计出的模型误差修正改进二阶插值滤波器的状态，实现对真实误差状态量——SINS/GNSS组合导航的姿态误差、速度误差及位置误差的精确估计；最后利用估计出的误差状态量计算出更加准确的SINS/GNSS组合导航的位置、速度和姿态。
1.一种捷联惯性/卫星组合导航滤波方法，其特征在于实现步骤如下：(1)建立包含模型误差的捷联惯性导航系统/全球导航卫星系统组合导航滤波非线性数学模型；
(2)对传统二阶插值滤波进行改进，并基于步骤(1)建立的数学模型对状态进行估计，然后利用预测滤波估计出的模型误差修正改进二阶插值滤波的状态估计，得到修正后的姿态误差、位置误差和速度误差；
(3)利用以上步骤(2)得到的修正后的姿态误差、位置误差和速度误差对捷联惯性导航系统捷联解算出的姿态、位置和速度进行补偿，获得更加准确的导航信息，即补偿后的姿态、位置和速度信息；
(4)将以上步骤(3)中补偿后的位置、速度和姿态作为下一导航时刻的初始值，不断重复以上步骤(2)和步骤(3)，直至捷联惯性导航系统/全球导航卫星系统组合导航结束；
所述的步骤(1)中，包含模型误差的捷联惯性导航系统/全球导航卫星系统组合导航滤波的非线性数学模型包括状态方程和量测方程，其中状态方程为：T
其中，x＝[δL δλ δh δVE δVN δVU δq0 δq1 δq2 δq3 εx εy εz] 为状态变量；δL、δλ、δh为位置误差，分别代表纬度误差、经度误差、高度误差；δVE、δVN、δVU为速度误差，分别代表东向速度误差、北向速度误差、天向速度误差；δq0、δq1、δq2、δq3为姿态误差，即计算四元数 与真实四元数(q0、q1、q2、q3)各分量的差；εx、dεy、εz分别为载体坐标系中x、y、z轴上陀螺常值漂移；d为模型误差；G 为模型误差分布w矩阵；G 为系统噪声输入矩阵；w为系统噪声，Q为系统噪声方差阵；f(x)的具体表达式如下Tf(x)＝[f1 f2 f3 f4]
其中，RM和RN分别为沿子午圈和卯酉圈的主曲率半径；L、λ、h分别为捷联惯性n T导航系统捷联解算出的纬度、经度和高度；V ＝[VE VN VU] 为捷联惯性导航系统捷联解算出的载体相对地球的速度在导航坐标系中的投影，VE、VN、VU分别代表东向、北向和T天向速度； δV＝[δVE δVN δVU] ； 为SINS捷联解算出的载体坐标系到计算导航坐标系的方向余弦矩阵，也称为计算姿态矩阵；
和 分别为载体坐标系到导航坐标系的真实四元数和计算四元数； 为加速度计的测量值； 为地球坐标系相对惯性系的旋转角速度在导航坐标系的投影； 为导航坐标系相对地球坐标系的旋转角速度在导航坐标系的投影；
和 分别为 和 的计算值； 为陀螺实际测量值， 为陀螺的理论输出，ωx、ωy、ωz分别代表载体坐标系中x、y、z轴上陀螺的理论输出， 表示陀螺的测量误差； 为计算导航坐标系相对惯性坐标系的旋转角速度， 为真实导航坐标系相对惯性坐标系的旋转角速度在导航坐标系的投影，ωE、ωN、ωU分别为 在真实导航坐标系中东向、北向、天向轴上的投影； 为 的计算误差；dx、dy、dz代表模型误差分量，包含载体坐标系中x、y、z轴上的加速度计误差等未知模型误差量；N(δQ)和Y(δQ)是由δq0、δq1、δq2、δq3组成的矩阵，和 是由 组成的矩阵， 和 是分别由ωx、ωy、ωz和ωE、ωN、ωU组成的矩阵，N(δQ)、 Y(δQ)、 和 的具体定义如下取捷联惯性导航系统捷联解算与全球导航卫星系统的速度和位置之差作为量测值，量测方程为：y＝H(x)+v
其中y＝[δL′δλ′δh′δV′E δV′N δV′U] 为量测变量，δL′，δλ′和δh′分别表示捷联惯性导航系统与全球导航卫星系统输出的纬度、经度和高度之差；
δV′E，δV′N和δV′U分别表示捷联惯性导航系统与全球导航卫星系统输出的东向、北向和天向速度之差；量测噪声 vδL′、vδλ′、vδH′、分别代表全球导航卫星系统的纬度、经度、高度、东向速度、北向速度、天向速度的量测噪声；量测噪声方差阵R根据全球导航卫星系统位置、速度噪声水平选取；H(x)T的具体表达式为H(x)＝[(RM+h)δL(RN+h)cosL·δλ δh δVE δVN δVU] ；
所述的步骤(2)中对传统二阶插值滤波进行改进，并基于步骤(1)建立的数学模型对状态进行估计的步骤为：a)计算出tk-1时刻协方差阵 和系统噪声方差阵Qk-1均方根的各列向量i＝1，2，…，13
其中，下标k-1表示tk-1时刻； 表示矩阵 的平方根矩阵中的第i列；
b)计算系统状态一步预测 和状态一步预测协方差阵其中，下标k/k-1表示从tk-1到tk时刻的预测； 为tk-1时刻的状态估计，wk-1为系统噪声w在tk-1时刻的值。
c)计算出tk时刻状态协方差阵 和量测噪声方差阵Rk均方根的各列向量其中，下标k表示tk时刻； 表示矩阵 的平方根矩阵中的第i列； 表示矩阵Rk的平方根矩阵中的第i列；
d)量测修正更新，即计算量测一步预测 量测协方差阵 协方差阵 滤波增益Kk、状态估计 和状态协方差阵
其中，yk和vk分别为量测变量y和量测噪声v在tk时刻的值；
e)状态修正更新，首先计算平滑值 然后用 替换 重新计算 和综合以上a)至e)步骤，即为改进后的二阶插值滤波算法。
2.根据权利要求1所述的捷联惯性/卫星组合导航滤波方法，其特征在于：所述的步骤(1)中预测滤波估计模型误差d的步骤为：a)由tk-1时刻系统的状态估值 计算系统的输出估值 计算公式为b)估计出tk-1时刻的模型误差 计算公式为其中， 为6维列向量，其各分量为
Λ(Δt)∈R 为对角阵，其对角元素为i＝1，...，6
3.根据权利要求1所述的捷联惯性/卫星组合导航滤波方法，其特征在于：所述的步骤(2)中利用预测滤波估计出的模型误差修正改进二阶插值滤波算法的状态估计，得到修正后的姿态误差、位置误差和速度误差的步骤为：a)在改进二阶插值滤波 的计算式中增加修正项 以修正模型误差引起的状态估计误差
其中， 为模型误差分布矩阵Gd在tk-1时刻的值；
b)利用步骤a)中计算出的 重新计算 和以上获得的 即为最终修正后的状态变量，包括姿态误差、位置误差、速度误差。
4.根据权利要求1所述的捷联惯性/卫星组合导航滤波方法，其特征在于：所述的步骤(3)修正SINS捷联解算出的姿态、位置和速度的公式为：a)姿态修正
式中φE、φN和φU为导航坐标系东向、北向和天向轴上的数学平台失准角， 为 姿 态 误 差， 和
分别为载体坐标系到导航坐标系的真实四元数和计算四元数，是由 组成的矩阵，
利用 修正捷联惯性导航系统捷联解算出的姿态矩阵其中， 即为修正后的姿态矩阵，根据 即可计算出捷联惯性导航系统/全球导航卫星系统组合导航的姿态角，所述姿态角包括航向角、俯仰角和横滚角；
其中，V′E、V′N和V′U为修正后的东向速度、北向速度和天向速度；
其中，L′、λ′和h′为修正后的纬度、经度和高度。
一种捷联惯性/卫星组合导航滤波方法
技术领域
[0001] 本发明涉及一种捷联惯性导航系统(Strapdown Inertial Navigation System，SINS)与全球导航卫星系统(Global Navigation Satellite System，GNSS)组合导航的滤波方法，可用于航空、航海、陆地等领域的SINS/GNSS组合导航系统。
背景技术
[0002] 捷联惯性导航系统(SINS)的基本原理是根据牛顿提出的相对惯性空间的力学定律，利用陀螺仪、加速度计测量载体相对惯性空间的线运动和角运动参数，在给定的运动初始条件下，由计算机进行积分运算，连续、实时地提供位置、速度和姿态信息。SINS完全依靠自身的惯性敏感元件，不依赖任何外界信息测量导航参数，因此，具有隐蔽性好、不受气候条件限制、无信号丢失以及不受干扰等优点，是一种完全自主式、全天候的导航系统。但是，SINS也有其自身的不足。由于捷联解算中的积分原理，惯性器件的误差会导致导航误差随时间积累，因而纯惯性导航系统难以满足远程、长时间运动载体的高精度导航要求。全球导航卫星系统(GNSS)是在无线电技术的基础上，伴随航天技术发展而形成的一种天基无线电导航系统。它的优点是定位精度高，导航误差不随时间积累，可全天时、全天候工作。但是，GNSS难以直接提供姿态信息，并存在数据更新率低、易受电磁干扰等缺点。若将SINS与GNSS组合起来，能够实现二者的优势互补，显著提高导航系统的综合性能。目前，SINS/GNSS组合导航系统已广泛应用于航空、航海等领域，是一种较为理想的组合导航系统。
[0003] SINS/GNSS组合导航系统通常采用滤波技术对SINS和GNSS的数据进行融合，获得优于单一子系统的位置、速度和姿态精度。实现SINS/GNSS的高精度组合滤波面临的两大问题，一是SINS/GNSS组合系统为非线性系统，二是系统噪声和量测噪声为非高斯噪声。因此，滤波器的设计以及滤波方法的选择对SINS/GNSS组合导航系统的精度起着至关重要的作用。
[0004] 在非线性滤波方面，传统非线性滤波方法扩展卡尔曼滤波(Extended KalmanFiltering，EKF)存在模型线性化截断误差问题，且假设噪声为高斯白噪声；迭代扩展卡尔曼滤波(Iterated Extended Kalman Filtering，IEKF)和Unscented卡尔曼滤波(Unscented Kalman Filtering，UKF)是EKF的改进方法，其中UKF无需对非线性模型进行线性化，IEKF通过多次迭代运算能够减小模型线性化截断误差从而提高了估计精度，但这两种方法同样假设噪声为高斯白噪声，且滤波算法耗时较长，难以在实时导航中应用。二阶插值非线性滤波(Second-order Divided Difference Filtering，DD2)采用多维Stirling插值方法来替代EKF非线性函数导数的计算，并获得优于EKF的估计精度，但仍无法克服插值近似引起的模型误差对滤波估计精度的不利影响。因此提高SINS/GNSS组合导航系统的滤波精度和实时性有着十分重要的意义。
发明内容
[0005] 本发明的技术解决问题是：克服现有技术的不足，提供一种可以达到较高适用精度要求的基于改进二阶插值滤波与预测滤波相结合的捷联惯性/卫星组合导航滤波方法。
[0006] 本发明的技术解决方案为：一种捷联惯性/卫星组合导航滤波方法，具体步骤如下：
[0007] (1)建立包含模型误差的捷联惯性导航系统/全球导航卫星系统组合导航滤波非线性数学模型；
[0008] (2)对传统二阶插值滤波进行改进，并基于步骤(1)建立的数学模型对状态进行估计，然后利用预测滤波估计出的模型误差修正改进二阶插值滤波的状态估计，得到修正后的姿态误差、位置误差和速度误差；
[0009] (3)利用以上步骤(2)得到的修正后的姿态误差、位置误差和速度误差对捷联惯性导航系统捷联解算出的姿态、位置和速度进行补偿，获得更加准确的导航信息，即补偿后的姿态、位置和速度信息；
[0010] (4)将以上步骤(3)中补偿后的位置、速度和姿态作为下一导航时刻的初始值，不断重复以上步骤(2)和步骤(3)，直至捷联惯性导航系统/全球导航卫星系统组合导航结束。
[0011] 本发明与现有技术相比的优点在于：本发明克服了现有SINS/GNSS组合滤波估计方法的不足，对传统二阶插值滤波进行改进，利用精度优于 的平滑值 替换 计算获得更高精度的状态估计 然后将基于最小模型误差准则的预测滤波思想引入到改进二阶插值滤波估计中，建立包含模型误差的SINS/GNSS组合导航滤波的非线性数学模型，利用预测滤波在线估计出未知模型误差并以此修正改进二阶插值滤波的状态变量，提高状态估计的精度，进而提高SINS捷联解算的位置、速度和姿态精度。该方法克服了传统二阶插值滤波插值近似引起的模型误差对滤波估计精度的不利影响，同时可以在线实时进行，从而能够实现实时、准确的SINS/GNSS组合导航，为载体提供更高精度的位置、速度和姿态信息。
附图说明
[0012] 图1为本发明采用改进二阶插值滤波与预测滤波的SINS/GNSS组合导航流程图；
[0013] 图2为导航坐标系与载体坐标系之间的关系示意图， θ、γ分别为SINS的航向角、俯仰角和横滚角。图中Oxnynzn为导航坐标系，取为东北天地理坐标系，Oxbybzb为载体坐标系。其中，图2a表示从导航坐标系Oxnynzn绕zn轴逆时针旋转 至Oxn1yn1zn；图2b表示Oxn1yn1zn绕xn1轴逆时针旋转θ至Oxn1yn2zn1，即Oxn1ybzn1；图2c表示Oxn1yn2zn1绕yb(yn2)轴逆时针旋转γ至Oxn2yn2zn2，即Oxbybzb；通过以上三次旋转，可实现导航坐标系Oxnynzn到载体坐标系Oxbybzb的转换；
[0014] 图3为导航坐标系Oxnynzn与计算导航坐标系Oxn′yn′zn′之间的关系示意图。
具体实施方式
[0015] 如图1所示，本发明的具体实施方法如下：
[0016] 1、建立包含模型误差的SINS/GNSS组合导航滤波非线性数学模型
[0017] 针对SINS/GNSS组合导航系统具有非线性的特点，建立SINS的非线性误差方程。在此基础上，建立包含模型误差的SINS/GNSS组合导航滤波非线性数学模型。
[0018] ①SINS非线性误差方程
[0019] 采用东北天地理坐标系作为导航坐标系。导航坐标系Oxnynzn和载体坐标系Oxbybzb的定义如图2a所示。SINS的非线性误差方程如下：
[0020] a)姿态误差方程
[0021] SINS的姿态误差角指SINS数学平台坐标系与导航坐标系之间的失准角。在SINS的姿态计算过程中，四元数法因计算简单且精度高而被广泛采用。下面给出采用加性四元数误差来描述失准角的SINS姿态误差方程。
[0022]
[0023] 式中 为姿态误差；和 分别为载体坐标系到导航坐标系的真实四元数和计算四元数；
为陀螺的理论输出，ωx、ωy、ωz分别代表载体坐标系中x、y、z轴上陀
螺的理论输出， 表示陀螺的测量误差； 为真实导航坐标系相对惯
性坐标系的旋转角速度在导航坐标系的投影，ωE、ωN、ωU分别为 在真实导航坐标系中东向、北向、天向轴上的投影， 为 的计算误差；M是由ωx、ωy、ωz、ωE、ωN、ωU组成的矩阵， 和 是由 组成的矩阵，M， 和 的具体定义如下
[0024]
[0025]
[0026] b)速度误差方程
[0027] (2)
[0028]
[0029] 式中Vn＝[VE VN VU]T为SINS解算出的载体相对地球的速度在导航坐标系中的投T影，VE、Vn、VU分别代表东向速度、北向速度、天向速度；δV＝[δVE δVn δVU] 为速度误差，δVE、δVN、δVU分别代表东向速度误差、北向速度误差、天向速度误差； 为载体坐标系到导航坐标系的方向余弦矩阵，也称为姿态矩阵； 为 的计算值， 为 的计
b
算误差； 为加速度计的测量值，f 为载体真实加速度， 为加
速度计的测量误差， 分别载体坐标系x、y、z轴上加速度计的测量误差； 为地球坐标系相对惯性坐标系的旋转角速度在导航坐标系的投影； 为 的计算误差；
为导航坐标系相对地球坐标系的旋转角速度在导航坐标系的投影； 为 的计算误差；
是δQ的非线性函数，具体表达式为
[0030]
[0031]
[0032] c)位置误差方程
[0033] 位置误差方程的矩阵形式为
[0034]
[0035] 式中δL、δλ、δh为位置误差，分别代表纬度误差、经度误差、高度误差；RM和RN分别为沿子午圈和卯酉圈的主曲率半径；L、λ、h分别为纬度、经度和高度，有
2 2
RM＝Re(1-2e+3e sinL)，RN＝Re(1+e sinL)，其中
椭圆度e＝1/298.257，地球椭球长半轴Re＝6378137m。
[0036] ②SINS/GNSS组合导航滤波非线性数学模型
[0037] SINS/GNSS组合导航滤波的非线性数学模型包括状态方程和量测方程。
[0038] a)状态方程
[0039] 综合以上SINS的姿态误差方程、速度误差方程和位置误差方程，并将SINS的陀螺漂移进行建模扩充为状态变量，考虑模型误差，可以得到如下系统状态方程：
[0040]
[0041] 式中x＝[δL δλ δh δVE δVN δVU δq0 δq1 δq2 δq3 εx εy εz]T为状态变量，εx、εy、εz分别为载体坐标系中x、y、z轴上陀螺常值漂移；d为模型误差；Gd为模型误差分布矩阵；Gw为系统噪声输入矩阵；w为系统噪声，Q为方差阵。
[0042] 将式(4)表示成矩阵形式：
[0043]
[0044] 其中
[0045]
[0046] 系数矩阵FN可写成如下形式：
[0047]
[0048] 其中
[0049]
[0050]
[0051]
[0052]
[0053]
[0054]
[0055]
[0056] 其中，ωie为地球自转角速度。
[0057] q(x，t)为非线性部分，其表达式为
[0058]
[0059] 将 简记为N′，即
[0060]
[0061] 式(4)中，Gd、Gw和d的具体表达式分别如下
[0062]
[0063] 其中，dx、dy、dz代表模型误差分量，包含载体坐标系中x、y、z轴上的加速度计误差等未知模型误差量。
[0064] b)量测方程
[0065] 取SINS捷联解算出的位置和速度与GNSS的速度和位置之差作为量测值，SINS/GNSS组合导航滤波量测方程为
[0066] y＝H(x)+v (5)
[0067] 式中y＝[δL′δλ′δh′δV′E δV′N δV′U]T，δL′，δλ′和δh′分别表示SINS与GNSS输出的纬度、经度和高度之差；δV′E，δV′N和δV′U分别表示SINS与GNSS输出的东向、北向和天向速度之差；量测噪声vδL′、vδλ′、vδH′、 分别代表GNSS的纬度、经度、高度、东向速度、北向速度、天向速度的量测噪声；量测噪声方差阵R根据GNSS的位置、速度噪声水平选取；H(x)的具体T
表达式为H(x)＝[(RM+h)δL(RN+h)cosL·δλ δh δVE δVN δVU]。
[0068] 2、对传统二阶插值滤波进行改进，并基于1中建立的数学模型对状态进行估计[0069] a)计算出tk-1时刻协方差阵 和系统噪声方差阵Qk-1均方根的各列向量[0070]
[0071]
[0072] 式中，下标k-1表示tk-1时刻； 表示矩阵 的平方根矩阵中的第i列；表示矩阵Qk-1的平方根矩阵中的第i列。
[0073] b)计算系统状态一步预测 和状态一步预测协方差阵
[0074]
[0075]
[0076]
[0077]
[0078]
[0079]
[0080] 式中，下标k/k-1表示从tk-1到tk时刻的预测； 为tk-1时刻的状态估计，wk-1为系统噪声w在tk-1时刻的值。
[0081] c)计算出tk时刻状态协方差阵 和量测噪声方差阵Rk均方根的各列向量[0082]
[0083]
[0084] 式中，下标k表示tk时刻； 表示矩阵 的平方根矩阵中的第i列； 表示矩阵Rk的平方根矩阵中的第i列。
[0085] d)量测修正更新，即计算量测一步预测 量测协方差阵 协方差阵 滤波增益Kk、状态估计 和状态协方差阵
[0086]
[0087]
[0088]
[0089]
[0090]
[0091]
[0092]
[0093]
[0094]
[0095]
[0096] 式中yk和vk分别为量测变量y和量测噪声v在tk时刻的值。
[0097] e)状态修正更新。当tk时刻的量测值yk未到来时， 和 分别是状态xk-1和xk的最优估值。但是，当yk到来时， 要优于 若以平滑值 取代式(8)中的将使滤波估计的精度更高。因此，本发明为进一步提高状态估计 的精度，首先计算然后用 替换 重新计算 和
[0098]
[0099](19)
[0100]
[0101]
[0102] 综合以上a)至e)步骤，即为改进后的二阶插值滤波算法。
[0103] 3、利用预测滤波估计模型误差d
[0104] a)由tk-1时刻系统的状态估值 计算系统的输出估值
[0105] 令采样时间间隔为Δt(可根据SINS/GNSS组合导航的滤波周期来定)，根据量测方程(5)式，量测输出估值 的计算公式为
[0106]
[0107] b)估计出tk-1时刻的模型误差
[0108] 模型误差 的计算公式为
[0109]
[0110]
[0111] 式中参数 为6维列向量，其各分量为
[0112]
[0113] 式中Hi为H(x)的第i个分量；pi为模型误差向量d的任何分量第一次出现在hi的微分中的最低阶数； 的计算公式为
[0114]
[0115]
[0116] 求得
[0117]
[0118]
[0119]
[0120]
[0121]
[0122]
[0123]
[0124]
[0125] Z4＝Δt([0 0 0 1 0 0 0 0 0 0 0 0 0]FN·x+N′1)
[0126] Z5＝Δt([0 0 0 0 1 0 0 0 0 0 0 0 0]FN·x+N′2)
[0127] Z6＝Δt([0 0 0 0 0 1 0 0 0 0 0 0 0]FN·x+N′3)
[0128] Λ(Δt)∈R6×6为对角阵，其对角元素计算公式为
[0129]
[0130] 求得
[0131] 为灵敏度矩阵，各行元素可表示为
[0132]
[0133] 式中，gj为Gd的第j列，标量函数 关于向量场 的一阶李导数记为 其表达式为
[0134]
[0135] 求得
[0136] U11＝C11，U12＝C12，U13＝C13，U21＝C21，U22＝C22，U23＝C23，[0137] U31＝C31，U32＝C32，U33＝C33，U41＝C21/RM，U42＝C22/RM，U43＝C23/RM，[0138] U51＝C11/RN/cosL，U52＝C12/RN/cosL，U53＝C13/RN/cosL，
[0139] U61＝C31，U62＝C32，U66＝C33。
[0140] 其中Cij(i，j＝1，2，3)为 的第i行第j列元素，即
[0141] 4、利用预测滤波估计出的模型误差 修正改进后的二阶插值滤波算法的状态估计，得到修正后的姿态误差、位置误差和速度误差
[0142] a)在改进二阶插值滤波 的计算式(18)中增加修正项 以修正模型误差引起的状态估计误差
[0143]
[0144] 式中， 为模型误差分布矩阵Gd在tk-1时刻的值。
[0145] b)将步骤a)中计算出的 代入式(19)中计算 然后利用式(20)计算最终计算得到的 即为最终修正后的状态变量，包括姿态误差(δq0、δq1、δq2、δq3)、位置误差(δL、δλ、δh)、速度误差(δVE、δVN、δVU)等状态变量。
[0146] 5、利用修正后的姿态误差、位置误差和速度误差修正SINS捷联解算出的姿态、位置和速度。将修正后的姿态、位置和速度作为下一导航时刻的初始值。
[0147] ①姿态修正
[0148] 利用修正后的姿态误差(δq0、δq1、δq2、δq3)计算出计算导航坐标系到真实导航坐标系间的转移矩阵 然后对SINS计算出的姿态矩阵 (即上述的 )进行校正，得到 最后根据 计算出更为准确的姿态——航向角 俯仰角θ和横滚角γ。
[0149] a)计算真实导航坐标系与计算导航坐标系间的转移矩阵
[0150]
[0151] 式中φE、φN和φU为导航坐标系东向、北向和天向轴上的数学平台失准角。
[0152] 由φE、φN和φU可得 的表达式为：
[0153]
[0154] b)计算姿态矩阵
[0155] 利用 对SINS解算得到的 进行如下校正：
[0156]
[0157] 式中 即为修正后的姿态矩阵。
[0158] c)计算航向角 俯仰角θ和横滚角γ航向角 俯仰角θ和横滚角γ的定义如图2a、图2b和图2c所示。
[0159] 将(31)式计算出的 记为
[0160]
[0161] 式中，Tij(i＝1，2，3，j＝1，2，3)表示 的第i行第j列元素。
[0162] 又因为
[0163]
[0164] 因此，由(32)式和(33)式，可以确定航向角 俯仰角θ和横滚角γ的主值，即[0165]
[0166]
[0167] θ主＝arcsin(T32)
[0168] 若航向角 俯仰角θ和横滚角γ的取值范围分别定义为[0，2π]、[-π，+π]。那么， θ和γ的真值可由下式确定：
[0169]
[0170] θ＝θ主 (35)
[0171]
[0172] 由(35)式确定的 θ和γ即为经过修正后的航向角、俯仰角和横滚角。
[0173] ②速度修正
[0174] 由SINS捷联解算出的东向速度VE、北向速度VN和天向速度VU分别减去速度误差δVE、δVN和δVU，得到修正后的速度：
[0175]
[0176] 式中V′E、V′N和V′U为修正后的东向速度、北向速度和天向速度。
[0177] ③位置修正
[0178] 由SINS捷联解算出的纬度L、经度λ和高度h分别减去位置误差δL、δλ和δh，得到修正后的位置：
[0179]
[0180] 式中L′、λ′和h′为修正后的纬度、经度和高度。
[0181] 本发明未详细阐述部分属于本领域公知技术。
