一种基于罗德里格参数和二阶非线性量测的滤波对准算法
【技术领域】
[0001] 本发明涉及惯性导航技术领域，具体是一种基于罗德里格参数和二阶非线性量测 的滤波对准算法。
【背景技术】
[0002] 动基座对准能够有效提高捷联惯导系统载体平台的机动性能，具有很高的军事应 用价值。动基座对准的研宄内容主要包括两个方面，一是建立大失准角条件下的非线性误 差模型 [1~3];二是设计相应的非线性滤波估计算法[2~9]。
[0003] 依据姿态描述方式的不同，可以得到不同的非线性误差模型，如基于四元数非线 性误差模型 [1]、基于欧拉角非线性误差模型[2]、基于修正Rodrigues参数非线性误差模型
[3] ，以及大方位失准角条件下，基于方位角正余弦函数的非线性误差模型[4]等。其中，四 元数非线性误差模型无奇异点，使用最为广泛，但在设计滤波算法时需要考虑其模值约束 的影响。欧拉角姿态描述法存在奇异点，因此不适用于任意姿态对准，且基于欧拉角的非 线性误差模型中含有状态量的正余弦函数，使得误差模型非线性增大。传统的基于修正 Rodrigues参数的误差模型虽可避免奇异点，但是模型非线性度同样很大。此外，传统方法 建立动基座对准非线性误差模型时，均以动态载体系和动态导航系之间的实时姿态为估计 对象，系统方程及量测方程均建立在动态的导航坐标系下。以速度为观测量时，以上误差模 型只有系统方程为非线性，量测方程则是线性的。
[0004] 在非线性滤波算法的选择上，常规EKF滤波算法需要求导计算Jacobian矩阵，且 在处理严重非线性问题时，可能出现滤波误差增大甚至发散的现象。因此，一类基于sigma 点的非线性滤波算法成为研宄的热点，如UKF滤波[2'3'4]、改进强跟踪UKF滤波[5]、粒子滤波 [6]、Gauss-Hermite滤波[7'8]，以及容积卡尔曼滤波（CKF)[9]等。基于sigma点的非线性滤 波算法，用确定性或随机采样策略逼近非线性函数的概率分布，无需对非线性模型求导，且 可以通过优化采样策略，减小计算量并提高非线性函数概率分布的近似精度。总体来讲，基 于sigma点的非线性滤波算法可以做到计算量与EKF滤波相当，但是精度优于EKF滤波。
【发明内容】
[0005] 本发明的目的在于提供一种基于罗德里格参数和二阶非线性量测的滤波对准算 法，以解决上述【背景技术】中提出的问题。
[0006] 为实现上述目的，本发明提供如下技术方案：
[0007] 一种基于罗德里格参数和二阶非线性量测的滤波对准算法，由惯性系动基座对准 过程、二阶非线性量测滤波估计算法、经典Rodrigues参数奇异点及其处理方法，
[0008] 具体描述如下：
[0009] 步骤1，惯性系动基座对准过程
[0010] 惯性系动基座对准算法以实时姿态阵的链式分解为基础，
[0012] 其中，nt系为实时导航坐标系，即载体时变位置东北天地理坐标系；in为导航惯性 系，与动基座对准开始时刻的n系重合；b为载体坐标系；ib为载体惯性系，与对准开始时刻 的b系重合；式（1)中，是运动的nt系相对于导航惯性系i^勺姿态阵，由GPS输出位置 信息解析计算；能利用陀螺输出进行姿态跟踪，所以，惯性系动基座对准过程是对常值姿 态阵ct的估计；
[0013] 利用牛顿第二定律和哥氏定理，得到惯性系比力方程如下
[0015] 其中，P⑴为t时刻载体对地速度在导航惯性系in内的投影；#(0为t时刻载体 所在位置重力加速度在导航惯性系in内投影；_T'(〇为t时刻理想比力值；
[0016] 对式（2)两端分别进行积分，并记
[0019] 利用GPS输出完成式⑶中"仏)的求解，
[0024] 进一步，在tk_^tk更新周期内，假设，为常矢量，导航惯性系in内对地速度 为线性函数，即
[0027] 其中，tG[tH，tk];T=VtH为GPS量测更新周期
[0028] 将式（9)、（10)代入式（7)、（8)中，整理得
[0032] 利用捷联惯导姿态、速度二子样更新算法实现对式（4)中)^化)的求解；进一步， 考虑陀螺仪随机常值漂移eb和加速度计随机常值零偏V6的影响，推导得
[0036] 其中，#为姿态误差角；汾。仏)为加速度计惯性系比力积分误差；
[0037]由式（2)、（3)、⑷和式（13)可得，求解常值姿态阵q的观测方程为
[0039] 进一步，用经典Rodrigues参数法来等价描述姿态阵，记对应Rodrigues参数 为1，则二者满足凯莱变换关系式，即
[0041] 将式（17)代入式（16)，整理得，
[0043] 其中，#^ 匕）；wv包含惯性器件测量噪声的积分和随机扰动的积分，且有
[0045] 式（18)即是与姿态阵q等价的Rodrigues参数1的观测方程，若能估计出 Rodrigues参数1，则依据式（17)得到<^ . ?
[0046] 综上，惯性系动基座对准选取如下15维状态
[0048] 由上述推导，系统方程及量测方程分别为，
[0051] 利用式（21)、式（22)设计滤波算法实现对Rodrigues参数1的估计，进而得到姿 态阵q,通过式（1)即实现动基座对准；
[0052] 步骤2,二阶非线性量测滤波估计算法
[0053] 式（21)描述的系统方程为线性，式（22)描述的量测方程为非线性，但仅是状态量 的二阶非线性函数，能用有限阶Taylor级数展开描述，即
[0055] 其中，Xk(l为Taylor级数展开点；
ei是第i个分量为1，其余元素为 0的3维单位向量；Hk为非线性函数h的雅克比阵；Di为非线性函数h的二阶偏导数阵；Tr为矩阵求迹函数，且有
[0059] 其中，h=IXh2h3]T;
[0060] 同时，对于二阶非线性函数，其二阶偏导数阵为常值矩阵，故由式（21)、（22)、 (25)，知DiS15维常值对称阵，其中非零元素仅有
[0061] Di(2,6) = 1，D2(l，6) = -1，D3(l，5) = 1，
[0062] Di(3,5) = -1，D2(3,4) = 1，D3(2,4) = -1
[0063] Di(6,2) = 1，D2(6,1) =-1，D3(5,1) = 1，
[0064] Di(5,3) =-1，D2(4,3) = 1，D3(4,2) =-1 (26)
[0065] 步骤2. 1，滤波时间更新算法
[0066] 式（21)系统方程为线性，采用标准卡尔曼滤波算法完成时间更新，得到状态量和 估计误差方差阵的一步预测，即和用状态一步预测结果;代替式（23)中Xk(l， 建立起当前观测量与状态一步预测关系式，进而设计量测更新算法对一步预测结果 进行校正，得到当前时刻的状态最优估计值下面推导基于式（23)二阶泰勒级数量测方 ? 程的滤波量测更新算法；
[0067] 步骤2. 2,滤波量测更新算法
[0068] 量测更新形式定义为与线性卡尔曼滤波量测更新一致，假设k时刻状态估计结果 为
[0070] 其中，Lk为引入的补偿项，和最佳增益Kk一样均为待定值；Lk和Kk的确定原则分 别为使为无偏估计和使的均方误差阵Pk的迹最小；
[0071] 定义状态估计误差
[0074]由式（23)、式（24)、式（27)、式（28)整理得
[0076] 要使&为无偏估计，即要求&期望为零；假设时间更新为无偏估计，则对式 (29)右端取期望并令结果为零，得补偿项Lk为
[0078] 其中，E[ ?]表示对括号内变量求期望；
[0079] 将式（30)代入式（29)中，整理得
[0081]其中，
[0083]由于E[足]=〇，E[wv]= 0,且^与A不相关，故由式（31)、式（32)得Pk为
[0085] 其中
[0086]Ak=E[AAt] (34)
[0087] 式（32)中，A为3维列向量，从而Ak为3阶方阵；利用式（32)，经过推导得Ak第 i行第j列元素为
[0089] 式（33)中协方差阵更新方式与标准卡尔曼滤波形式一致，从而式（27)中最佳增 益阵Kk，
[0090] 考察量测更新方程式（27)、（33)、（36)，基于二阶泰勒级数的量测更新算法与标准 卡尔曼滤波在形式上完全一致，仅增加了对Lk、Ak的计算；而由式（30)、（35)知，Lk、Ak的 求解简单；考虑到DiS常值稀疏矩阵，将Lk、Ak描述为仅与Pk/H相关的形式，如此进一步 减小在线计算量；
[0091] 步骤3,经典Rodrigues参数奇异点及其处理方法
[0092] 经典Rodrigues参数是最少参数姿态描述方法之一，存在奇异点，一种等价表示 方法为
[0094] 其中，u为两坐标系之