CN104897170B - 一种基于罗德里格参数和二阶非线性量测的滤波对准算法 - Google Patents

一种基于罗德里格参数和二阶非线性量测的滤波对准算法 Download PDF

Info

Publication number
CN104897170B
CN104897170B CN201510297409.9A CN201510297409A CN104897170B CN 104897170 B CN104897170 B CN 104897170B CN 201510297409 A CN201510297409 A CN 201510297409A CN 104897170 B CN104897170 B CN 104897170B
Authority
CN
China
Prior art keywords
mrow
msub
msubsup
mtd
msup
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201510297409.9A
Other languages
English (en)
Other versions
CN104897170A (zh
Inventor
严恭敏
梅春波
付强文
秦永元
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Sichuan Hanghao Technology Co ltd
Original Assignee
Northwestern Polytechnical University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN201510297409.9A priority Critical patent/CN104897170B/zh
Publication of CN104897170A publication Critical patent/CN104897170A/zh
Application granted granted Critical
Publication of CN104897170B publication Critical patent/CN104897170B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Manufacturing & Machinery (AREA)
  • Automation & Control Theory (AREA)
  • Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于罗德里格参数和二阶非线性量测的滤波对准算法，包括由惯性系动基座对准过程、二阶非线性量测滤波估计算法、经典罗德里格参数奇异点及其处理方法，利用姿态阵分解，将动态姿态的估计问题转化为一个常值姿态的估计，借助经典Rodrigues参数和姿态阵之间的凯莱变换，建立了在惯性坐标系描述的系统方程线性，量测方程具有二阶非线性的弱非线性误差模型。同时对经典罗德里格参数描述姿态存在奇异点的问题，也进行了详细分析并给出了解决办法。本发明在车辆启动段典型机动条件下，能够快速收敛，对准精度高，以与线性卡尔曼滤波相当的计算量及计算复杂度实现了任意失准角非线性动基座对准。

Description

一种基于罗德里格参数和二阶非线性量测的滤波对准算法
技术领域
本发明涉及惯性导航技术领域，具体是一种基于罗德里格参数和二阶非线性量测的滤波对准算法。
背景技术
动基座对准能够有效提高捷联惯导系统载体平台的机动性能，具有很高的军事应用价值。动基座对准的研究内容主要包括两个方面，一是建立大失准角条件下的非线性误差模型[1～3]；二是设计相应的非线性滤波估计算法[2～9]。
依据姿态描述方式的不同，可以得到不同的非线性误差模型，如基于四元数非线性误差模型[1]、基于欧拉角非线性误差模型[2]、基于修正Rodrigues参数非线性误差模型[3]，以及大方位失准角条件下，基于方位角正余弦函数的非线性误差模型[4]等。其中，四元数非线性误差模型无奇异点，使用最为广泛，但在设计滤波算法时需要考虑其模值约束的影响。欧拉角姿态描述法存在奇异点，因此不适用于任意姿态对准，且基于欧拉角的非线性误差模型中含有状态量的正余弦函数，使得误差模型非线性增大。传统的基于修正Rodrigues参数的误差模型虽可避免奇异点，但是模型非线性度同样很大。此外，传统方法建立动基座对准非线性误差模型时，均以动态载体系和动态导航系之间的实时姿态为估计对象，系统方程及量测方程均建立在动态的导航坐标系下。以速度为观测量时，以上误差模型只有系统方程为非线性，量测方程则是线性的。
在非线性滤波算法的选择上，常规EKF滤波算法需要求导计算Jacobian矩阵，且在处理严重非线性问题时，可能出现滤波误差增大甚至发散的现象。因此，一类基于sigma点的非线性滤波算法成为研究的热点，如UKF滤波[2，3，4]、改进强跟踪UKF滤波[5]、粒子滤波[6]、Gauss-Hermite滤波[7，8]，以及容积卡尔曼滤波(CKF)[9]等。基于sigma点的非线性滤波算法，用确定性或随机采样策略逼近非线性函数的概率分布，无需对非线性模型求导，且可以通过优化采样策略，减小计算量并提高非线性函数概率分布的近似精度。总体来讲，基于sigma点的非线性滤波算法可以做到计算量与EKF滤波相当，但是精度优于EKF滤波。
发明内容
本发明的目的在于提供一种基于罗德里格参数和二阶非线性量测的滤波对准算法，以解决上述背景技术中提出的问题。
为实现上述目的，本发明提供如下技术方案：
一种基于罗德里格参数和二阶非线性量测的滤波对准算法，由惯性系动基座对准过程、二阶非线性量测滤波估计算法、经典Rodrigues参数奇异点及其处理方法，
具体描述如下：
步骤1，惯性系动基座对准过程
惯性系动基座对准算法以实时姿态阵的链式分解为基础，
其中，nt系为实时导航坐标系，即载体时变位置东北天地理坐标系；in为导航惯性系，与动基座对准开始时刻的n系重合；b为载体坐标系；ib为载体惯性系，与对准开始时刻的b系重合；式(1)中，是运动的nt系相对于导航惯性系in的姿态阵，由GPS输出位置信息解析计算；能利用陀螺输出进行姿态跟踪，所以，惯性系动基座对准过程是对常值姿态阵的估计；
利用牛顿第二定律和哥氏定理，得到惯性系比力方程如下
其中，为t时刻载体对地速度在导航惯性系in内的投影；为t时刻载体所在位置重力加速度在导航惯性系in内投影；为t时刻理想比力值；
对式(2)两端分别进行积分，并记
利用GPS输出完成式(3)中的求解，
其中，
进一步，在tk-1至tk更新周期内，假设为常矢量，导航惯性系in内对地速度为线性函数，即
其中，t∈[tk-1，tk]；T＝tk-tk-1为GPS量测更新周期；
将式(9)、(10)代入式(7)、(8)中，整理得
其中，
利用捷联惯导姿态、速度二子样更新算法实现对式(4)中的求解；进一步，考虑陀螺仪随机常值漂移εb和加速度计随机常值零偏的影响，推导得
其中，为姿态误差角；为加速度计惯性系比力积分误差；
由式(2)、(3)、(4)和式(13)可得，求解常值姿态阵的观测方程为
进一步，用经典Rodrigues参数法来等价描述姿态阵记对应Rodrigues参数为l，则二者满足凯莱变换关系式，即
将式(17)代入式(16)，整理得，
其中，wv包含惯性器件测量噪声的积分和随机扰动的积分，且有
式(18)即是与姿态阵等价的Rodrigues参数l的观测方程，若能估计出Rodrigues参数l，则依据式(17)得到
综上，惯性系动基座对准选取如下15维状态
由上述推导，系统方程及量测方程分别为，
利用式(21)、式(22)设计滤波算法实现对Rodrigues参数l的估计，进而得到姿态阵通过式(1)即实现动基座对准；
步骤2，二阶非线性量测滤波估计算法
式(21)描述的系统方程为线性，式(22)描述的量测方程为非线性，但仅是状态量的二阶非线性函数，能用有限阶Taylor级数展开描述，即
其中，Xk0为Taylor级数展开点；ei是第i个分量为1，其余元素为0的3维单位向量；Hk为非线性函数h的雅克比阵；Di为非线性函数h的二阶偏导数阵；Tr为矩阵求迹函数，且有
其中，h＝[h1 h2 h3]T；
同时，对于二阶非线性函数，其二阶偏导数阵为常值矩阵，故由式(21)、(22)、(25)，知Di为15维常值对称阵，其中非零元素仅有
D1(2，6)＝1，D2(1，6)＝-1，D3(1，5)＝1，
D1(3，5)＝-1，D2(3，4)＝1，D3(2，4)＝-1
D1(6，2)＝1，D2(6，1)＝-1，D3(5，1)＝1，
D1(5，3)＝-1，D2(4，3)＝1，D3(4，2)＝-1 (26)
步骤2.1，滤波时间更新算法
式(21)系统方程为线性，采用标准卡尔曼滤波算法完成时间更新，得到状态量和估计误差方差阵的一步预测，即和Pk/k-1；用状态一步预测结果代替式(23)中Xk0，建立起当前观测量与状态一步预测关系式，进而设计量测更新算法对一步预测结果进行校正，得到当前时刻的状态最优估计值下面推导基于式(23)二阶泰勒级数量测方程的滤波量测更新算法；
步骤2.2，滤波量测更新算法
量测更新形式定义为与线性卡尔曼滤波量测更新一致，假设k时刻状态估计结果为
其中，Lk为引入的补偿项，和最佳增益Kk一样均为待定值；Lk和Kk的确定原则分别为使为无偏估计和使的均方误差阵Pk的迹最小；
定义状态估计误差
其中，
由式(23)、式(24)、式(27)、式(28)整理得
要使为无偏估计，即要求期望为零；假设时间更新为无偏估计，则对式(29)右端取期望并令结果为零，得补偿项Lk为
其中，E[·]表示对括号内变量求期望；
将式(30)代入式(29)中，整理得
其中，
由于E[wv]＝0，且wv与A不相关，故由式(31)、式(32)得Pk为
其中
Λk＝E[AAT] (34)
式(32)中，A为3维列向量，从而Λk为3阶方阵；利用式(32)，经过推导得Λk第i行第j列元素为
式(33)中协方差阵更新方式与标准卡尔曼滤波形式一致，从而式(27)中最佳增益阵Kk，
考察量测更新方程式(27)、(33)、(36)，基于二阶泰勒级数的量测更新算法与标准卡尔曼滤波在形式上完全一致，仅增加了对Lk、Λk的计算；而由式(30)、(35)知，Lk、Λk的求解简单；考虑到Di为常值稀疏矩阵，将Lk、Λk描述为仅与Pk/k-1相关的形式，如此进一步减小在线计算量；
步骤3，经典Rodrigues参数奇异点及其处理方法
经典Rodrigues参数是最少参数姿态描述方法之一，存在奇异点，一种等价表示方法为
其中，u为两坐标系之间等效旋转矢量的方向向量；φ为等效旋转矢量转过的角度；
当φ取值为±π时，l值为无穷大，是经典Rodrigues参数法进行姿态描述时的奇异点，对ib系进行某种虚拟转动得到ib1，使得ib1系相对于in系的Rodrigues参数远离奇异点，然后采用前述滤波对准算法完成对应Rodrigues参数的估计，进而借助已知的虚拟转动，计算
作为本发明进一步的方案：所述步骤3中具体包括如下步骤：
步骤3.1，奇异点判别函数设计
当φ取值为±π时，ib系至in系姿态矩阵描述为
此时，任意矢量R在两坐标系内投影满足
在式(39)两端分别加上并利用单位向量恒等关系式I+(u×)2＝uuT，则有
也即是，此时任意矢量在两坐标系内的投影之和均共线，且与等效旋转矢量平行；
因此，对当前滤波变量l是否接近奇异点的判断等价于判断式(19)中不同时刻的是否接近共线；为了缩短得出有效判断所需时间，设定如下判别函数
其中，为的方向向量；|·|表示取方阵行列式；
若用于计算gk的所有均共线，即当前对变量l的估计位于奇异点时，gk理论值为零；
步骤3.2，虚拟转动设计
虚拟转动的设计依据：若ib系至in系等效旋转矢量转角大于π/2，将ib系绕着其某个坐标轴转动π角得到ib1系，使ib1系至in系等效旋转矢量转角小于π/2；
据此，记ib系绕其x轴转动π角得到ibx系；ib系绕其y轴转动π角得到iby系；ib系绕其z轴转动π角得到ibz系，则有
其中，为已知常值对角阵，对角元为1或-1；
无论载体动基座对准开始时刻真实姿态为何值，姿态阵所对应的四个等效旋转矢量中，至少有一个等效旋转矢量的转角小于π/2，远离奇异点；
步骤3.3，奇异点规避的滤波算法
依据上面设计的判别函数和虚拟转动，得到奇异点规避的滤波算法：
(1)给定奇异点判别时间tk，在tk时刻以前，利用步骤1、步骤2推导的对准滤波算法，同时处理四个独立的滤波过程，分别对进行估计；其中，对 进行估计时，式(21)、式(22)中相关参变量需要进行相应的坐标变换；所有滤波器状态量初值均设为零，考虑式(27)及虚拟转动分析结论，将l对应的方差阵初值设定为P00＝1，P11＝1，P22＝1；
(2)在tk时刻，分别计算姿态阵所对应的判别函数值，gk、gkx、gky、gkz，仅选择最大判别函数值所对应的滤波过程完成后续滤波对准。
与现有技术相比，本发明的有益效果是：本发明对于捷联惯导系统动基座对准问题，给出了一种基于罗德里格参数和二阶非线性量测的滤波对准算法，并对罗德里格参数奇异点问题进行了详细讨论，设计了处理方案。仿真分析结果表明，该算法在车辆启动段典型机动条件下，能够快速收敛，对准精度高，以与线性卡尔曼滤波相当的计算量及计算复杂度实现了任意失准角非线性动基座对准。
附图说明
图1是姿态1时判别函数随时间变化曲线；
图2是姿态1时姿态误差角均方根曲线；
图3是姿态1时姿态误差角均方根曲线；
图4是姿态2时判别函数随时间变化曲线；
图5是姿态2时姿态误差角均方根曲线；
图6是姿态2时姿态误差角均方根曲线。
具体实施方式
下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明提供了一种基于罗德里格参数和二阶非线性量测的滤波对准算法，利用姿态阵分解，将动态姿态的估计问题转化为一个常值姿态的估计，借助经典Rodrigues参数和姿态阵之间的凯莱变换，建立了在惯性坐标系描述的系统方程线性，量测方程具有二阶非线性的弱非线性误差模型。对于这种非线性系统的估计，采用标准线性卡尔曼滤波算法完成时间更新。对于量测更新，借鉴高阶EKF量测更新的思路，推导了一种基于二阶非线性量测完整泰勒级数展开的量测更新算法。新的量测更新算法在实现形式上与线性卡尔曼滤波量测更新完全一致，且计算量相当。综合时间更新和量测更新，可以认为新算法用线性滤波方式解决了大失准角非线性滤波对准问题，在计算量上远小于导航系内基于sigma点的各类非线性滤波对准算法。同时对经典Rodrigues参数描述姿态存在奇异点的问题，也进行了详细分析并给出了解决办法。根据上述方案进行仿真分析，具体描述如下。
实施例1
本发明实施例中，以车载SINS/GPS动基座对准为例，仿真轨迹为：由静止加速至10m/s，加速度为0.5m/s2；然后匀速行驶50s；之后转弯90°，角速率为15°/s；之后匀速直线行驶。对准总时间取为120s。
惯性器件精度：陀螺随机常值漂移为0.01°/h，角随机游走系数为加速度计随机常值偏置为5×10-5g，加速度计白噪声方差强度为不考虑其他误差项。
GPS精度：速度测量噪声为0.1m/s，水平位置测量噪声为3m，高度位置测量噪声为5m，GPS数据周期为1s。
考虑车载SINS测量坐标系与右前上车体坐标系近似安装，故水平姿态角通常为小角，奇异点只出现在方位角接近180°时。此外，由小节3的分析易知，采用奇异点规避策略后，最终对准算法所估计的姿态对应的等效旋转矢量转角一定不大于π/2。综上，仿真中，验证两种姿态初值条件下的对准效果：姿态1，俯仰角、滚转角、方位角分别为5°、3°、80°；姿态2，俯仰角、滚转角、方位角分别为5°、3°、180°，对上述两种姿态初值条件下的初始对准算法各进行100次的蒙特卡洛仿真，仿真结果如图1～图6所示。
图1、图4描述的是两种初始姿态条件下，奇异点规避策略所使用判别函数的值随时间变化曲线，该函数值为无量纲量。从图1、4中可以得出两点结论：第一，不同等效旋转矢量对应的判别函数在较短时间内即具有很高的区分度，这样奇异点判别时间tk即可设计的很小，以仿真为例，tk最短可以选择为2s；第二，判别函数的值与对准过程中载体机动相关。仿真程序中tk设计为5s。
姿态1时对准姿态误差收敛曲线如图2、图3所示，其中，图2为整个对准区间收敛曲线；图3为70s至120s的收敛曲线。姿态2时对准姿态误差收敛曲线如图5、图6所示，图5对应整个对准区间收敛曲线，图6为70s到120s收敛曲线。
与gk、gkx、gky、gkz相关的等效旋转矢量转角，在姿态1初值下，分别为80.05°、174.24°、179.08°、100.26°；在姿态2初值条件下，分别为179.87°、177°、175°、5.83°。由图1、图4易知，姿态1初值条件下，等效旋转矢量转角为80.05°的滤波过程在判别完成后被保留；姿态2初值条件下，等效旋转矢量转角为5.83°的滤波过程在判别完成后被保留。
两种姿态初值条件下，滤波对准结果如图2、图5所示。在车辆加速机动条件下，对准算法能够快速收敛，在150s对准结束时刻，姿态1初值条件下，两水平姿态误差角和方位姿态误差角均方根分别为9.6″、10″、3.87′；姿态2初值条件下，两水平姿态误差角和方位姿态误差角均方根分别为9.9″、9.4″、3.18′。
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。
此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (2)

1.一种基于罗德里格参数和二阶非线性量测的滤波对准算法，包括惯性系动基座对准过程、二阶非线性量测滤波估计算法、经典Rodrigues参数奇异点及其处理方法，其特征在于，具体描述如下：
步骤1，惯性系动基座对准过程
惯性系动基座对准算法以实时姿态阵的链式分解为基础，
<mrow> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> <mi>b</mi> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> <mi>b</mi> </msubsup> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>
其中，nt系为实时导航坐标系，即载体时变位置东北天地理坐标系；in为导航惯性系，与动基座对准开始时刻的n系重合；b为载体坐标系；ib为载体惯性系，与对准开始时刻的b系重合；式(1)中，是运动的nt系相对于导航惯性系in的姿态阵，由GPS输出位置信息解析计算；能利用陀螺输出进行姿态跟踪，所以，惯性系动基座对准过程是对常值姿态阵的估计；
利用牛顿第二定律和哥氏定理，得到惯性系比力方程如下
<mrow> <msup> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <msub> <mi>i</mi> <mi>n</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>&amp;times;</mo> <msup> <mi>v</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <msup> <mi>g</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msup> <mi>f</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>
其中，为t时刻载体对地速度在导航惯性系in内的投影；为t时刻载体所在位置重力加速度在导航惯性系in内投影；为t时刻理想比力值；
对式(2)两端分别进行积分，并记
<mrow> <msubsup> <mo>&amp;Integral;</mo> <mn>0</mn> <msub> <mi>t</mi> <mi>k</mi> </msub> </msubsup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <msup> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <msub> <mi>i</mi> <mi>n</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>&amp;tau;</mi> <mo>)</mo> </mrow> <mo>+</mo> <mrow> <mo>(</mo> <mrow> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>&amp;times;</mo> <msup> <mi>v</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>&amp;tau;</mi> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> <mo>-</mo> <msup> <mi>g</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>&amp;tau;</mi> <mo>)</mo> </mrow> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mi>d</mi> <mi>&amp;tau;</mi> <mo>=</mo> <msubsup> <mi>V</mi> <mi>r</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msubsup> <mo>&amp;Integral;</mo> <mn>0</mn> <msub> <mi>t</mi> <mi>k</mi> </msub> </msubsup> <msup> <mi>f</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>&amp;tau;</mi> <mo>)</mo> </mrow> <mi>d</mi> <mi>&amp;tau;</mi> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <msubsup> <mo>&amp;Integral;</mo> <mn>0</mn> <msub> <mi>t</mi> <mi>k</mi> </msub> </msubsup> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <msup> <mi>f</mi> <mi>b</mi> </msup> <mrow> <mo>(</mo> <mi>&amp;tau;</mi> <mo>)</mo> </mrow> <mi>d</mi> <mi>&amp;tau;</mi> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <msubsup> <mi>V</mi> <mi>m</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>
其中，记为从0到tk时刻载体视速度在导航惯性系in上的积分；记为从0到tk时刻载体视速度在载体惯性系ib上的积分；
利用GPS输出完成式(3)中的求解，
<mrow> <msubsup> <mi>V</mi> <mi>r</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>V</mi> <mi>r</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>dV</mi> <mrow> <mi>r</mi> <mn>1</mn> <mi>k</mi> </mrow> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>&amp;times;</mo> <msubsup> <mi>dV</mi> <mrow> <mi>r</mi> <mn>2</mn> <mi>k</mi> </mrow> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>+</mo> <msubsup> <mi>dV</mi> <mrow> <mi>r</mi> <mn>3</mn> <mi>k</mi> </mrow> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>
其中，
<mrow> <msubsup> <mi>dV</mi> <mrow> <mi>r</mi> <mn>2</mn> <mi>k</mi> </mrow> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>=</mo> <msubsup> <mo>&amp;Integral;</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>t</mi> <mi>k</mi> </msub> </msubsup> <msup> <mi>v</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>d</mi> <mi>t</mi> <mo>=</mo> <msubsup> <mo>&amp;Integral;</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>t</mi> <mi>k</mi> </msub> </msubsup> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <msup> <mi>v</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>d</mi> <mi>t</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msubsup> <mi>dV</mi> <mrow> <mi>r</mi> <mn>3</mn> <mi>k</mi> </mrow> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>=</mo> <mo>-</mo> <msubsup> <mo>&amp;Integral;</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>t</mi> <mi>k</mi> </msub> </msubsup> <msup> <mi>g</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>d</mi> <mi>t</mi> <mo>=</mo> <mo>-</mo> <msubsup> <mo>&amp;Integral;</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>t</mi> <mi>k</mi> </msub> </msubsup> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <msup> <mi>g</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>d</mi> <mi>t</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>
进一步，在tk-1至tk更新周期内，假设为常矢量，导航惯性系in内对地速度为线性函数，即
<mrow> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <mi>I</mi> <mo>+</mo> <mrow> <mo>(</mo> <mrow> <mi>t</mi> <mo>-</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mrow> <msubsup> <mover> <mi>&amp;omega;</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <msub> <mi>i</mi> <mi>n</mi> </msub> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> </mrow> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> </msubsup> <mo>&amp;times;</mo> </mrow> <mo>)</mo> </mrow> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msup> <mi>v</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msup> <mi>v</mi> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> </msup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>+</mo> <mfrac> <mrow> <msup> <mi>v</mi> <msub> <mi>n</mi> <msub> <mi>t</mi> <mi>k</mi> </msub> </msub> </msup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <msup> <mi>v</mi> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> </msup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> <mi>T</mi> </mfrac> <mrow> <mo>(</mo> <mrow> <mi>t</mi> <mo>-</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>
其中，t∈[tk-1，tk]；T＝tk-tk-1为GPS量测更新周期；
将式(9)、(10)代入式(7)、(8)中，整理得
<mrow> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>dV</mi> <mrow> <mi>r</mi> <mn>2</mn> <mi>k</mi> </mrow> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <mfrac> <mrow> <mi>I</mi> <mi>T</mi> </mrow> <mn>2</mn> </mfrac> <mo>+</mo> <mfrac> <msup> <mi>T</mi> <mn>2</mn> </msup> <mn>6</mn> </mfrac> <mrow> <mo>(</mo> <mrow> <msubsup> <mover> <mi>&amp;omega;</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <msub> <mi>i</mi> <mi>n</mi> </msub> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> </mrow> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> </msubsup> <mo>&amp;times;</mo> </mrow> <mo>)</mo> </mrow> </mrow> <mo>&amp;rsqb;</mo> </mrow> <msup> <mi>v</mi> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> </msup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>+</mo> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <mfrac> <mrow> <mi>I</mi> <mi>T</mi> </mrow> <mn>2</mn> </mfrac> <mo>+</mo> <mfrac> <msup> <mi>T</mi> <mn>2</mn> </msup> <mn>3</mn> </mfrac> <mrow> <mo>(</mo> <mrow> <msubsup> <mover> <mi>&amp;omega;</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <msub> <mi>i</mi> <mi>n</mi> </msub> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> </mrow> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> </msubsup> <mo>&amp;times;</mo> </mrow> <mo>)</mo> </mrow> </mrow> <mo>&amp;rsqb;</mo> </mrow> <msup> <mi>v</mi> <msub> <mi>n</mi> <msub> <mi>t</mi> <mi>k</mi> </msub> </msub> </msup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msubsup> <mi>dV</mi> <mrow> <mi>r</mi> <mn>3</mn> <mi>k</mi> </mrow> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>=</mo> <mo>-</mo> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <mi>I</mi> <mi>T</mi> <mo>+</mo> <mfrac> <msup> <mi>T</mi> <mn>2</mn> </msup> <mn>2</mn> </mfrac> <mrow> <mo>(</mo> <mrow> <msubsup> <mover> <mi>&amp;omega;</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <msub> <mi>i</mi> <mi>n</mi> </msub> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> </mrow> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> </msubsup> <mo>&amp;times;</mo> </mrow> <mo>)</mo> </mrow> </mrow> <mo>&amp;rsqb;</mo> </mrow> <msup> <mover> <mi>g</mi> <mo>&amp;OverBar;</mo> </mover> <msub> <mi>n</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>
其中，
利用捷联惯导姿态、速度二子样更新算法实现对式(4)中的求解；进一步，考虑陀螺仪随机常值漂移εb和加速度计随机常值零偏的影响，推导得
<mrow> <msubsup> <mover> <mi>V</mi> <mo>^</mo> </mover> <mi>m</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>V</mi> <mi>m</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>&amp;delta;V</mi> <mi>m</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <mi>&amp;delta;</mi> <msubsup> <mover> <mi>V</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>m</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>C</mi> <mo>^</mo> </mover> <mi>b</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <msup> <mo>&amp;dtri;</mo> <mi>b</mi> </msup> <mo>+</mo> <msup> <mover> <mi>f</mi> <mo>^</mo> </mover> <msub> <mi>i</mi> <mi>b</mi> </msub> </msup> <mo>&amp;times;</mo> <msubsup> <mi>&amp;phi;</mi> <mn>1</mn> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mo>,</mo> <msubsup> <mi>&amp;delta;V</mi> <mi>m</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>=</mo> <mn>0</mn> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msubsup> <mover> <mi>&amp;phi;</mi> <mo>&amp;CenterDot;</mo> </mover> <mn>1</mn> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mo>=</mo> <mo>-</mo> <msubsup> <mover> <mi>C</mi> <mo>^</mo> </mover> <mi>b</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <msup> <mi>&amp;epsiv;</mi> <mi>b</mi> </msup> <mo>,</mo> <msubsup> <mi>&amp;phi;</mi> <mn>1</mn> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>=</mo> <mn>0</mn> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>
其中，为姿态误差角；为加速度计惯性系比力积分误差；
由式(2)、(3)、(4)和式(13)可得，求解常值姿态阵的观测方程为
<mrow> <msubsup> <mover> <mi>V</mi> <mo>^</mo> </mover> <mi>m</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <msubsup> <mi>&amp;delta;V</mi> <mi>m</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <msubsup> <mi>V</mi> <mi>r</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow>
进一步，用经典Rodrigues参数法来等价描述姿态阵记对应Rodrigues参数为l，则二者满足凯莱变换关系式，即
<mrow> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mo>=</mo> <msup> <mrow> <mo>(</mo> <mrow> <mi>I</mi> <mo>+</mo> <mrow> <mo>(</mo> <mrow> <mi>l</mi> <mo>&amp;times;</mo> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <mrow> <mi>I</mi> <mo>-</mo> <mrow> <mo>(</mo> <mrow> <mi>l</mi> <mo>&amp;times;</mo> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow>
将式(17)代入式(16)，整理得，
<mrow> <msub> <mi>d</mi> <msub> <mi>t</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <msub> <mi>s</mi> <msub> <mi>t</mi> <mi>k</mi> </msub> </msub> <mo>&amp;times;</mo> <mi>l</mi> <mo>+</mo> <mi>l</mi> <mo>&amp;times;</mo> <msubsup> <mi>&amp;delta;V</mi> <mrow> <mi>m</mi> <mo>,</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> </mrow> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;delta;V</mi> <mrow> <mi>m</mi> <mo>,</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> </mrow> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mo>+</mo> <msub> <mi>w</mi> <mi>v</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow>
其中，wv包含惯性器件测量噪声的积分和随机扰动的积分，且有
<mrow> <msub> <mi>d</mi> <msub> <mi>t</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <msubsup> <mover> <mi>V</mi> <mo>^</mo> </mover> <mi>m</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <msubsup> <mi>V</mi> <mi>r</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>,</mo> <msub> <mi>s</mi> <msub> <mi>t</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <msubsup> <mover> <mi>V</mi> <mo>^</mo> </mover> <mi>m</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>V</mi> <mi>r</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>
式(18)即是与姿态阵等价的Rodrigues参数l的观测方程，若能估计出Rodrigues参数l，则依据式(17)得到
综上，惯性系动基座对准选取如下15维状态
<mrow> <mi>X</mi> <mo>=</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <msup> <mi>l</mi> <mi>T</mi> </msup> <msup> <mrow> <mo>(</mo> <mrow> <msubsup> <mi>&amp;delta;V</mi> <mi>m</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> </mrow> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>&amp;phi;</mi> <mn>1</mn> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msup> <mrow> <mo>(</mo> <msup> <mi>&amp;epsiv;</mi> <mi>b</mi> </msup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msup> <mrow> <mo>(</mo> <msup> <mo>&amp;dtri;</mo> <mi>b</mi> </msup> <mo>)</mo> </mrow> <mi>T</mi> </msup> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow>
由上述推导，系统方程及量测方程分别为，
<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mover> <mi>l</mi> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;delta;</mi> <msubsup> <mover> <mi>V</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>m</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <msubsup> <mover> <mi>&amp;phi;</mi> <mo>&amp;CenterDot;</mo> </mover> <mn>1</mn> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> </mtd> </mtr> <mtr> <mtd> <msup> <mover> <mi>&amp;epsiv;</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>b</mi> </msup> </mtd> </mtr> <mtr> <mtd> <msup> <mover> <mo>&amp;dtri;</mo> <mo>&amp;CenterDot;</mo> </mover> <mi>b</mi> </msup> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msup> <mover> <mi>f</mi> <mo>^</mo> </mover> <msub> <mi>i</mi> <mi>b</mi> </msub> </msup> <mo>&amp;times;</mo> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msubsup> <mover> <mi>C</mi> <mo>^</mo> </mover> <mi>b</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mover> <mi>C</mi> <mo>^</mo> </mover> <mi>b</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mi>l</mi> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;delta;V</mi> <mi>m</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>&amp;phi;</mi> <mn>1</mn> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> </mtd> </mtr> <mtr> <mtd> <msup> <mi>&amp;epsiv;</mi> <mi>b</mi> </msup> </mtd> </mtr> <mtr> <mtd> <msup> <mo>&amp;dtri;</mo> <mi>b</mi> </msup> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mover> <mi>C</mi> <mo>^</mo> </mover> <mi>b</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <msub> <mi>w</mi> <mi>a</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msubsup> <mover> <mi>C</mi> <mo>^</mo> </mover> <mi>b</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <msub> <mi>w</mi> <mi>g</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>21</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>d</mi> <msub> <mi>t</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <mi>h</mi> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>w</mi> <mi>v</mi> </msub> <mo>=</mo> <msub> <mi>s</mi> <msub> <mi>t</mi> <mi>k</mi> </msub> </msub> <mo>&amp;times;</mo> <mi>l</mi> <mo>+</mo> <mrow> <mo>(</mo> <mrow> <mi>I</mi> <mo>+</mo> <mi>l</mi> <mo>&amp;times;</mo> </mrow> <mo>)</mo> </mrow> <msubsup> <mi>&amp;delta;V</mi> <mi>m</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mo>+</mo> <msub> <mi>w</mi> <mi>v</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>22</mn> <mo>)</mo> </mrow> </mrow>
利用式(21)、式(22)设计滤波算法实现对Rodrigues参数l的估计，进而得到姿态阵通过式(1)即实现动基座对准；
步骤2，二阶非线性量测滤波估计算法
式(21)描述的系统方程为线性，式(22)描述的量测方程为非线性，但仅是状态量的二阶非线性函数，能用有限阶Taylor级数展开描述，即
<mrow> <mi>h</mi> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mi>h</mi> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mrow> <mi>k</mi> <mn>0</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> <mo>+</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mn>3</mn> </munderover> <msub> <mi>e</mi> <mi>i</mi> </msub> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>D</mi> <mi>i</mi> </msub> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> <msubsup> <mover> <mi>X</mi> <mo>~</mo> </mover> <mi>k</mi> <mi>T</mi> </msubsup> </mrow> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>23</mn> <mo>)</mo> </mrow> </mrow>
其中，Xk0为Taylor级数展开点；ei是第i个分量为1，其余元素为0的3维单位向量；Hk为非线性函数h的雅克比阵；Di为非线性函数h的二阶偏导数阵；Tr为矩阵求迹函数，且有
<mrow> <msub> <mi>H</mi> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mrow> <mo>&amp;part;</mo> <mi>h</mi> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mi>X</mi> <mi>k</mi> </msub> </mrow> </mfrac> <msub> <mo>|</mo> <mrow> <msub> <mi>X</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>X</mi> <mrow> <mi>k</mi> <mn>0</mn> </mrow> </msub> </mrow> </msub> <mo>=</mo> <mrow> <mo>&amp;lsqb;</mo> <mrow> <mrow> <mo>(</mo> <mrow> <msub> <mi>s</mi> <msub> <mi>t</mi> <mi>k</mi> </msub> </msub> <mo>-</mo> <msubsup> <mi>&amp;delta;V</mi> <mi>m</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> </mrow> <mo>)</mo> </mrow> <mo>&amp;times;</mo> <mrow> <mo>(</mo> <mrow> <mi>I</mi> <mo>+</mo> <mi>l</mi> <mo>&amp;times;</mo> </mrow> <mo>)</mo> </mrow> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>9</mn> </mrow> </msub> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>24</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>D</mi> <mn>1</mn> </msub> <mo>=</mo> <mo>&amp;dtri;</mo> <msup> <mo>&amp;dtri;</mo> <mi>T</mi> </msup> <msub> <mi>h</mi> <mn>1</mn> </msub> <mo>,</mo> <msub> <mi>D</mi> <mn>2</mn> </msub> <mo>=</mo> <mo>&amp;dtri;</mo> <msup> <mo>&amp;dtri;</mo> <mi>T</mi> </msup> <msub> <mi>h</mi> <mn>2</mn> </msub> <mo>,</mo> <msub> <mi>D</mi> <mn>3</mn> </msub> <mo>=</mo> <mo>&amp;dtri;</mo> <msup> <mo>&amp;dtri;</mo> <mi>T</mi> </msup> <msub> <mi>h</mi> <mn>3</mn> </msub> </mrow>
<mrow> <mo>&amp;dtri;</mo> <msup> <mo>&amp;dtri;</mo> <mi>T</mi> </msup> <msub> <mi>h</mi> <mi>i</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>h</mi> <mi>i</mi> </msub> </mrow> <mrow> <mo>&amp;part;</mo> <msubsup> <mi>X</mi> <mn>1</mn> <mn>2</mn> </msubsup> </mrow> </mfrac> </mtd> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>h</mi> <mi>i</mi> </msub> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mi>X</mi> <mn>1</mn> </msub> <mo>&amp;part;</mo> <msub> <mi>X</mi> <mn>2</mn> </msub> </mrow> </mfrac> </mtd> <mtd> <mo>...</mo> </mtd> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>h</mi> <mi>i</mi> </msub> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mi>X</mi> <mn>1</mn> </msub> <mo>&amp;part;</mo> <msub> <mi>X</mi> <mn>15</mn> </msub> </mrow> </mfrac> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>h</mi> <mi>i</mi> </msub> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mi>X</mi> <mn>2</mn> </msub> <mo>&amp;part;</mo> <msub> <mi>X</mi> <mn>1</mn> </msub> </mrow> </mfrac> </mtd> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>h</mi> <mi>i</mi> </msub> </mrow> <mrow> <mo>&amp;part;</mo> <msubsup> <mi>X</mi> <mn>2</mn> <mn>2</mn> </msubsup> </mrow> </mfrac> </mtd> <mtd> <mo>...</mo> </mtd> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>h</mi> <mi>i</mi> </msub> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mi>X</mi> <mn>2</mn> </msub> <mo>&amp;part;</mo> <msub> <mi>X</mi> <mn>15</mn> </msub> </mrow> </mfrac> </mtd> </mtr> <mtr> <mtd> <mtable> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> </mtable> </mtd> <mtd> <mtable> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> </mtable> </mtd> <mtd> <mtable> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> </mtable> </mtd> <mtd> <mtable> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> </mtable> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>h</mi> <mi>i</mi> </msub> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mi>X</mi> <mn>15</mn> </msub> <mo>&amp;part;</mo> <msub> <mi>X</mi> <mn>1</mn> </msub> </mrow> </mfrac> </mtd> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>h</mi> <mi>i</mi> </msub> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mi>X</mi> <mn>15</mn> </msub> <mo>&amp;part;</mo> <msub> <mi>X</mi> <mn>2</mn> </msub> </mrow> </mfrac> </mtd> <mtd> <mo>...</mo> </mtd> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>h</mi> <mi>i</mi> </msub> </mrow> <mrow> <mo>&amp;part;</mo> <msubsup> <mi>X</mi> <mn>15</mn> <mn>2</mn> </msubsup> </mrow> </mfrac> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>25</mn> <mo>)</mo> </mrow> </mrow>
其中，h＝[h1 h2 h3]T；
同时，对于二阶非线性函数，其二阶偏导数阵为常值矩阵，故由式(21)、(22)、(25)，知Di为15维常值对称阵，其中非零元素仅有
D1(2，6)＝1，D2(1，6)＝-1，D3(1，5)＝1，
D1(3，5)＝-1，D2(3，4)＝1，D3(2，4)＝-1
D1(6，2)＝1，D2(6，1)＝-1，D3(5，1)＝1，
D1(5，3)＝-1，D2(4，3)＝1，D3(4，2)＝-1 (26)
步骤2.1，滤波时间更新算法
式(21)系统方程为线性，采用标准卡尔曼滤波算法完成时间更新，得到状态量和估计误差方差阵的一步预测，即和Pk/k-1；用状态一步预测结果代替式(23)中Xk0，建立起当前观测量与状态一步预测关系式，进而设计量测更新算法对一步预测结果进行校正，得到当前时刻的状态最优估计值下面推导基于式(23)二阶泰勒级数量测方程的滤波量测更新算法；
步骤2.2，滤波量测更新算法
量测更新形式定义为与线性卡尔曼滤波量测更新一致，假设k时刻状态估计结果为
<mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>&amp;lsqb;</mo> <mrow> <msub> <mi>d</mi> <msub> <mi>t</mi> <mi>k</mi> </msub> </msub> <mo>-</mo> <mi>h</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>L</mi> <mi>k</mi> </msub> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>27</mn> <mo>)</mo> </mrow> </mrow>
其中，Lk为引入的补偿项，和最佳增益Kk一样均为待定值；Lk和Kk的确定原则分别为使为无偏估计和使的均方误差阵Pk的迹最小；
定义状态估计误差
<mrow> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>X</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mrow> <mo>(</mo> <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>-</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>28</mn> <mo>)</mo> </mrow> </mrow>
其中，
由式(23)、式(24)、式(27)、式(28)整理得
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>w</mi> <mi>v</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>&amp;lsqb;</mo> <mrow> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mn>3</mn> </munderover> <msub> <mi>e</mi> <mi>i</mi> </msub> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>D</mi> <mi>i</mi> </msub> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> </mrow> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>L</mi> <mi>k</mi> </msub> </mrow> <mo>&amp;rsqb;</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>29</mn> <mo>)</mo> </mrow> </mrow>
要使为无偏估计，即要求期望为零；假设时间更新为无偏估计，则对式(29)右端取期望并令结果为零，得补偿项Lk为
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>L</mi> <mi>k</mi> </msub> <mo>=</mo> <mi>E</mi> <mrow> <mo>&amp;lsqb;</mo> <mrow> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mn>3</mn> </munderover> <msub> <mi>e</mi> <mi>i</mi> </msub> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>D</mi> <mi>i</mi> </msub> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> </mrow> <mo>)</mo> </mrow> </mrow> <mo>&amp;rsqb;</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mn>3</mn> </munderover> <msub> <mi>e</mi> <mi>i</mi> </msub> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>D</mi> <mi>i</mi> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>30</mn> <mo>)</mo> </mrow> </mrow>
其中，E[·]表示对括号内变量求期望；
将式(30)代入式(29)中，整理得
<mrow> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <mrow> <mo>(</mo> <mrow> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>H</mi> <mi>k</mi> </msub> </mrow> <mo>)</mo> </mrow> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mi>A</mi> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>w</mi> <mi>v</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>31</mn> <mo>)</mo> </mrow> </mrow>
其中，
<mrow> <mi>A</mi> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mn>3</mn> </munderover> <msub> <mi>e</mi> <mi>i</mi> </msub> <mi>T</mi> <mi>r</mi> <mrow> <mo>&amp;lsqb;</mo> <mrow> <msub> <mi>D</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> </mrow> <mo>)</mo> </mrow> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>32</mn> <mo>)</mo> </mrow> </mrow>
由于E[wv]＝0，且wv与A不相关，故由式(31)、式(32)得Pk为
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>=</mo> <mi>E</mi> <mrow> <mo>&amp;lsqb;</mo> <mrow> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> <msubsup> <mover> <mi>X</mi> <mo>~</mo> </mover> <mi>k</mi> <mi>T</mi> </msubsup> </mrow> <mo>&amp;rsqb;</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mrow> <mo>(</mo> <mrow> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>H</mi> <mi>k</mi> </msub> </mrow> <mo>)</mo> </mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mrow> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>H</mi> <mi>k</mi> </msub> </mrow> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <mrow> <msub> <mi>R</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>&amp;Lambda;</mi> <mi>k</mi> </msub> </mrow> <mo>)</mo> </mrow> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>33</mn> <mo>)</mo> </mrow> </mrow>
其中
Λk＝E[AAT] (34)
式(32)中，A为3维列向量，从而Λk为3阶方阵；利用式(32)，经过推导得Λk第i行第j列元素为
<mrow> <msub> <mi>&amp;Lambda;</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mi>T</mi> <mi>r</mi> <mrow> <mo>&amp;lsqb;</mo> <mrow> <mrow> <mo>(</mo> <mrow> <msub> <mi>D</mi> <mi>i</mi> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mrow> <msub> <mi>D</mi> <mi>j</mi> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> <mo>)</mo> </mrow> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>35</mn> <mo>)</mo> </mrow> </mrow>
式(33)中协方差阵更新方式与标准卡尔曼滤波形式一致，从而式(27)中最佳增益阵Kk，
考察量测更新方程式(27)、(33)、(36)，基于二阶泰勒级数的量测更新算法与标准卡尔曼滤波在形式上完全一致，仅增加了对Lk、Λk的计算；而由式(30)、(35)知，Lk、Λk的求解简单；考虑到Di为常值稀疏矩阵，将Lk、Λk描述为仅与Pk/k-1相关的形式，如此进一步减小在线计算量；
步骤3，经典Rodrigues参数奇异点及其处理方法
经典Rodrigues参数是最少参数姿态描述方法之一，存在奇异点，一种等价表示方法为
<mrow> <mi>l</mi> <mo>=</mo> <mi>u</mi> <mi> </mi> <mi>tan</mi> <mfrac> <mi>&amp;phi;</mi> <mn>2</mn> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>37</mn> <mo>)</mo> </mrow> </mrow>
其中，u为两坐标系之间等效旋转矢量的方向向量；φ为等效旋转矢量转过的角度；
当φ取值为±π时，l值为无穷大，是经典Rodrigues参数法进行姿态描述时的奇异点，对ib系进行某种虚拟转动得到ib1，使得ib1系相对于in系的Rodrigues参数远离奇异点，然后采用前述滤波对准算法完成对应Rodrigues参数的估计，进而借助已知的虚拟转动，计算
2.根据权利要求1所述的基于罗德里格参数和二阶非线性量测的滤波对准算法，其特征在于，所述步骤3中具体包括如下步骤：
步骤3.1，奇异点判别函数设计
当φ取值为±π时，ib系至in系姿态矩阵描述为
<mrow> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>=</mo> <mi>I</mi> <mo>+</mo> <mi>sin</mi> <mi>&amp;phi;</mi> <mo>&amp;CenterDot;</mo> <mrow> <mo>(</mo> <mrow> <mi>u</mi> <mo>&amp;times;</mo> </mrow> <mo>)</mo> </mrow> <mo>+</mo> <mrow> <mo>(</mo> <mrow> <mn>1</mn> <mo>-</mo> <mi>cos</mi> <mi>&amp;phi;</mi> </mrow> <mo>)</mo> </mrow> <mo>&amp;CenterDot;</mo> <msup> <mrow> <mo>(</mo> <mrow> <mi>u</mi> <mo>&amp;times;</mo> </mrow> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>=</mo> <mi>I</mi> <mo>+</mo> <mn>2</mn> <mo>&amp;CenterDot;</mo> <msup> <mrow> <mo>(</mo> <mrow> <mi>u</mi> <mo>&amp;times;</mo> </mrow> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>38</mn> <mo>)</mo> </mrow> </mrow>
此时，任意矢量R在两坐标系内投影满足
<mrow> <msup> <mi>R</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msup> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <msup> <mi>R</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msup> <mo>=</mo> <mrow> <mo>&amp;lsqb;</mo> <mrow> <mi>I</mi> <mo>+</mo> <mn>2</mn> <mo>&amp;CenterDot;</mo> <msup> <mrow> <mo>(</mo> <mrow> <mi>u</mi> <mo>&amp;times;</mo> </mrow> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> <mo>&amp;rsqb;</mo> </mrow> <msup> <mi>R</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>39</mn> <mo>)</mo> </mrow> </mrow>
在式(39)两端分别加上并利用单位向量恒等关系式I+(u×)2＝uuT，则有
<mrow> <msup> <mi>R</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msup> <mo>+</mo> <msup> <mi>R</mi> <msub> <mi>i</mi> <mi>n</mi> </msub> </msup> <mo>=</mo> <msub> <mi>S</mi> <mi>R</mi> </msub> <mo>=</mo> <mn>2</mn> <mrow> <mo>(</mo> <mrow> <msup> <mi>u</mi> <mi>T</mi> </msup> <msup> <mi>R</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> </msup> </mrow> <mo>)</mo> </mrow> <mo>&amp;CenterDot;</mo> <mi>u</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>40</mn> <mo>)</mo> </mrow> </mrow>
也即是，此时任意矢量在两坐标系内的投影之和均共线，且与等效旋转矢量平行；
因此，对当前滤波变量l是否接近奇异点的判断等价于判断式(19)中不同时刻的是否接近共线；为了缩短得出有效判断所需时间，设定如下判别函数
<mrow> <msub> <mi>g</mi> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>k</mi> </mfrac> <mo>|</mo> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>k</mi> </munderover> <msup> <mrow> <mo>(</mo> <mrow> <msub> <mover> <mi>s</mi> <mo>&amp;OverBar;</mo> </mover> <msub> <mi>t</mi> <mi>i</mi> </msub> </msub> <mo>&amp;times;</mo> </mrow> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>|</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>41</mn> <mo>)</mo> </mrow> </mrow>
其中，为的方向向量；|·|表示取方阵行列式；
若用于计算gk的所有均共线，即当前对变量l的估计位于奇异点时，gk理论值为零；
步骤3.2，虚拟转动设计
虚拟转动的设计依据：若ib系至in系等效旋转矢量转角大于π/2，将ib系绕着其某个坐标轴转动π角得到ib1系，使ib1系至in系等效旋转矢量转角小于π/2；
据此，记ib系绕其x轴转动π角得到ibx系；ib系绕其y轴转动π角得到iby系；ib系绕其z轴转动π角得到ibz系，则有
<mrow> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mrow> <mi>b</mi> <mi>x</mi> </mrow> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mrow> <mi>b</mi> <mi>x</mi> </mrow> </msub> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mo>,</mo> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mo>,</mo> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mrow> <mi>b</mi> <mi>z</mi> </mrow> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mi>b</mi> </msub> <msub> <mi>i</mi> <mi>n</mi> </msub> </msubsup> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <mrow> <mi>b</mi> <mi>z</mi> </mrow> </msub> <msub> <mi>i</mi> <mi>b</mi> </msub> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>42</mn> <mo>)</mo> </mrow> </mrow>
其中，为已知常值对角阵，对角元为1或-1；
无论载体动基座对准开始时刻真实姿态为何值，姿态阵所对应的四个等效旋转矢量中，至少有一个等效旋转矢量的转角小于π/2，远离奇异点；
步骤3.3，奇异点规避的滤波算法
依据上面设计的判别函数和虚拟转动，得到奇异点规避的滤波算法：
(1)给定奇异点判别时间tk，在tk时刻以前，利用步骤1、步骤2推导的对准滤波算法，同时处理四个独立的滤波过程，分别对进行估计；其中，对 进行估计时，式(21)、式(22)中相关参变量需要进行相应的坐标变换；所有滤波器状态量初值均设为零，考虑式(27)及虚拟转动分析结论，将l对应的方差阵初值设定为P00＝1，P11＝1，P22＝1；
(2)在tk时刻，分别计算姿态阵所对应的判别函数值，gk、gkx、gky、gkz，仅选择最大判别函数值所对应的滤波过程完成后续滤波对准。
CN201510297409.9A 2015-05-29 2015-05-29 一种基于罗德里格参数和二阶非线性量测的滤波对准算法 Active CN104897170B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510297409.9A CN104897170B (zh) 2015-05-29 2015-05-29 一种基于罗德里格参数和二阶非线性量测的滤波对准算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510297409.9A CN104897170B (zh) 2015-05-29 2015-05-29 一种基于罗德里格参数和二阶非线性量测的滤波对准算法

Publications (2)

Publication Number Publication Date
CN104897170A CN104897170A (zh) 2015-09-09
CN104897170B true CN104897170B (zh) 2018-01-16

Family

ID=54029982

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510297409.9A Active CN104897170B (zh) 2015-05-29 2015-05-29 一种基于罗德里格参数和二阶非线性量测的滤波对准算法

Country Status (1)

Country Link
CN (1) CN104897170B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108981696B (zh) * 2018-08-01 2022-02-18 西北工业大学 一种sins任意失准角无奇异快速传递对准方法
CN112284379B (zh) * 2020-09-17 2023-09-22 江苏大学 一种基于非线性积分补偿的组合运动测量系统的惯性预积分方法
CN113532479B (zh) * 2021-08-01 2024-03-29 北京工业大学 基于李群乘性卡尔曼滤波的捷联惯导系统运动初始对准方法

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102980580A (zh) * 2012-11-16 2013-03-20 北京理工大学 基于张量积多胞鲁棒h2滤波的无陀螺卫星姿态确定方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102980580A (zh) * 2012-11-16 2013-03-20 北京理工大学 基于张量积多胞鲁棒h2滤波的无陀螺卫星姿态确定方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
一种适用于SINS动基座初始对准的新算法;严恭敏等;《系统工程与电子技术》;20090331;第31卷(第3期);634-637 *
车载导航系统动态高精度初始对准技术;严恭敏等;《系统工程与电子技术》;20060930;第28卷(第9期);1404-1407 *

Also Published As

Publication number Publication date
CN104897170A (zh) 2015-09-09

Similar Documents

Publication Publication Date Title
Heo et al. Consistent EKF-based visual-inertial odometry on matrix Lie group
Chang et al. Optimization-based alignment for strapdown inertial navigation system: Comparison and extension
Chang et al. Initial alignment by attitude estimation for strapdown inertial navigation systems
Hasberg et al. Simultaneous localization and mapping for path-constrained motion
CN105865452B (zh) 一种基于间接卡尔曼滤波的移动平台位姿估计方法
CN105806363B (zh) 基于srqkf的sins/dvl水下大失准角对准方法
CN103900574B (zh) 一种基于迭代容积卡尔曼滤波姿态估计方法
CN105737823A (zh) 基于五阶ckf的gps/sins/cns组合导航方法
CN109931955B (zh) 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN102353378B (zh) 一种矢量形式信息分配系数的组合导航系统自适应联邦滤波方法
Crocoll et al. Model‐aided navigation for a quadrotor helicopter: A novel navigation system and first experimental results
CN108318038A (zh) 一种四元数高斯粒子滤波移动机器人姿态解算方法
CN106052716A (zh) 惯性系下基于星光信息辅助的陀螺误差在线标定方法
CN101216321A (zh) 捷联惯性导航系统的快速精对准方法
CN107831660A (zh) 微陀螺仪自适应高阶超扭曲滑模控制方法
CN106500693A (zh) 一种基于自适应扩展卡尔曼滤波的ahrs算法
CN101706287A (zh) 一种基于数字高通滤波的旋转捷联系统现场标定方法
CN107703757A (zh) 微陀螺仪系统的超扭曲滑模控制方法
CN106940193A (zh) 一种基于Kalman滤波的船舶自适应摇摆标定方法
CN104897170B (zh) 一种基于罗德里格参数和二阶非线性量测的滤波对准算法
CN106153073A (zh) 一种全姿态捷联惯导系统的非线性初始对准方法
CN107807527A (zh) 微陀螺仪增益可调的自适应超扭曲滑模控制方法
CN104613966B (zh) 一种地籍测量事后数据处理方法
CN105004351A (zh) 基于自适应upf的sins大方位失准角初始对准方法
Sun et al. A robust indirect Kalman filter based on the gradient descent algorithm for attitude estimation during dynamic conditions

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20180802

Address after: 611731 3 building, 1 New Route 4, hi-tech West District, Chengdu, Sichuan.

Patentee after: Sichuan Hanghao Technology Co.,Ltd.

Address before: 710072 automation school, Northwestern Polytechnical University, 127 Youyi Road, Xi'an, Shaanxi

Patentee before: Northwestern Polytechnical University

PE01 Entry into force of the registration of the contract for pledge of patent right
PE01 Entry into force of the registration of the contract for pledge of patent right

Denomination of invention: A filtering alignment algorithm based on Rodrigues parameters and second-order nonlinear measurements

Effective date of registration: 20231227

Granted publication date: 20180116

Pledgee: Bank of China Limited Chengdu Chenghua sub branch

Pledgor: Sichuan Hanghao Technology Co.,Ltd.

Registration number: Y2023980074988