CN109931955A - 基于状态相关李群滤波的捷联惯性导航系统初始对准方法 - Google Patents

Abstract

本发明公开了基于状态相关李群滤波的捷联惯性导航初始对准方法，使用李群和李代数描述载体的姿态，建立模型，将初始对准问题转化成姿态估计问题进行对准。SVD分解法是基于最小二乘原理的数学解算方法，无法考虑到观测矢量中的噪声项，解算过程包含了大量噪声信息，在实际环境当中干扰较大，惯性单元噪声很难统计造成数值解算精度大幅下降。由于模型线性化时带来的误差导致估计精度下降。四元数滤波算法的精度优于数值解算的方法，然而构建四元数量测方程时需要也需要对模型进行线性变换对滤波结果的精度和收敛速度都有影响。本发明提出的方法基于李群直接对量测模型进行建模不需要线性化，并且考虑到状态相关不确定噪声对系统的影响。

Description

基于状态相关李群滤波的捷联惯性导航系统初始对准方法

技术领域

本发明公开了一种基于状态相关李群滤波的捷联惯性导航系统初始对准方法，该方法属于导航方法及应用技术领域。

背景技术

所谓导航，就是正确地引导载体沿着预定的航线、以要求的精度、在指定的时间内将载体引导至目的地的过程。惯性导航系统根据自身传感器的输出，以牛顿第二定律为理论基础，对载体的各项导航参数进行解算。它是一种自主式的导航系统，在工作时不依靠外界信息，也不向外界辐射任何能量，隐蔽性好、抗扰性强，能够全天时、全天候为载体提供完备的运动信息。

早期的惯导系统以平台惯导为主，随着惯性器件的成熟和计算机技术的发展，上世纪60年代开始出现了惯性器件与载体直接固联的捷联惯导系统。与平台惯导相比，捷联惯导系统省去了复杂的实体稳定平台，具有成本低、体积小、重量轻、可靠性高等优点。近年来，捷联惯导系统日趋成熟，精度逐步提高，应用范围也逐渐扩大。捷联式惯性导航技术将陀螺仪和加速度计直接安装在载体上，得到载体系下的加速度和角速度，通过导航计算机将测得的数据转换至导航坐标系完成导航，它不需要实体的稳定平台，成本低、体积小、重量轻、可靠性高。

捷联式惯性导航系统进入导航任务前要先进行初始对准，目的是建立精确的初始姿态矩阵，从而得到载体相对空间的姿态。对准时间和对准精度是初始对准的两个重要指标。对准时间反映系统的快速反应能力，对准精度反映系统的导航性能。作为惯导系统的一个关键技术，初始对准是国内外学者多年来的研究热点。惯导系统的初始对准分为粗对准和精对准两个阶段。

比较传统的初始对准方法，仅适用于静止或微幅晃动的对准环境。对于工作在复杂环境中的载体，诸如发动机处于高频振动的汽车、格斗状态下的战斗机、浪涌下的舰船等，载体的角振动和线振动会导致初始对准的精度和稳定性下降。捷联惯导系统在自对准过程中的姿态时刻都在发生变化，初始对准的时间和精度会受到影响。因此，在运载体晃动干扰环境下完成初始对准过程，就必须屏蔽这些无法消除的扰动影响。对准领域的研究重点是动态情况下捷联式惯性导航系统的初始对准，致力于研究出抗扰能力强、对准精度高、适应于各种复杂恶劣环境的对准方法。本发明将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，并考虑可有效地解决晃动条件下初始对准的问题。

四元数的表示方法弥补了欧拉角的的不足，计算过程中不存在奇异点的问题，但是四元数存在约束条件，在采用四元数描述姿态运动时，在滤波过程中由于难以满足其约束条件而出现问题。四元数卡尔曼初始对准方法为了避免非线性模型，构造了伪量测方程进行滤波估计。构造过程中的求逆运算会导致滤波系统的不稳定。李群表示方法弥补了四元数存在的问题。基于SO(3)群的概率分布函数推导，由于SO(3)群是紧的，李群表示方法有效的避免了姿态解算中的奇异之问题，可以对姿态进行全局表示。基于李群描述的滤波运算，也大大减少了矩阵和向量转化中的计算误差，提升了计算速度和计算精度。但是现有的基于李群滤波的初始对准方法没有考虑到状态相关噪声对滤波过程的影响，导致计算结果中存在误差，影响对准精度。

针对上述问题，本发明对李群滤波中的状态相关噪声项进行计算与归纳，设计新的状态相关李群滤波代替原有的李群滤波完成初始对准任务。根据状态相关噪声的形式，设计状态噪声协方差矩阵，使其可以随着滤波过程进行迭代更新，逐渐向真实值逼近。状态相关李群滤波器相比于现有的李群滤波器，拥有更高的计算精度，能更好的完成晃动条件下的初始对准任务。

发明内容

基于状态相关李群滤波的初试对准方法是应用状态相关李群滤波进行惯性导航的初始对准过程。使用李群和李代数对载体姿态进行描述，将初始对准转化成为姿态估计的问题，构建滤波器进行估计。将初始姿态矩阵分解为三个SO(3)群的连续相乘，通过惯性元件的积分计算和地理位置信息，建立新的观测模型。采用李群和李代数描述捷联解算，应用状态相关李群滤波器进行递归迭代，求出姿态矩阵。本发明能快速、精确的完成初始对准任务，去除了状态相关噪声对滤波估计过程中的影响，适应性强，在实际工程中具有良好的应用前景。

在本发明方法的详细描述中坐标系定义如下：地球坐标系e系，原点选取地球中心，X轴位于赤道平面内，从地心指向载体所在点经线，Z轴沿地球自转轴方向，随地球自转而转动，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；地心惯性坐标系i系，是在粗对准起始时刻将地球坐标系e系惯性凝固后形成的坐标系；导航坐标系n系，即导航基准的坐标系，导航相关运算都在该坐标系下进行，原点位于舰载机重心，X轴指向东向E，Y轴指向北向N，Z轴指向天向U；载体坐标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上。

在本发明中，根据现有的捷联惯性导航系统初始对准中的问题，提出了基于状态相关李群滤波的捷联惯性导航初试对准方法。

为实现方法流程，本发明采用的技术方案为基于状态相关李群滤波的捷联惯性导航初始对准方法，该方法通过下述流程实现，

(1)捷联惯导系统进行预热准备，启动系统，获得载体所在位置的经度λ、纬度L的基本信息，采集惯性测量单元IMU中陀螺的输出角速度信息和加速度计的输出信息f^b等；

(2)对采集到的陀螺和加速度计的数据进行处理，应用状态相关李群滤波方法估计姿态矩阵；

方法的整体计算流程如图1所示。

描述坐标系变换的旋转矩阵符合李群三维特殊正交群SO(3)的性质，构成了SO(3)群：

其中，R对应了旋转矩阵，表示3×3的向量空间，上标T表示矩阵的转置，I表示三维单位矩阵，det(*)表示矩阵的行列式。根据姿态矩阵的链式法则，从载体系到导航系的姿态矩阵可以分解为三个连续特殊正交阵的乘积形式：导航系从初始时刻到当前时刻的旋转矩阵；初始载体运动速度的惯性矩阵；载体系从当前时刻与初始时刻之间的旋转矩阵。

其中，t表示时间变量，n(t)表示t时刻的导航坐标系，n(0)表示t₀时刻的导航坐标系，b(t)表示t时刻的载体坐标系，b(0)表示t₀时刻的载体坐标系，和分别为导航坐标系和机体坐标系下从初始t₀时刻到t时刻的姿态转换矩阵。和由陀螺和加速度计的信息计算得到。那么，初始对准的任务由求解姿态矩阵的问题转化为求解初始姿态阵的问题。为初始t₀时刻的机体系与导航系之间的姿态转换矩阵，是一个常值矩阵。根据李群的微分方程，得到和的表达式。

式中 表示导航系和惯性系之间的相对角速率；表示地球自转速率；表示导航系相与地球坐标系之间的相对角速率。为陀螺测得的载体相对于惯性系的角速率。[·]×表示将括号内的矢量转换成反对称矩阵的过程。即

由旋转角度矢量到姿态矩阵的指数映射关系得更新矩阵如下

式中，t_M表示经过了M个时间间隔之后的时刻；表示对在采样间隔的积分，采用双子样算法Δθ₁和Δθ₂是两个相邻采样周期陀螺测量增量。

由于导航系相对于惯性系角度变化速率比较缓慢近似为

由定义可知

当前时刻的和由公式(8)和(9)迭代计算得到。

在根据李群姿态描述和惯性导航信息确定和后，只需要确定惯性系姿态转换矩阵便可根据公式(3)实时地求解出导航所需要的姿态矩阵。

惯性导航的速度微分方程为

式中，fⁿ表示加速度计测得的比力在导航坐标系下的投影，f^b表示比力在载体坐标系下的投影，gⁿ表示地球重力加速度在导航坐标系下的投影，vⁿ表示载体速度在导航坐标系下的投影。

将公式(2)代入公式(10)得

公式两边同乘以

移项得

对上式在[0，t]上对其进行积分整理得

所以基于李群描述的量测模型为

式子中

GPS、陀螺仪、加速度计的采样频率可知，所以β(t)根据离散化数值算法计算得到。采样频率足够高时，角速率和加速度近似为线性的形式

f^b＝a_ft+b_f (20)

对角速度积分可得角度的增量

从上式可得

同理可得

使用离散数值的方法，使用采样时间的陀螺和加速度的采样数据进行积分时的近似运算可得β(t)在采样时刻的离散化数值。

计算β(t)所用到的物理量和gⁿ可直接得到较为精确，更新频率不高变化几乎可忽略，vⁿ为GPs采集的相对可靠信息，所以β(t)较为精确。而α(t)完全靠IMU信息高频更新获得其中包含了大量的陀螺和加速度的噪声项。下面对α(t)误差项的组成进行分析

其中ε_g为陀螺仪随机误差，ε_a为加速度计随机误差，k为比例系数。在上式中

将公式(28)代入公式(29)得

式子中

由公式(29)可知α(t)所包含的噪声项是由陀螺仪噪声、加速度噪声、陀螺仪和加速度测量值等多种信息耦合而成。

包含耦合噪声项的李群描述下的系统离散化量测方程为

β_k＝R_k(α_k+κε_a+λε_g) (31)

其中β_k和α_k分别为β(t)和α(t)在t_k时刻的观测值。

基于李群描述，本方法直接将旋转矩阵作为状态值进行估计。由于需要估计的为两个惯性坐标系之间旋转矩阵，是一个常值矩阵，所以系统李群形式下的状态方程和量测方程表示为

R_k+1＝R_k (32)

β_k＝R_k(α_k+δα_k) (33)

为了表示简明，将量测矩阵重写为

β_k＝R_kα_k+V_k (34)

其中，

V_k＝R_kδα_k

认为δα_k为两个高斯白噪声之和，其协方差矩阵为P_v＝(σ₁+σ₂)I₃，高斯白噪声的均值为零，所以有E{δα_k}＝0。噪声和状态量在统计学上相互独立，传统方法中将V_k的均值近似为零，E{R_kδα_k}＝0，其中R_k为状态量的真实值。状态相关量测噪声协方差矩阵的表示形式如下

在实际的滤波过程中，R_k是一个未知量，传统的与状态相关量测噪声协方差矩阵通常被近似为

式中表示姿态矩阵的状态量的估计值。状态量估计值和真实值之间的误差导致了传统的量测噪声协方差矩阵不够精确。假设则V_k的协方差矩阵的真实表示形式为

该矩阵可以被拆分为两部分

上式简写为

公式右侧的第二项为由与状态相关的测量噪声所导致的状态量和噪声项耦合的矩阵表示形式。本发明提出一种状态相关量测噪声协方差矩阵的精确表达形式。

根据定义给出一般形式下李群状态相关噪声协方差矩阵的表示形式，将一般性的结论应用到本发明提出的系统模型上。R_k表示t_k时刻状态量的估计值，ξ_k为李代数形式下的状态误差。ξ_k和V_k的协方差矩阵分别为P_k和的具体表示形式为

式中表示克罗内克尔积，矩阵的具体形式为其中E_i＝-[e_i]×，i＝1，2，3向量e_i，i＝1，2，3是三维欧式空间中的标准基底向量。的精确表达形式的为下面李群形式下的相关噪声项的具体形式。

用A_l∈G^m×m表示一个李群，其李代数为a，李代数形式下的状态误差协方差矩阵为 表示一个零均值白噪声序列，协方差矩阵为假设{α_k}和{l_k}在统计学上相互独立。有定义如下

v_k＝G(α_k)l_k (41)其中G(·)：α→A_l为李代数到李群的映射。v_k的协方差矩阵定义如下

定义为Γ＝[G_k(e₁)G_k(e₂)…G_k(e_i)]，i＝1，2，...，m。

列向量e_i为下的标准基底向量，G_k(·)为满足G_k(e_i)x＝G_k(x)e_i的线性转换函数。

证明：李群表示形式下，状态误差和状态误差协方差矩阵的定义已经给出。公式(41)中的第一项为传统的状态相关噪声协方差矩阵，第二项为包含李群形式下状态误差协方差矩阵和噪声协方差矩阵的耦合项。

公式(42)融合了噪声协方差矩阵和状态协方差矩阵，将矩阵维数扩展至mm。

式中i＝1，2，...，m

根据李群的同构性原理，通过G_k(·)将状态误差量分解到各个基底上完成了状态误差协方差和噪声协方差的耦合并保证了的对称性。

由上节误差分析可知，假设测量噪声为两个均值为零的高斯白噪声，协方差矩阵分别为σ₁I₃和σ₂I₃，由李群性质可得

再由李群中矩阵运算的性质计算得

将公式(44)和(45)代入(39)可得状态相关噪声的计算公式为

使用上述优化的噪声协方差矩阵设计状态相关李群滤波器对公式(31)提出的系统模型进行估计，滤波过程如下：

根据以上滤波方法进行递推迭代，求出再根据公式(2)求出从而完成捷联惯导系统初始对准过程。

附图说明

图1基于状态相关李群滤波的动态初始对准过程框图。

图2状态相关李群滤波算法流程图。

图3导航系到机体系变换示意图。

图4初始对准仿真结果图。

图5实验装置示意图。

图6上位机采集页面示意图。

图7初始对准实验结果图。

具体实施方式

本发明是基于状态相关李群滤波的捷联惯性导航系统初试对准设计，下面结合本发明系统流程图对本发明的具体实施步骤进行详细的描述：

步骤1：捷联惯导系统进行预热准备，启动系统，获得载体所在位置的经度λ、纬度L的基本信息，采集惯性测量单元IMU中陀螺的输出角速度信息和加速度计的输出信息f^b等.

步骤2：通过利用公式(3)，(5)，(8)更新计算

步骤3：通过陀螺仪测量的数据利用公式(4)，(6)，(9)更新计算

步骤4：建立滤波的系统方程，即状态方程和观测方程。由于状态量是常数矩阵，以公式(31)作为滤波的状态方程。利用加速度计和外部辅助信息得到的速度矢量构建与状态量相关的观测方程，方程如公式(32)所示。

步骤5：使用状态相关李群滤波器进行滤波估计滤波过程如公式(47)所示。

步骤6：利用估计得到的计算计算过程如公式(2)所示。由解算出载体的姿态角作为对准结果进行输出。

本发明的有益效果如下：

(1)在以下仿真环境下，对该方法进行仿真实验。

载体处于动态情况下，其航向角ψ、俯仰角θ、横滚角γ作周期变化，姿态变化情况如下：

存在各个方向上振荡所导致的线速度：

其中，i＝x，y，z，A_Dx＝0.02m，A_Dy＝0.03m，A_Dz＝0.3m；ω_Di＝2π/T_Di，其中T_Dx＝7s，T_Dx＝6s，T_Dx＝8s；为[0，2π]上服从均匀分布的随机相位。加速度计的常值偏差1×10^-4g，随机测量噪声为陀螺仪常值漂移为0.01°/h，随机漂移为0.001°/h。初始经纬度为东经118°，北纬40°。

常数设置：

赤道半径：R_e＝6378165.0m；

椭球扁率：e＝3.352e-3；

由万有引力得地球表面重力加速度：g₀＝9.78049；

地球自转角速度(单位为rad/s)：7.2921158e-5；

常数：π＝3.1415926；

方法仿真结果如下：

进行了600s仿真，仿真结果如图4所示。以姿态误差角的估计误差作为衡量精对准的指标。航向角误差在200s能够达到10′，俯仰角和横滚角误差在200s降低到了0.3′左右。数值解算的SVD方法在收敛速度上较快，但是最终航向角的对准精度较差，约为30′，不能满足初始对准精度的要求。使用递推估计的滤波方法能够考虑到系统噪声对初始对准的影响精度相对较高。四元数滤波航向角的最终对准精度为24′，精度达到2°以下需要280s。李群滤波航向角最终对准精度为14′左右，精度达到2°以下需要200s，状态相关李群滤波在此基础上精度和收敛速度又有一定提高。通过仿真结果可知李群滤波的对准精度和收敛速度相对于传统的方法均有所提高。仿真结果表明本发明所提出的算法在三轴角度对准上都有比较好的收敛速度和对准精度。

(2)通过真实实验对本发明提出的基于李群滤波的捷联惯导初试系统进行验证。真实试验中，系统装置放置在车上，有人员上下车、开关车门、对车进行晃动等干扰。实验装置如图5所示。实验历时600s，试验地点在北京工业大学羽毛球馆南广场位置。上位导航计算机控制导航系统，以100HZ的数据更新速率，115200bps的波特率，采集航向精度达0.1°、姿态精度达0.05°的实际三轴姿态信息，其上位机采集页面示意图如图6。解算获得的载体姿态信息与本步骤中得到的高精度真实载体姿态信息做比较，证明本方法和系统的可行性和有效性。

系统实验结果如下：

截取600s的实际数据，结果如图7所示。以姿态误差角的估计误差作为衡量精对准的指标。本发明提出的方法解算出的姿态角的最终精度要优于传统的四元数滤波和SVD分解法。从图7中可以更直观地看出，四元数滤波和SVD分解法航向角的误差分别大约在110′和150′，本发明提出的状态相关李群算法航向角误差在24′左右，大幅地提高了对准精度。由图7的(b)、(c)可以看出本发明所提出的算法对俯仰和横滚角的精度和稳定性也有一定的提高。结果证明该算法适用于动态条件下的初始对准。

Claims (2)

1.基于状态相关李群滤波的捷联惯性导航初始对准方法，地球坐标系e系，原点选取地球中心，X轴位于赤道平面内，从地心指向载体所在点经线，Z轴沿地球自转轴方向，随地球自转而转动，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；地心惯性坐标系i系，是在粗对准起始时刻将地球坐标系e系惯性凝固后形成的坐标系；导航坐标系n系，即导航基准的坐标系，导航相关运算都在该坐标系下进行，原点位于舰载机重心，X轴指向东向E，Y轴指向北向N，Z轴指向天向U；载体坐标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上；

其特征在于：该方法通过下述流程实现，

(1)捷联惯导系统进行预热准备，启动系统，获得载体所在位置的经度λ、纬度L的基本信息，采集惯性测量单元IMU中陀螺的输出角速度信息和加速度计的输出信息f^b；

(2)对采集到的陀螺和加速度计的数据进行处理，应用状态相关李群滤波方法估计姿态矩阵；

描述坐标系变换的旋转矩阵符合李群三维特殊正交群SO(3)的性质，构成了SO(3)群：

其中，R对应了旋转矩阵，表示3×3的向量空间，上标T表示矩阵的转置，I表示三维单位矩阵，det(*)表示矩阵的行列式；根据姿态矩阵的链式法则，从载体系到导航系的姿态矩阵可以分解为三个连续特殊正交阵的乘积形式：导航系从初始时刻到当前时刻的旋转矩阵；初始载体运动速度的惯性矩阵；载体系从当前时刻与初始时刻之间的旋转矩阵；

其中，t表示时间变量，n(t)表示t时刻的导航坐标系，n(0)表示t₀时刻的导航坐标系，b(t)表示t时刻的载体坐标系，b(0)表示t₀时刻的载体坐标系，和分别为导航坐标系和机体坐标系下从初始t₀时刻到t时刻的姿态转换矩阵；和由陀螺和加速度计的信息计算得到；那么，初始对准的任务由求解姿态矩阵的问题转化为求解初始姿态阵的问题；为初始t₀时刻的机体系与导航系之间的姿态转换矩阵，是一个常值矩阵；根据李群的微分方程，得到和的表达式；

式中 表示导航系和惯性系之间的相对角速率；表示地球自转速率；表示导航系相与地球坐标系之间的相对角速率；为陀螺测得的载体相对于惯性系的角速率；[·]×表示将括号内的矢量转换成反对称矩阵的过程；即

由旋转角度矢量到姿态矩阵的指数映射关系得更新矩阵如下

式中，t_M表示经过了M个时间间隔之后的时刻；表示对在采样间隔的积分，采用双子样算法Δθ₁和Δθ₂是两个相邻采样周期陀螺测量增量；

由于导航系相对于惯性系角度变化速率比较缓慢近似为

由定义可知

当前时刻的和由公式(8)和(9)迭代计算得到；

在根据李群姿态描述和惯性导航信息确定和后，只需要确定惯性系姿态转换矩阵便可根据公式(3)实时地求解出导航所需要的姿态矩阵；

惯性导航的速度微分方程为

式中，fⁿ表示加速度计测得的比力在导航坐标系下的投影，f^b表示比力在载体坐标系下的投影，gⁿ表示地球重力加速度在导航坐标系下的投影，vⁿ表示载体速度在导航坐标系下的投影；

将公式(2)代入公式(10)得

公式两边同乘以

移项得

对上式在[0,t]上对其进行积分整理得

所以基于李群描述的量测模型为

式子中

GPS、陀螺仪、加速度计的采样频率可知，所以β(t)根据离散化数值算法计算得到；采样频率足够高时，角速率和加速度近似为线性的形式

f^b＝a_ft+b_f (20)

对角速度积分可得角度的增量

从上式可得

同理可得

使用离散数值的方法，使用采样时间的陀螺和加速度的采样数据进行积分时的近似运算可得β(t)在采样时刻的离散化数值；

计算β(t)所用到的物理量和gⁿ可直接得到较为精确，更新频率不高变化几乎可忽略，vⁿ为GPS采集的相对可靠信息，所以β(t)较为精确；而α(t)完全靠IMU信息高频更新获得其中包含了大量的陀螺和加速度的噪声项；下面对α(t)误差项的组成进行分析

其中ε_g为陀螺仪随机误差，ε_a为加速度计随机误差，k为比例系数；在上式中

将公式(28)代入公式(29)得

式子中

由公式(29)可知α(t)所包含的噪声项是由陀螺仪噪声、加速度噪声、陀螺仪和加速度测量值信息耦合而成；

包含耦合噪声项的李群描述下的系统离散化量测方程为

β_k＝R_k(α_k+κε_a+λε_g) (31)

其中β_k和α_k分别为β(t)和α(t)在t_k时刻的观测值；

基于李群描述，本方法直接将旋转矩阵作为状态值进行估计；由于需要估计的为两个惯性坐标系之间旋转矩阵，是一个常值矩阵，所以系统李群形式下的状态方程和量测方程表示为

R_k+1＝R_k (32)

β_k＝R_k(α_k+δα_k) (33)

为了表示简明，将量测矩阵重写为

β_k＝R_kα_k+V_k (34)

其中，

V_k＝R_kδα_k

认为δα_k为两个高斯白噪声之和，其协方差矩阵为P_v＝(σ₁+σ₂)I₃，高斯白噪声的均值为零，所以有E{δα_k}＝0；噪声和状态量在统计学上相互独立，传统方法中将V_k的均值近似为零，E{R_kδα_k}＝0，其中R_k为状态量的真实值；状态相关量测噪声协方差矩阵的表示形式如下

在实际的滤波过程中，R_k是一个未知量，传统的与状态相关量测噪声协方差矩阵被近似为

式中表示姿态矩阵的状态量的估计值；状态量估计值和真实值之间的误差导致了传统的量测噪声协方差矩阵不够精确；假设则V_k的协方差矩阵的真实表示形式为

该矩阵被拆分为两部分

上式简写为

公式右侧的第二项为由与状态相关的测量噪声所导致的状态量和噪声项耦合的矩阵表示形式；本方法提出一种状态相关量测噪声协方差矩阵的精确表达形式；

R_k表示t_k时刻状态量的估计值，ξ_k为李代数形式下的状态误差；ξ_k和V_k的协方差矩阵分别为P_k和 的具体表示形式为

式中表示克罗内克尔积，矩阵的具体形式为其中E_i＝-[e_i]×，i＝1,2,3向量e_i，i＝1,2,3是三维欧式空间中的标准基底向量；的精确表达形式的为下面李群形式下的相关噪声项的具体形式；

用A_l∈G^m×m表示一个李群，其李代数为a，李代数形式下的状态误差协方差矩阵为 表示一个零均值白噪声序列，协方差矩阵为假设{a_k}和{l_k}在统计学上相互独立；有定义如下

v_k＝G(a_k)l_k (41)

其中G(·):a→A_l为李代数到李群的映射；v_k的协方差矩阵定义如下

定义为Γ＝[G_k(e₁)G_k(e₂)…G_k(e_i)]，i＝1,2,...,m；

列向量e_i为下的标准基底向量，G_k(·)为满足G_k(e_i)x＝G_k(x)e_i的线性转换函数。

2.基于状态相关李群滤波的捷联惯性导航初始对准方法，其特征在于：

公式(41)中的第一项为传统的状态相关噪声协方差矩阵，第二项为包含李群形式下状态误差协方差矩阵和噪声协方差矩阵的耦合项；

公式(42)融合了噪声协方差矩阵和状态协方差矩阵，将矩阵维数扩展至mm；

式中i＝1,2,...,m

根据李群的同构性原理，通过G_k(·)将状态误差量分解到各个基底上完成了状态误差协方差和噪声协方差的耦合并保证了的对称性；

由误差分析可知，假设测量噪声为两个均值为零的高斯白噪声，协方差矩阵分别为σ₁I₃和σ₂I₃，由李群性质可得

再由李群中矩阵运算的性质计算得

将公式(44)和(45)代入(39)可得状态相关噪声的计算公式为

使用上述优化的噪声协方差矩阵设计状态相关李群滤波器对公式(31)提出的系统模型进行估计，滤波过程如下：

根据以上滤波方法进行递推迭代，求出再根据公式(2)求出从而完成捷联惯导系统初始对准过程。