CN113532479B - 基于李群乘性卡尔曼滤波的捷联惯导系统运动初始对准方法 - Google Patents

Abstract

本发明公开了基于李群滤波的运动状态下捷联惯性导航对准方法，采用李群描述代替传统四元数描述实现对SINS姿态变换的计算，对利用李群和捷联惯导系统的特性构建的新系统进行分析，并根据李群中特殊正交群的性质定义的误差旋转矩阵，建立了基于误差状态的线性初始对准模型，利用凯莱变换恒等地映射到向量空间进行滤波。最后将误差补偿，得到姿态矩阵的最优估计。该方法避免了避免奇异值分解法的大失准角下的奇异点问题和传统四元数方法的非线性近似带来的精度问题；利用误差作为状态量进行估计并对误差进行补偿，状态相关误差得以减小，提高了对准精度。更适合于实际工程应用。

Description

基于李群乘性卡尔曼滤波的捷联惯导系统运动初始对准方法

技术领域

本发明公开了一种基于李群描述的捷联惯性导航运动初始对准方法，该方法属于导航方法及应用技术领域。

背景技术

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

惯性导航系统又可划分为平台式惯导系统和捷联式惯导系统(SINS)。在惯性导航系统早期研究中，最先建立的是平台式惯导系统，当时计算机的计算处理能力和惯性元件的测量精度都还处于较为落后的水平，因此制约了捷联式惯导系统的发展，而体积较大、构造复杂的平台式惯导系统在当时是主流的惯导系统。上世纪60年代，随着计算机处理能力和惯性元件精度的逐步提升，使得捷联式惯导系统得以迅速发展，与平台惯导相比，由于它不需要实际的物理平台，通过算法构建数学平台实现导航计算，体积小，结构简单，因此在小型飞行器或一些小型运载体上经常使用。捷联式惯导系统对于惯性导航器件的性能与计算的效能都有着比较高的要求，近些年来随着各类捷联惯性导航算法的研究深入，算法的运算速度、效能以及精度稳步提升，捷联式惯导系统也日趋成熟。

自对准的研究在捷联惯导系统中占有重要意义，尤其是运动状态下的自对准方法更是当下的研究热点。在目前运动状态下的自对准所使用的方法中，奇异值分解和四元数卡尔曼等方法是应用较为普遍的对准方法。但这些方法存在一些不可忽视的缺陷。奇异值分解是一种基于矩阵奇异值分解的最优化方法，由于其计算过程中未能合理估计传感器误差，且这些误差在奇异值分解过程中对运算的结果影响较大，所以其对准的精度并不理想，且由于其不符合李群特性，在大失准角时还会产生奇异点等问题。四元数卡尔曼滤波法是利用伪量测方程构建线性化滤波模型，使用卡尔曼滤波器完成对准任务的方法，这种方法虽然不会产生奇异点，但通常四元数的对准模型是非线性的，在对准过程中必须采用非线性估计方法或进行线性化近似，从而对最终对准精度产生影响。而在李群滤波对准过程中，由于构建增益矩阵和误差协方差矩阵时在李代数和李群空间的转换导致了计算误差的产生；其滤波器的设计原理是基于卡尔曼滤波改进而来，新息的定义不符合李群旋转矩阵的性质。因此会对对准精度产生影响。

为了解决以上方法中存在的问题，进一步提高初始对准的性能，本发明借鉴李群的性质和乘性卡尔曼滤波的思想，提出了一种新的李群乘性卡尔曼滤波算法。利用李群误差旋转矩阵的定义构建了误差状态，并且利用凯莱变换将误差状态矩阵转化为了误差状态向量，在恒等变换的基础上进行了向量卡尔曼滤波，所以整个滤波模型是线性的。基于这种误差状态构建的估计模型避免了传统方法中的奇异值问题和非线性问题，且每一步的迭代计算都符合李群旋转矩阵的特性。仿真实验证明了该算法的可行性，可以作为运动状态下的自对准方法使用。

发明内容

运动是载体最常见的状态，运动状态下的自对准算法具有很高的研究意义与应用价值。由于在运动状态下的初始对准过程中，载体更容易受到外界的各种干扰因素的影响，所以对算法有效性的要求也更高。本发明的目的是为了应对现有运动自对准方法存在的问题：(1)本发明通过李群描述初始姿态矩阵，且可用于大失准角下的运动对准，避免了奇异值分解法的大失准角下的奇异点问题；(2)本发明通过李群误差旋转矩阵的定义和凯莱变换，将李群误差旋转矩阵作为状态量并利用凯莱变换恒等地映射到误差向量空间，构建了线性的向量空间上的对准模型；并利用乘性卡尔曼滤波进行解算，得到李群误差旋转矩阵的最优估计；最后误差旋转矩阵补偿，便实现了运动状态下的初始对准。通过这种方法，避免了传统四元数方法的非线性近似带来的精度问题。

为了达到上述目的，本发明提供如下技术方案：

基于李群乘性卡尔曼滤波的SINS捷联惯性导航系统运动状态自对准方法，其特征在于，该方法通过下述步骤实现：

步骤(1)：SINS捷联惯性导航系统进行系统预热准备，启动系统，获得载体所在位置的经度λ、纬度L、当地重力加速度在导航系下的投影gⁿ等基本信息，采集惯性测量单元IMU中陀螺仪输出的载体系相对于惯性系的旋转角速率信息在载体系的投影和加速度计输出的载体系加速度信息f^b等；

步骤(2)：对采集到的陀螺仪和加速度计的数据进行预处理，基于李群微分方程，建立基于李群描述的线性对准系统模型：

本方法的详细描述中坐标系定义如下：

地球坐标系e系，选取地球中心为原点，X轴位于赤道平面内，从地心指向本初子午线，Z轴从地心指向地理北极，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；

地心惯性坐标系i系，选取地球中心为原点，X轴位于赤道平面内，从地心指向春分点，Z轴从地心指向地理北极，X轴、Y轴和Z轴构成右手坐标系；

导航坐标系n系，本方法中导航坐标系选取为地理坐标系，以载体重心为原点，与东-北-天坐标轴对齐，X轴与东向(E)重合，Y轴与北向(N)重合，Z轴与天向(U)重合；

载体坐标系b系，表示捷联惯性导航系统中惯性传感器的输出所在的坐标系，以载体重心为原点，X轴、Y轴、Z轴分别沿载体横轴指向右、沿纵轴指向前、沿立轴指向上；

初始导航坐标系n(0)系，表示捷联惯性导航系统开机运行时的导航坐标系，并在整个对准过程中相对于惯性空间保持静止；

初始载体坐标系b(0)系，表示捷联惯性导航系统开机运行时的载体坐标系，并在整个对准过程中相对于惯性空间保持静止；

结合李群的性质和捷联惯性导航系统的输出，建立线性的李群乘性卡尔曼滤波初始对准模型：

根据捷联惯性导航系统的特点，可以将运动状态下的对准问题转化为载体的姿态估计问题，姿态变换矩阵代表导航坐标系n系和载体坐标系b系之间的旋转，该矩阵是一个3×3的正交矩阵且行列式等于1，恰好符合李群中的三维特殊正交群SO(3)的性质，构成了三维旋转群SO(3)：

其中，R∈SO(3)表示用来代表姿态变换矩阵的三维旋转群SO(3)中的元素，表示3×3的向量空间，上标T表示矩阵的转置，I表示三维单位矩阵，det(R)表示为矩阵R的行列式；

运动状态下的对准问题转化为李群描述下载体的姿态变换矩阵R的估计问题；根据基于李群描述的实时姿态矩阵分解链式法则，将待求的姿态矩阵在时间域分解为三个矩阵的乘积形式：

其中，t表示时间，表示初始导航坐标系相对于t时刻导航坐标系的姿态矩阵，初始姿态矩阵/>表示初始载体坐标系相对于初始导航坐标系的姿态矩阵，/>表示t时刻载体坐标系相对于初始载体坐标系的姿态矩阵；

根据运动学特性和李群微分方程，姿态矩阵和/>随时间变化的更新微分方程为：

其中，表示初始载体坐标系相对于当前载体坐标系的姿态矩阵，/>表示导航坐标系相对于惯性坐标系的旋转角速率在导航坐标系下的投影，/>表示陀螺仪输出的载体坐标系相对于惯性坐标系的旋转角速率在载体坐标系的投影，符号(·×)表示将一个三维向量转换成一个反对称矩阵的运算，运算规则如下：

将公式(3)、(4)离散化，可以得到姿态矩阵和/>的迭代更新方程：

由公式(2)-(7)可得，和/>可以由IMU传感器数据实时计算得到，要求解/>只需要/>的值。而/>表示初始时刻的姿态矩阵，其不随时间变化，是一个常值姿态矩阵；因此，运动状态下的对准过程中姿态矩阵/>的求解问题，转化为对基于李群描述的初始姿态矩阵/>的求解问题；

根据捷联惯性导航中IMU和GPS的输出，可以构造出两个速度矢量，且其与基于李群描述的初始姿态矩阵的关系可以写成如下形式：

其中，β和α分别表示载体在n系下构造的速度矢量和b系下的构造的速度矢量，它们可通过下式求得：

其中vⁿ表示GPS在n系下的载体速度，vⁿ(0)表示初始时刻GPS在n系下的载体速度。

将常值漂移，随机漂移和噪声考虑后，将(10)改写为：

α_m＝α+Δα (11)

其中α_m表示传感器实际输出项，α表示真值项，Δα表示积分形式的误差项。

考虑初始姿态矩阵为常值姿态矩阵和公式(9)-(11)，运动状态下基于李群描述的对准模型为：

运用李群上真值与估计值之间误差旋转矩阵的定义，定义误差状态如下：

将式(13)带入(12)中，原状态转换为误差状态，状态方程为：

η_k＝η_k-1 (14)

由于选择了误差姿态矩阵η作为状态进行估计，所以不再表示状态的估计值，而是新状态η的一个关联值，表示为参考值R_ref。根据(13)，真值R、参考值R_ref和状态η之间的关系可以重写为:

R＝ηR_ref (15)

式中，R＝ηR_ref表示真实姿态R等于估计误差状态η乘以参考值R_ref。R_ref是在离散滤波每个时刻的最后一步进行校正的。R，η和R_ref都是SO(3)中的元素。

利用(12)中的观测方程，考虑(15)，基于李群误差状态η，得到如下观测方程：

β＝R(α_m-Δα)＝ηe_ref(α_m-Δα) (16)综上，重构的基于误差状态的李群对准模型为：

步骤(3)：根据上述误差状态的对准模型，根据凯莱变换将误差旋转群的元素映射到向量空间，并利用乘性卡尔曼滤波对其进行估计，再将误差补偿求得真实姿态R。

为了建立代数空间上的等效滤波模型，将误差特殊正交群，同时也是正交矩阵的η用下列凯莱变换进行群空间到代数空间的转换：

其中是η在误差代数空间上的投影。

将(18)代入(17)，得到误差向量空间中的等效状态方程和观测方程：

对(20)进行化简，两边左乘得到：

进一步化简(21)，可以得到误差代数空间的观测方程：

由于表示真值与估计值之间的误差代数估计值，且其高阶次项/>在短时间过滤间隔△t内变化不大，因此(22)可以近似为：

综上，代数空间上的等效滤波模型为：

对模型(24)使用乘性卡尔曼滤波器进行滤波，根据(24)其参数设置如下：

将这些参数带入乘性卡尔曼滤波器中，并综合考虑等效误差代数状态和原误差特殊正交群群状态，得到完整的滤波方程：

在修正误差状态后，姿态参考值R_ref仍然保留其上一步的值，因此不再表示最优估计。必须将更新信息从误差状态/>移动到参考值R_ref。故最后考虑R_ref的更新并将误差归零：

在滤波的每一步中将作为/>的估计值并结合(2)、(3)和(4)，即可得到每一离散时刻对姿态矩阵/>的最优估计。

步骤(4)：求解导航系统所需的姿态矩阵从而完成运动状态下对准过程：

根据之前步骤中求解得到的姿态变化矩阵和/>信息，通过公式(2)即可求解导航姿态矩阵，完成运动状态下捷联惯性导航系统的对准。

与现有技术相比，本发明具有如下优点和有益效果：

(1)本发明通过李群代替四元数描述初始姿态矩阵，利用凯莱变换使得整个滤波模型成为线性模型，避免了奇异值分解法的大失准角下的奇异点问题和传统四元数方法的非线性近似带来的精度问题；

(2)本发明通过根据李群中误差旋转矩阵的物理定义，利用误差作为状态量进行估计并对误差进行补偿，由于误差状态向量小于一般的状态向量，状态相关误差得以减小，提高了对准精度。

附图说明

图1捷联惯性导航系统装置总体简图。

图2捷联惯性导航系统流程图。

图3对准仿真结果图。

具体实施方式

本发明是基于李群乘性卡尔曼滤波的SINS捷联惯性导航系统运动状态下初始对准方法设计，下面结合本发明系统流程图对本发明的具体实施步骤进行详细的描述：

本发明提供的基于李群乘性卡尔曼滤波的SINS捷联惯性导航系统运动状态下初始对准方法，首先获取传感器实时数据；对采集到的数据进行处理，基于李群微分方程，建立基于李群描述的线性对准系统模型；使用李群乘性卡尔曼滤波算法，估计得到基于李群描述的初始姿态矩阵并求解姿态矩阵/>在对准期间，经过多次滤波迭代解算，最终得到精确的初始姿态矩阵/>和姿态矩阵/>完成对准过程。

步骤1：SINS捷联惯性导航系统进行系统预热准备，启动系统，获得载体所在位置的经度λ、纬度L、当地重力加速度在导航系下的投影gⁿ等基本信息，采集惯性测量单元IMU中陀螺仪输出的载体系相对于惯性系的旋转角速率信息在载体系的投影和加速度计输出的载体系加速度信息f^b等；

步骤2：对采集到的陀螺仪和加速度计的数据进行预处理，基于李群微分方程，建立基于李群描述的线性对准系统模型：

根据捷联惯性导航系统的特点，可以将运动状态下的对准问题转化为载体的姿态估计问题，姿态变换矩阵代表导航坐标系n系和载体坐标系b系之间的旋转，该矩阵是一个3×3的正交矩阵且行列式等于1，恰好符合李群中的三维特殊正交群SO(3)的性质，构成了三维旋转群SO(3)：

其中，R∈SO(3)表示用来代表姿态变换矩阵的三维旋转群SO(3)中的元素，表示3×3的向量空间，上标T表示矩阵的转置，I表示三维单位矩阵，det(R)表示为矩阵R的行列式；

运动状态下的对准问题转化为李群描述下载体的姿态变换矩阵R的估计问题；根据基于李群描述的实时姿态矩阵分解链式法则，将待求的姿态矩阵在时间域分解为三个矩阵的乘积形式：

其中，t表示时间，表示初始导航坐标系相对于t时刻导航坐标系的姿态矩阵，初始姿态矩阵/>表示初始载体坐标系相对于初始导航坐标系的姿态矩阵，/>表示t时刻载体坐标系相对于初始载体坐标系的姿态矩阵；

根据运动学特性和李群微分方程，姿态矩阵和/>随时间变化的更新微分方程为：

其中，表示初始载体坐标系相对于当前载体坐标系的姿态矩阵，/>表示导航坐标系相对于惯性坐标系的旋转角速率在导航坐标系下的投影，/>表示陀螺仪输出的载体坐标系相对于惯性坐标系的旋转角速率在载体坐标系的投影，符号(·×)表示将一个三维向量转换成一个反对称矩阵的运算，运算规则如下：

将公式(30)、(31)离散化，可以得到姿态矩阵和/>的迭代更新方程：

由公式(29)-(34)可得，和/>可以由IMU传感器数据实时计算得到，要求解只需要/>的值。而/>表示初始时刻的姿态矩阵，其不随时间变化，是一个常值姿态矩阵；因此，运动状态下的对准过程中姿态矩阵/>的求解问题，转化为对基于李群描述的初始姿态矩阵/>的求解问题；

根据捷联惯性导航中IMU和GPS的输出，可以构造出两个速度矢量，且其与基于李群描述的初始姿态矩阵的关系可以写成如下形式：

其中，β和α分别表示载体在n系下构造的速度矢量和b系下的构造的速度矢量，它们可通过下式求得：

其中vⁿ表示GPS在n系下的载体速度，vⁿ(0)表示初始时刻GPS在n系下的载体速度。

将常值漂移，随机漂移和噪声考虑后，将(37)改写为：

α_m＝α+Δα (38)

其中α_m表示传感器实际输出项，α表示真值项，Δα表示积分形式的误差项。

考虑初始姿态矩阵为常值姿态矩阵和公式(36)-(38)，运动状态下基于李群描述的对准模型为：

运用李群上真值与估计值之间误差旋转矩阵的定义，定义误差状态如下：

将式(40)带入(39)中，原状态转换为误差状态，状态方程为：

η_k＝η_k-1 (41)

由于选择了误差姿态矩阵η作为状态进行估计，所以不再表示状态的估计值，而是新状态η的一个关联值，表示为参考值R_ref。根据(40)，真值R、参考值R_ref和状态η之间的关系可以重写为:

R＝ηR_ref (42)

式中，R＝ηR_ref表示真实姿态R等于估计误差状态η乘以参考值R_ref。R_ref是在离散滤波每个时刻的最后一步进行校正的。R，η和R_ref都是SO(3)中的元素。

利用(39)中的观测方程，考虑(42)，基于李群误差状态η，得到如下观测方程：

β＝R(α_m-Δα)＝ηR_ref(α_m-Δα) (43)

综上，重构的基于误差状态的李群对准模型为：

步骤3：根据上述误差状态的对准模型，根据凯莱变换将误差旋转群的元素映射到向量空间，并利用乘性卡尔曼滤波对其进行估计，再将误差补偿求得真实姿态R。

为了建立代数空间上的等效滤波模型，将误差特殊正交群，同时也是正交矩阵的η用下列凯莱变换进行群空间到代数空间的转换：

其中是η在误差代数空间上的投影。

将(45)代入(44)，得到误差向量空间中的等效状态方程和观测方程：

对(47)进行化简，两边左乘得到：

进一步化简(48)，可以得到误差代数空间的观测方程：

由于表示真值与估计值之间的误差代数估计值，且其高阶次项/>在短时间过滤间隔△t内变化不大，因此(49)可以近似为：

综上，代数空间上的等效滤波模型为：

对模型(51)使用乘性卡尔曼滤波器进行滤波，根据(24)其参数设置如下：

将这些参数带入乘性卡尔曼滤波器中，并综合考虑等效误差代数状态和原误差特殊正交群群状态，得到完整的滤波方程：

在修正误差状态后，姿态参考值R_ref仍然保留其上一步的值，因此不再表示最优估计。必须将更新信息从误差状态/>移动到参考值R_ref。故最后考虑R_ref的更新并将误差归零：

在滤波的每一步中将作为/>的估计值并结合(29)、(30)和(31)，即可得到每一离散时刻对姿态矩阵/>的最优估计。

步骤4：求解姿态矩阵并解算姿态信息，

求解导航系统所需的姿态矩阵从而完成运动状态下对准过程：

根据之前步骤中求解得到的姿态变化矩阵和/>信息，通过公式(29)即可求解导航姿态矩阵，完成运动状态下捷联惯性导航系统的对准。

本发明的有益效果如下：

(1)在以下的仿真条件下，对该方法进行仿真实验：

步骤(1)中，模拟运动状态下载体收到风浪影响，其航向角ψ、俯仰角θ、横滚角γ作周期变化，姿态变化情况如下：

步骤(1)中，初始地理位置：东经118°，北纬40°；

步骤(1)中，传感器输出频率为100Hz；

步骤(1)中，陀螺仪漂移：三个方向轴上的陀螺常值漂移为0.02°/h，随机漂移为0.005°/h；

步骤(1)中，加速度计零位偏置：三个方向轴上的加速度计常值偏置为2×10^-4g，随机偏置为

步骤(2)中，地球自转角速率7.2921158e^-5rad/s；

步骤(2)中，时间间隔T为0.02s；

步骤(3)中，李群最优估计算法初始值

方法仿真结果如下：

进行了600s仿真，以姿态角的估计误差作为衡量指标，仿真结果如图(3)所示。从图中可以看出，俯仰姿态在90s左右完成对准，收敛到0.5′；横滚姿态在110s左右完成对准，收敛到0.6′；航向姿态在135s左右完成对准，收敛到5.8′。由仿真结果可知，本方法可以快速有效的完成运动状态下对准任务。

本发明采用李群描述代替传统四元数描述实现对SINS姿态变换的计算，采用李群描述代替传统四元数描述实现对SINS姿态变换的计算，利用李群和捷联惯导系统的特性进行了取代四元数描述方法的新系统分析，并根据李群中特殊正交群的性质定义的误差旋转矩阵，建立了基于误差状态的线性初始对准模型，利用凯莱变换恒等地映射到向量空间进行滤波。最后将误差补偿，得到姿态矩阵的最优估计，从而实现了SINS的一步直接自对准；本发明利用凯莱公式恒等地将状态量映射到向量空间进行滤波，避免了近似问题，有效提高了对准精度。

以上所述仅是本发明的优选实施方式，并不用于限制本发明。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和变型，这些改进和变型也应视为本发明的保护范围。

Claims (1)

1.基于李群滤波的捷联惯导系统运动状态初始对准方法，该方法通过下述步骤实现：

步骤(1)：运行SINS捷联惯性导航系统，获取载体所在位置的经度λ、纬度L、当地重力加速度在导航系下的投影gⁿ，采集惯性测量单元IMU中陀螺仪输出的载体系相对于惯性系的旋转角速率信息在载体系的投影和加速度计输出的载体系下载体的加速度信息f^b；

步骤(2)：初始对准获得准确的姿态信息，以用一个3×3的正交矩阵来表示，称为姿态矩阵；此姿态矩阵表示的是t时刻从载体坐标系b系到导航坐标系n系的正交旋转矩阵；该正交旋转矩阵符合李群的特殊正交群SO(n)的性质，构成三维旋转群SO(3)：

故姿态矩阵用李群中的三维特殊正交群来表示，记作/>在时间域下对/>分解，得到三个三维特殊正交群的乘积：

其中R表示三维特殊正交群的元素，表示t时刻n0系到n系的地球运动的旋转矩阵,是初始时刻b0系到n0系的初始旋转矩阵，/>表示t时刻b系到b0系的载体运动的旋转矩阵；

而和/>通过下列计算求得：

其中和/>是Δt期间的角速度积分项，所以/>和/>通过迭代(5)和(6)更新；为了得到/>还需要估计初始常值姿态矩阵/>利用IMU和GPS的输出构造出两个向量，并与有如下关系：

将时变特殊正交群中的元素的估计问题转化为时不变特殊正交群中的元素/>的估计问题；且由于/>的大小不随时间变化，根据卡尔曼滤波状态估计理论，改写上下角标，并考虑常值漂移、随机漂移和噪声综合影响构成的误差项Δα，将α改写为包含误差项的传感器输出α_m＝α+Δα，得到李群描述下的初始对准模型：

运用李群上真值与估计值之间误差旋转矩阵的定义，定义误差状态如下：

将式(11)带入式(10)中，原状态转换为误差状态，状态方程为：

η_k＝η_k-1 (12)

由于选择了误差姿态矩阵η作为状态进行估计，所以不再表示状态的估计值，而是新状态η的一个关联值，表示为参考值R_ref；根据式(11)，真值R、参考值R_ref和状态η之间的关系重写为:

R＝ηR_ref (13)

式中，R＝ηR_ref表示真实姿态R等于估计误差状态η乘以参考值R_ref；R_ref是在离散滤波每个时刻的最后一步进行校正的；R、η和R_ref都是SO(3)中的元素；

利用式(10)中的观测方程，考虑式(13)，基于李群误差状态η，得到如下观测方程：

β＝R(α_m-Δα)＝ηR_ref(α_m-Δα) (14)

重构的基于误差状态的李群对准模型为：

步骤(3)：基于步骤(2)，建立代数空间上等效滤波模型，并对η_k进行最优估计，再利用η_k和R_ref的关系更新R_ref：

为了建立代数空间上的等效滤波模型，将误差特殊正交群，同时也是正交矩阵的η用下列凯莱变换进行群空间到代数空间的转换：

是η在误差代数空间上的投影；其中符号(·×)表示将一个三维向量转换成一个反对称矩阵的运算，运算规则如下：

将式(16)代入式(15)，得到误差向量空间中的等效状态方程和观测方程：

对式(19)进行化简，两边左乘得到：

化简式(20)，得到误差代数空间的观测方程：

由于表示真值与估计值之间的误差代数估计值，且其高阶次项/>在短时间过滤间隔△t内变化不大，式(21)近似为：

综上，代数空间上的等效滤波模型为：

对式(23)使用乘性卡尔曼滤波器进行滤波，根据式(23)参数设置如下：

将这些参数带入乘性卡尔曼滤波器中，并综合考虑等效误差代数状态和原误差特殊正交群群状态，得到完整的滤波方程：

在修正误差状态后，姿态参考值R_ref仍然保留其上一步的值，因此不再表示最优估计；必须将更新信息从误差状态/>移动到参考值R_ref；故最后考虑R_ref的更新并将误差归零：

在滤波的每一步中将作为/>的估计值并结合式(2)、式(3)和式(4)，即得到每一离散时刻对姿态矩阵/>的最优估计。