CN103712621B - 偏振光及红外传感器辅助惯导***定姿方法 - Google Patents

Abstract

本发明偏振光及红外传感器辅助惯导***定姿方法属于飞行器姿态测量与估计技术领域，涉及一种偏振光及红外传感器辅助惯导***定姿方法。该方法采用偏振光传感器、红外传感器测量飞行器的航向角、横滚角、俯仰角与惯性导航***中陀螺仪测量的三维姿态角进行卡尔曼滤波，将所得最优估计姿态角反馈至惯性导航***中，提高惯性导航***的测量精度，本发明将两种方式测量的捷联矩阵进行最优化融合，将融合后结果反馈至惯导***对其矫正，由惯导***向用户输出飞行器的位置、速度。本发明中所用传感器均属于自主导航器件，不易受外界干扰，较GPS、北斗等导航方式，具有一定的防欺骗性和隐蔽性。

Description

偏振光及红外传感器辅助惯导***定姿方法

技术领域

本发明属于飞行器姿态测量与估计技术领域，涉及一种偏振光及红外传感器辅助惯导***定姿方法。

背景技术

惯性导航***(InertialNavigationSystem,INS)简称：惯导***，它是一种全自主的导航***，可以连续、实时地提供位置、速度和姿态(横滚角、俯仰角和航向角)信息，其短时精度很高，且具有隐蔽性好，不受气候条件限制等优点，因而广泛应用于航空、航天、航海等领域。但是，惯导***的误差随时间增长，因此需要与其他导航***构成组合导航***。在最常用的INS/GPS组合导航***中，利用卡尔曼滤波技术可以有效地降低组合导航***的位置误差和速度误差，但是对姿态误差的影响很小，尤其很难抑制航向角误差的积累，于是针对提高惯性导航姿态测量精度的研究成为当今很多学者的研究重点。

传统技术中对于惯导***的姿态误差修正大多采用差分GPS、磁力计或者倾角传感器来实现，但是由于以上方法均存在不同程度的缺点，比如差分GPS需要安装多天线，受体积限制，不适合微小型飞行器，且无线信号容易受干扰，不具有隐蔽性；磁力计采用地磁原理工作，容易受周围磁场影响；倾角传感器依靠重力分量进行测量姿态，不适用于加速运动的飞行器。

发明内容

本发明的目的是克服现有技术的缺陷，发明一种偏振光及红外传感器辅助惯导定姿方法，偏振光传感器根据光学原理测量航向信息，红外传感器根据红外热感原理测量横滚、俯仰姿态信息，将两者组合可以精确测量飞行器的三维姿态角。由于其基于光学和红外热感应原理工作，不受电磁波干扰，具有一定的自主性和隐蔽性，且精度不随时间变化，在长航时运动中具有突出表现。本发明将偏振光、红外传感器与INS进行组合，不但可以抑制惯导***姿态角误差的发散，同时对位置和速度误差也起到了一定的修正作用。此方法可以有效地补偿惯导***中陀螺仪在积分过程中随时间积累产生的误差，从而得到高精度、全自主的位置和姿态信息。

本发明采用的技术方案是偏振光及红外传感器辅助惯导***定姿，其特征是，采用偏振光传感器、红外传感器测量飞行器的航向角、横滚角、俯仰角与惯性导航***中陀螺仪测量的三维姿态角进行卡尔曼滤波，将所得最优估计姿态角反馈至惯性导航***中，提高惯性导航***的测量精度。

该方法的具体步骤如下：

步骤一：采集红外传感器和偏振光传感器输出的数据，确定飞行器的横滚角φ_IR、俯仰角θ_IR、航向角ψ_P，建立组合***的初始捷联矩阵

$C_b^n=\begin{array}{c}\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P& -\cos {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P+\sin {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P& \sin {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P+\cos {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P\\ \cos {\mathrm{\θ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P& \cos {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P+\sin {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_{\mathrm{IR}}& -\sin {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P+\cos {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P---\left(1\right)\\ -\sin {\mathrm{\θ}}_{\mathrm{IR}}& \sin {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\θ}}_{\mathrm{IR}}& \cos {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\θ}}_{\mathrm{IR}}\end{array}\right]\end{array}$

其中，φ_INFθ_INF分别为红外传感器的横滚角、俯仰角，ψ_P为偏振光传感器的航向角。

步骤二：采集加速度计输出，将输出f^b通过捷联矩阵投影到导航系（n）得到fⁿ，如公式(2)，经过修正Coriolis影响，积分运算得到导航系下的速度Vⁿ，从而得到n系相对e系的角速度在n系下的投影

$f^n=C_b^n{f}^b---\left(2\right)$

步骤三：利用更新n系与e系的坐标变换矩阵并根据经纬度信息与坐标变换矩阵之间的关系得到实时经纬度信息；

步骤四：采集陀螺仪的输出除去在载体系(b)下的投影得到如公式（3），利用更新捷联矩阵得到惯导***输出的横滚角φ_g、俯仰角θ_g、航向角ψ_g；

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-{\mathrm{\ω}}_{\mathrm{in}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b{\mathrm{\ω}}_{\mathrm{in}}^n---\left(3\right)$

其中为导航系与惯性系之间的旋转角速度在导航系下的投影；为***捷联矩阵转置形式；为陀螺仪的输出。

捷联矩阵更新可通过解算微分方程式（4）实现：

${\stackrel{\·}{C}}_b^n=C_b^n{\·\mathrm{\Ω}}_{\mathrm{nb}}^b---\left(4\right)$

其中，为向量的反对称矩阵，即 ${\mathrm{\Ω}}_{\mathrm{nb}}^b=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_{\mathrm{nbZ}}^b& {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbZ}}^b& 0& -{\mathrm{\ω}}_{\mathrm{nbx}}^b\\ -{\mathrm{\ω}}_{\mathrm{nby}}^b& {\mathrm{\ω}}_{\mathrm{nbx}}^b& 0\end{array}\right]$

步骤五：采集红外传感器和偏振光传感器输出的数据，确定飞行器三维姿态角φ_IRθ_IRψ_P，与步骤四中陀螺仪得到的姿态角进行卡尔曼滤波得到最优估计三维姿态角

步骤六：将最优估计姿态角组成捷联矩阵反馈至惯性***中修正位置和姿态的运算公式，其修正过程分三个部分：

(1)修正步骤二中公式(2)的***捷联矩阵得到由加速度计的输出f^b投影到导航系的新向量fⁿ；

(2)修正步骤四中公式(3)***捷联矩阵的转置形式，得到在载体系(b)下的新投影

(3)修正步骤四中公式(4)的***捷联矩阵通过解算微分方程得到新的捷联矩阵。

步骤七：重复步骤二至步骤六过程，实现***实时输出飞行器的位置、速度、姿态信息。

本发明与现有技术相比的优点在于：(1)所用传感器均属于自主导航器件，不易受外界干扰，较GPS、北斗等导航方式，具有一定的防欺骗性和隐蔽性。(2)偏振光、红外传感器的测量误差不随时间累积，与短时精度高但误差发散的角速度陀螺构成优势互补，经过卡尔曼滤波进行数据融合后，组合***的精度可以保持长时间稳定。

附图说明

图1为本发明的数据融合方法的计算流程图

图2为本发明的原理框图

具体实施方式

下面结合技术方案和附图详细说明本发明的具体实施，本发明涉及的坐标系有：载体坐标系(b)；导航坐标系(n)；地球坐标系(e)；惯性坐标系(i)。飞行器与载体坐标系固连，载体坐标系到导航坐标系之间的转换用捷联矩阵表示。在本发明中，惯导***中的陀螺仪可以确定捷联矩阵，同时红外传感器和偏振光传感器也可以确定捷联矩阵，本发明将两种方式测量的捷联矩阵进行最优化融合，将融合后结果反馈至惯导***对其矫正，由惯导***向用户输出飞行器的位置、速度。

在附图1中表示了本发明的数据融合方法的计算流程图。

1.采集红外传感器和偏振光传感器输出的数据，确定飞行器的横滚角φ_IR、俯仰角φ_IR、航向角ψ_P，建立组合***的初始捷联矩阵

$C_b^n=\begin{array}{c}\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P& -\cos {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P+\sin {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P& \sin {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P+\cos {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P\\ \cos {\mathrm{\θ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P& \cos {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P+\sin {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_{\mathrm{IR}}& -\sin {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P+\cos {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P\\ -\sin {\mathrm{\θ}}_{\mathrm{IR}}& \sin {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\θ}}_{\mathrm{IR}}& \cos {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\θ}}_{\mathrm{IR}}\end{array}\right]\end{array}$

$\left(5\right)$

其中，φ_INFθ_INF分别为红外传感器的横滚角、俯仰角，ψ_P为偏振光传感器的航向角。

2.采集加速度计输出，将输出f^b通过捷联矩阵投影到导航系（n）得到fⁿ，如公式(6)，经过修正Coriolis影响，积分运算得到导航系下的速度Vⁿ，从而得到n系相对e系的角速度在n系下的投影如公式(7)。

$f^n=C_b^n{f}^b---\left(6\right)$

${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{ccc}\frac{V_E}{r_p+h}& \frac{-V_N}{r_m+h}& \frac{V_E\tan \mathrm{Lat}}{r_p+h}\end{array}\right]---\left(7\right)$

其中V_E、V_N、Lat、h分别为导航系下东向速度、北向速度、纬度、高度；r_p、r_m为地球长、短半径。

3.利用更新n系与e系的坐标变换矩阵并根据经纬度信息与坐标变换矩阵之间的关系得到实时经纬度信息，矩阵更新可通过解算微分方程（8）实现：

${\stackrel{\·}{C}}_b^n=C_b^n\·{\mathrm{\Ω}}_{\mathrm{nb}}^b---\left(8\right)$

其中，为向量的反对称矩阵，即 ${\mathrm{\Ω}}_{\mathrm{nb}}^b=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_{\mathrm{nbZ}}^b& {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbZ}}^b& 0& -{\mathrm{\ω}}_{\mathrm{nbx}}^b\\ -{\mathrm{\ω}}_{\mathrm{nby}}^b& {\mathrm{\ω}}_{\mathrm{nbx}}^b& 0\end{array}\right]$

$C_e^n=\left[\begin{array}{ccc}\sin \mathrm{Lat}\·\cos \mathrm{long}& \sin \mathrm{Latlong}& \cos \mathrm{Lat}\\ -\sin \mathrm{long}& \cos \mathrm{long}& 0\\ -\cos \mathrm{Lat}\cos \mathrm{long}& -\cos \mathrm{Lat}\·\sin \log & \sin \mathrm{Lat}\end{array}\right]---\left(9\right)$

纬度： $\mathrm{Lat}=\arcsin C_e^n\left(\mathrm{3,3}\right)---\left(10\right)$

经度： $\mathrm{long}=\arctan \frac{C_e^n\left(\mathrm{3,2}\right)}{C_e^n\left(\mathrm{3,1}\right)}---\left(11\right)$

4.采集陀螺仪的输出除去在载体系(b)下的投影得到利用更新捷联矩阵更新过程与步骤3矩阵更新过程相似，参照公式（5）中三维姿态角与捷联矩阵的关系，得到惯导***输出的横滚角φ_g、俯仰角θ_g、航向角ψ_g。

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-{\mathrm{\ω}}_{\mathrm{in}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b{\mathrm{\ω}}_{\mathrm{in}}^n---\left(12\right)$

其中为导航系与惯性系之间的旋转角速度在导航系下的投影；为***捷联矩阵转置形式；为陀螺仪的输出。

捷联矩阵更新可通过解算微分方程（13）实现：

${\stackrel{\·}{C}}_b^n=C_b^n\·{\mathrm{\Ω}}_{\mathrm{nb}}^b---\left(13\right)$

其中，为向量的反对称矩阵，即 ${\mathrm{\Ω}}_{\mathrm{nb}}^b=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_{\mathrm{nbZ}}^b& {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbZ}}^b& 0& -{\mathrm{\ω}}_{\mathrm{nbx}}^b\\ -{\mathrm{\ω}}_{\mathrm{nby}}^b& {\mathrm{\ω}}_{\mathrm{nbx}}^b& 0\end{array}\right]$

5.采集红外传感器和偏振光传感器输出的数据，确定飞行器三维姿态角φ_IRθ_IRψ_P，与步骤4中陀螺仪得到的姿态角进行卡尔曼滤波得到最优估计三维姿态角

具体卡尔曼滤波计算过程如下：

①红外传感器、偏振光传感器与惯导组合的***方程包括状态方程和量测方程，分别为：

***状态方程

$\stackrel{\·}{X}=\mathrm{AX}+\mathrm{BU}+W---\left(14\right)$

其中X＝[φθψε_φε_θε_ψ]^T为***状态矢量； $U={\left[\begin{array}{cccccc}{\stackrel{\·}{\mathrm{\φ}}}_g& {\stackrel{\·}{\mathrm{\θ}}}_g& {\stackrel{\·}{\mathrm{\Ψ}}}_g& 0& 0& 0\end{array}\right]}^T$ 为惯导***测得三轴角速度的扩展向量；W＝[ω_φω_θω_ψ000]^T为***噪声矢量；A、B为***转移矩阵：

$A=\left[\begin{array}{cccccc}0& 0& 0& -1& 0& 0\\ 0& 0& 0& 0& -1& 0\\ 0& 0& 0& 0& 0& -1\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right],B=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right];$

***的量测方程：

Z＝HX+V(15)

其中Z＝[φ_INFθ_INFψ_P000]^T为观测矢量，φ_INFθ_INF分别为红外传感器的横滚角、俯仰角，ψ_P为偏振光传感器的航向角；V＝[υ_φυ_θυ_ψ000]^T为量测噪声矢量；H为观测矩阵。

$\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]$

设采样时间为ΔT，***方程的离散形式：

X_K=ΦX_K-1+CU_K+W_K-1(16)

其中X_k-1为k-1时刻状态矢量；X_k为k时刻状态矢量；Φ、C分别为离散化后的转移矩阵：

$\mathrm{\Φ}=\left[\begin{array}{cccccc}1& 0& 0& -\mathrm{\ΔT}& 0& 0\\ 0& 1& 0& 0& -\mathrm{\ΔT}& 0\\ 0& 0& 1& 0& 0& -\mathrm{\ΔT}\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right],C=\left[\begin{array}{cccccc}\mathrm{\ΔT}& 0& 0& 0& 0& 0\\ 0& \mathrm{\ΔT}& 0& 0& 0& 0\\ 0& 0& \mathrm{\ΔT}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right];$

量测方程的离散形式：

Z_K＝HX_K+V_K(17)

其中， $H_K=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& \mathrm{\ΔT}& 0& 0\\ 0& 0& 0& 0& \mathrm{\ΔT}& 0\\ 0& 0& 0& 0& 0& \mathrm{\ΔT}\end{array}\right]$

②卡尔曼滤波基本算法编排，该算法流程如下：

状态一步预测方程：

${\hat{X}}_{k/k-1}{\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+C_{k,k-1}{U}_k---\left(18\right)$

状态估值计算方程：

${\hat{X}}_k={\hat{X}}_{K/K-1}+{\hat{K}}_k(Z_k-H_K{\hat{X}}_{k/k-1})---\left(19\right)$

滤波增益矩阵方程：

${\hat{K}}_k={\hat{P}}_{k/k-1}{H}_k^T{(H_k{\hat{P}}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(20\right)$

一步预测均方误差方程：

${\hat{P}}_{k\backslash k-1}={\mathrm{\Φ}}_{k\backslash k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(21\right)$

估计均方误差方程

${\hat{P}}_k=(I-{\hat{K}}_k{H}_k){\hat{P}}_{k\backslash k-1}{(I-{\hat{K}}_k{H}_k)}^T+{\hat{K}}_k{R}_k{\hat{K}}_k^T---\left(22\right)$

6.将最优估计姿态角组成捷联矩阵反馈至惯性***中修正位置和姿态的运算公式，其修正过程分三个部分：

(1)修正步骤2中公式(6)的***捷联矩阵得到由加速度计的输出f^b投影到导航系的新向量fⁿ；

(2)修正步骤4中公式(12)***捷联矩阵的转置形式，得到在载体系(b)下的新投影

(3)修正步骤4中公式(13)的***捷联矩阵通过解算微分方程得到新的捷联矩阵。

7.重复步骤2至步骤6过程，实现***实时输出飞行器的位置、速度、姿态信息。

本发明将两种方式测量的捷联矩阵进行最优化融合，把融合后结果反馈至惯导***对其矫正，由惯导***向用户输出飞行器的位置、速度。本发明中所用传感器均属于自主导航器件，不易受外界干扰，较GPS、北斗等导航方式，具有一定的防欺骗性和隐蔽性。

Claims (1)

1.一种偏振光及红外传感器辅助惯导***定姿方法，其特征在于，该方法采用偏振光传感器、红外传感器测量飞行器的航向角、横滚角、俯仰角与惯性导航***中陀螺仪测量的三维姿态角进行卡尔曼滤波，将所得最优估计姿态角反馈至惯性导航***中，提高惯性导航***的测量精度，方法具体步骤如下：

步骤一：采集红外传感器和偏振光传感器输出的数据，确定飞行器的横滚角φ_IR、俯仰角θ_IR、航向角ψ_P，建立组合***的初始捷联矩阵

$C_b^n=\left[\begin{array}{ccc}{\mathrm{cos\θ}}_{IR}{\mathrm{cos\ψ}}_P& -{\mathrm{cos\φ}}_{IR}{\mathrm{sin\ψ}}_P+{\mathrm{sin\φ}}_{IR}{\mathrm{sin\θ}}_{IR}{\mathrm{cos\ψ}}_P& {\mathrm{sin\φ}}_{IR}{\mathrm{sin\ψ}}_P+{\mathrm{cos\φ}}_{IR}{\mathrm{sin\θ}}_{IR}{\mathrm{cos\ψ}}_P\\ {\mathrm{cos\θ}}_{IR}{\mathrm{sin\ψ}}_P& {\mathrm{cos\φ}}_{IR}{\mathrm{cos\ψ}}_P+{\mathrm{sin\φ}}_{IR}{\mathrm{sin\θ}}_{IR}{\mathrm{sin\ψ}}_P& -{\mathrm{sin\φ}}_{IR}{\mathrm{cos\ψ}}_P+{\mathrm{cos\φ}}_{IR}{\mathrm{sin\θ}}_{IR}{\mathrm{sin\ψ}}_P\\ -{\mathrm{sin\θ}}_{IR}& {\mathrm{sin\φ}}_{IR}{\mathrm{cos\θ}}_{IR}& {\mathrm{cos\φ}}_{IR}{\mathrm{cos\θ}}_{IR}\end{array}\right]---\left(1\right)$

其中，φ_INFθ_INF分别为红外传感器的横滚角、俯仰角，ψ_P为偏振光传感器的航向角；

步骤二：采集加速度计输出，将输出f^b通过捷联矩阵投影到导航系(n)得到fⁿ，如公式(2)，经过修正Coriolis影响，积分运算得到导航系下的速度Vⁿ，从而得到n系相对e系的角速度在n系下的投影

$f^n=C_b^n{f}^b---\left(2\right)$

步骤三：利用更新n系与e系的坐标变换矩阵并根据经纬度信息与坐标变换矩阵之间的关系得到实时经纬度信息，矩阵更新通过解算微分方程(8)实现：

步骤四：采集陀螺仪的输出除去在载体系(b)下的投影得到如公式(3)，利用更新捷联矩阵得到惯导***输出的横滚角φ_g、俯仰角θ_g、航向角ψ_g；

${\mathrm{\ω}}_{nb}^b={\mathrm{\ω}}_{ib}^b-{\mathrm{\ω}}_{in}^b={\mathrm{\ω}}_{ib}^b-C_n^b{\mathrm{\ω}}_{in}^n---\left(3\right)$

其中为导航系与惯性系之间的旋转角速度在导航系下的投影；为***捷联矩阵转置形式；为陀螺仪的输出；

捷联矩阵更新可通过解算微分方程式(4)实现：

步骤五：采集红外传感器和偏振光传感器输出的数据，确定飞行器三维姿态角φ_IRθ_IRψ_P，与步骤四中陀螺仪得到的姿态角进行卡尔曼滤波得到最优估计三维姿态角

步骤六：将最优估计姿态角组成捷联矩阵反馈至惯性***中修正位置和姿态的运算公式，其修正过程分三个部分：

(1)修正步骤二中公式(2)的***捷联矩阵得到由加速度计的输出f^b投影到导航系的新向量fⁿ；

(2)修正步骤四中公式(3)***捷联矩阵的转置形式，得到在载体系(b)下的新投影

(3)修正步骤四中公式(4)的***捷联矩阵通过解算微分方程得到新的捷联矩阵；

步骤七：重复步骤二至步骤六过程，实现***实时输出飞行器的位置、速度、姿态信息。