CN105222780B - 一种基于Stirling插值多项式逼近的椭球集员滤波方法 - Google Patents

Abstract

本发明公开了一种基于Stirling插值多项式逼近的椭球集员滤波方法，首先利用Stirling插值多项式获得非线性状态方程的线性化逼近，构造***状态变量的不确定区间，利用二阶差分算子获取线性化误差边界，构造虚拟噪声外包椭球，利用当前状态变量估计值预测下一时刻的***状态变量预测误差边界，根据观测向量开展***状态变量的更新操作，再利用线性化椭球集员滤波步骤开展***状态变量的估计计算以及***状态变量的估计误差椭球的计算，从而完成***状态变量的估计计算任务。本发明利用Stirling插值多项式逼近计算实现椭球集员滤波方法，有效减小了计算量，提高计算效率，改善了扩展集员滤波算法的计算精度。

Description

一种基于Stirling插值多项式逼近的椭球集员滤波方法

技术领域

本发明属于航空航天***处理的导航制导与控制的技术领域，具体涉及一种基于Stirling插值多项式逼近的扩展椭球集员滤波方法，可以应用于惯性导航***。

背景技术

传统的随机概率滤波方法一般要求已知过程噪声和观测噪声的统计特性，或者假设其满足一定的分布条件，而实际的非线性***中***状态或者参数的统计特性往往是未知的，因此，常规的随机概率滤波算法的应用有很大的局限性。集员滤波算法仅仅要求噪声的有界性，不需要精确获得噪声的统计特性，这一点在实际***中通常是能够得到保证的，并且在集员滤波计算框架下得到的状态参数估计结果是一个可行解集合，而不是常规滤波计算获得的单个估计值。从控制角度来说，集员滤波方法提供了鲁棒控制和最优控制等理论所要求的状态参数边界，可更好地实现滤波方法与控制策略结合。

考虑到可行状态参数集合形状一般无法准确确定，甚至是非凸的，集员滤波方法在形式上大多采用椭球定界算法。Schweppe和Bertsekas首先提出了可以利用外定界椭球集合来包含***的真实状态，但是没有考虑椭球的最优化问题。在此基础上，Fogel和Huang给出了最优化定界椭球算法，得到了最小体积和最小迹椭球集合；Maksarov、Kurzhanski和Chernousko等人进一步发展了针对状态和参数估计计算的椭球计算技术；并且Lin针对特定应用情况提出了一种自适应的边界估计计算的椭球算法；Polyak推倒了用于具有模型不确定性***的椭球算法，进一步扩展了椭球集员滤波算法的应用领域。

但是，上述这些算法都是应用于线性***的，Scholte和Campell将椭球定界算法推广到非线性***提出了一种扩展集员滤波算法，其主要思想是首先对非线性***线性化处理，并采用区间分析技术估计线性化近似后的高阶项误差范围，将其用椭球外包后与噪声椭球集合实施直和计算组成虚拟噪声椭球集合，然后对得到的线性化***实施线性椭球集员滤波计算，最终得到非线性***状态参数的估计计算结果。

但是，基于Taylor级数线性化处理得到的扩展集员滤波算法存在着很大的缺陷，首先当***非线性比较强时，围绕***状态参数预测估计或者状态参数预估值的一阶Taylor级数展开式往往存在着很大的截断误差，使得该算法存在着数值计算稳定性变差，计算复杂，甚至出现滤波算法发散的现象；再者一阶Taylor级数扩展需要计算Jacobi矩阵，二阶Taylor级数扩展需要计算复杂的Hessian矩阵，计算量巨大，对处理器要求很高，难以满足导航***快速初始对准的要求。

发明内容

为了解决现有技术在捷联掼导***(Strapdown inertial Navigation System，SINS)初始对准状态参数计算过程中，基于Taylor级数线性近似的扩展椭球集员滤波算法的计算复杂，效率低下，精算精度不能满足***要求的问题，本发明提出了一种基于Stirling插值多项式逼近的椭球集员滤波方法，有效地减小了计算量，提高了***状态参数估计的计算效率，并且能够有效地改善扩展集员滤波方法的计算精度。

为了达到上述目的，本发明的技术方案是：一种基于Stirling插值多项式逼近的椭球集员滤波方法，其步骤如下：

步骤一：建立组合导航***的非线性误差状态方程和观测方程；

步骤二：计算k-1时刻***状态参数向量的状态分量的不确定区间；

步骤三：基于Stirling插值多项式逼近法对组合导航***的非线性误差状态方程和观测方程实施线性化处理操作，确定Lagrange余子的取值区间；

步骤四：计算线性化误差边界，利用椭球将线性化误差外包得到非线性误差状态方程和观测方程的线性化误差的外包椭球；

步骤五：计算虚拟过程状态噪声误差椭球和虚拟观测噪声椭球；

步骤六：利用线性化椭球集员滤波算法的预测步骤计算预测状态椭球边界；

步骤七：利用线性椭球集员滤波算法的更新步骤更新状态椭球边界；

步骤八：利用线性椭球集员滤波算法的状态估计步骤完成***状态变量k时刻的估计计算和估计方差矩阵计算，从而完成组合导航***初始对准参数的估计计算任务。

所述组合导航***的非线性误差状态方程和观测方程为：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+w_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.---\left(1\right)$

其中，x_k∈Rⁿ和z_k∈R^m分别表示k时刻的状态变量和观测向量，f(·)和h(·)是非线性二阶可导函数，w_k∈Rⁿ和v_k∈R^m分别表示k时刻的过程噪声和观测噪声，m和n分别表示状态变量和观测向量维数，记w_k∈(0,Q_k)和v_k∈(0,R_k)，Q_k为过程噪声包络矩阵，Rk为观测噪声包络矩阵，且 ε为大于0的误差界；组合导航***状态变量的初始状态x₀属于一个已知的有界集合X₀，即x₀∈X₀，对于给定的测量序列向量那么k时刻的椭球集员滤波算法的状态可行集合为X_k；定义椭球集合E(a,P)＝{x∈Rⁿ|(x-a)^TP^-1(x-a)≤1}，其中，a表示椭球集合的中心，P为满足正定性的椭球包络矩阵，定义***初始状态估计椭球集合为那么k-1时刻估计得到的***状态椭球集合为

所述k-1时刻***状态参数向量的状态分量的不确定区间为：

其中i＝1,2,…,n，表示k-1时刻椭球包络矩阵P_k-1的第i个对角元素，s表示插值步长，表示k-1时刻的状态变量的估计点。

所述基于Stirling插值多项式逼近法对组合导航***的非线性误差状态方程和观测方程实施线性化处理操作，确定Lagrange余子的取值区间的方法是：基于区间分析技术利用Stirling插值多项式获得线性化生成的Lagrange余子的最大区间，以k-1时刻状态变量的估计点做Stirling插值多项式逼近获得***状态方程的线性化表达式为：

${\stackrel{\‾}{x}}_k=f\left({\hat{x}}_{k-1}\right)+D_{\mathrm{\Δ}x}f\left({\hat{x}}_{k-1}\right)+\frac{1}{2!}{D}_{\mathrm{\Δ}x}^{2}f\left({\hat{x}}_{k-1}\right)+...---\left(2\right)$

其中，为差分算子，定义为

$D_{\mathrm{\Δ}x}f\left({\hat{x}}_{k-1}\right)=\frac{1}{s}\[\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δx}}_p{\mathrm{\μ}}_p{\mathrm{\δ}}_p\]f\left({\hat{x}}_{k-1}\right)---\left(3\right)$

$D_{\mathrm{\Δ}x}^{2}f\left({\hat{x}}_{k-1}\right)=\frac{1}{s^2}\[\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δx}}_p^2{\mathrm{\δ}}_p^2+\underset{p=1}{\overset{n}{\mathrm{\Σ}}}\underset{q=1(p\≠q)}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δx}}_p{\mathrm{\Δx}}_q\left({\mathrm{\μ}}_p{\mathrm{\δ}}_p\right)\left({\mathrm{\μ}}_q{\mathrm{\δ}}_q\right)\]f\left({\hat{x}}_{k-1}\right)---\left(4\right)$

其中，μ_p为观测向量预测的偏差算子，δ_p为观测向量预测的平均算子，分别表示为

${\mathrm{\μ}}_p=f({\hat{x}}_{k-1}+\frac{s}{2}{e}_p)-f({\hat{x}}_{k-1}-\frac{s}{2}{e}_p),{\mathrm{\δ}}_p=\frac{1}{2}\[f({\hat{x}}_{k-1}+\frac{s}{2}{e}_p)+f({\hat{x}}_{k-1}-\frac{s}{2}{e}_p)\]---\left(5\right)$

其中，e_p为沿轴向的单位向量，s为插值步长；取Stirling插值多项式式(2)的前两项作为非线性***状态过程函数的线性化近似，那么Lagrange余子的取值区间为：

$X_{R_2}(\mathrm{\Δ}x,{\hat{x}}_{k-1})=D_{\mathrm{\Δ}x}^{2}f\left({\hat{x}}_{k-1}\right)=\frac{1}{s^2}\[\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δx}}_p^2{\mathrm{\δ}}_p^2+\underset{p=1}{\overset{n}{\mathrm{\Σ}}}\underset{q=1(p\≠q)}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δx}}_p{\mathrm{\Δx}}_q\left({\mathrm{\μ}}_p{\mathrm{\δ}}_p\right)\left({\mathrm{\μ}}_q{\mathrm{\δ}}_q\right)\]f\left({\hat{x}}_{k-1}\right)---\left(6\right)$

其中，R₂表示二阶差分算子余子符号；

基于区间分析技术利用Stirling插值多项式获得线性化生成的Lagrange余子的最大区间，以k-1时刻状态变量的一步预测估计点做Stirling插值多项式逼近获得观测过程方程的线性化表达式

${\stackrel{\‾}{z}}_{k,k-1}=h\left({\hat{x}}_{k,k-1}\right)+D_{\mathrm{\Δ}z}h\left({\hat{x}}_{k,k-1}\right)+\frac{1}{2!}{D}_{\mathrm{\Δ}z}^{2}h\left({\hat{x}}_{k,k-1}\right)+...---\left(2^{,}\right)$

其中，项称为差分算子，定义为

$D_{\mathrm{\Δ}z}h\left({\hat{x}}_{k,k-1}\right)=\frac{1}{s}\[\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δz}}_p{\mathrm{\μ}}_p{\mathrm{\δ}}_p\]h\left({\hat{x}}_{k.k-1}\right)---\left(3^{,}\right)$

$D_{\mathrm{\Δ}z}^{2}h\left({\hat{x}}_{k,k-1}\right)=\frac{1}{s^2}\[\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δz}}_p^2{\mathrm{\δ}}_p^2+\underset{p=1}{\overset{n}{\mathrm{\Σ}}}\underset{q=1(p\≠q)}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δz}}_p{\mathrm{\Δz}}_q\left({\mathrm{\μ}}_p{\mathrm{\δ}}_p\right)\left({\mathrm{\μ}}_q{\mathrm{\δ}}_q\right)\]h\left({\hat{x}}_{k,k-1}\right)---\left(4^{,}\right)$

式中μ_p为观测向量预测的偏差算子，δ_p为观测向量预测的平均算子，分别表示为

${\mathrm{\μ}}_p=h({\hat{x}}_{k,k-1}+\frac{s}{2}{e}_p)-h({\hat{x}}_{k,k-1}-\frac{s}{2}{e}_p),$

${\mathrm{\δ}}_p=\frac{1}{2}\[h({\hat{x}}_{k,k-1}+\frac{s}{2}{e}_p)+h({\hat{x}}_{k,k-1}-\frac{s}{2}{e}_p)\]---\left(5^{,}\right)$

其中，e_p为沿轴向的单位向量，s为插值步长；取Stirling插值多项式式(2’)的前两项作为非线性观测方程线性化近似，那么Lagrange余子的取值区间可表达为

$Z_{R_2}(\mathrm{\Δ}z,z_{k,k-1})=D_{\mathrm{\Δ}z}^{2}h\left({\hat{x}}_{k,k-1}\right)=\frac{1}{s^2}\[\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δz}}_p^2{\mathrm{\δ}}_p^2+\underset{p=1}{\overset{n}{\mathrm{\Σ}}}\underset{q=1(p\≠q)}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δz}}_p{\mathrm{\Δz}}_q\left({\mathrm{\μ}}_p{\mathrm{\δ}}_p\right)\left({\mathrm{\μ}}_q{\mathrm{\δ}}_q\right)\]h\left({\hat{x}}_{k,k-1}\right)---\left(6^{,}\right).$

所述计算线性化误差边界，利用椭球将线性化误差外包得到非线性误差状态方程和观测方程的方法是：利用Stirling插值多项式逼近的线性化操作获得二阶Stirling差分算子作为Lagrange余子的计算线性化误差边界，用椭球将状态方程的线性化误差外包

${\stackrel{\‾}{Q}}_{k-1}^{i,i}=2{\[X_{R_2}(\mathrm{\Δ}x,{\hat{x}}_{k-1})\]}^2,{\stackrel{\‾}{Q}}_{k-1}^{i,j}=0,(i\≠j)---\left(7\right)$

获得状态方程的线性化误差的外包椭球为其中，表示***状态方程线性化误差外包椭球包络矩阵，表示***状态方程线性化误差外包椭球包络矩阵的对角线元素；

用椭球将观测方程的线性化误差外包

${\stackrel{\‾}{R}}_{k-1}^{i,i}=2{\[Z_{R_2}(\mathrm{\Δ}z,{\stackrel{\‾}{z}}_{k,k-1})\]}^2,{\stackrel{\‾}{R}}_{k-1}^{i,j}=0,(i\≠j)---\left(7^{,}\right)$

获得观测方程的线性化误差的外包椭球为其中，为观测方程线性化误差外包椭球包络矩阵、表示观测方程线性化误差外包椭球包络矩阵的对角线元素。

所述计算虚拟过程状态噪声误差椭球和虚拟观测噪声椭球的方法是：计算虚拟过程的状态噪声误差椭球为：

${\stackrel{\‾}{w}}_{k-1}\∈E(0,{\hat{Q}}_{k-1})\⊃E(0,Q_{k-1})\⊕E(0,{\stackrel{\‾}{Q}}_{k-1})---\left(8\right)$

其中，表示k-1时刻***虚拟过程噪声包络矩阵，是由椭球的线性化误差和过程噪声相加获得的，涉及到两个椭球的直和计算：

${\hat{Q}}_{k-1}=\frac{{\stackrel{\‾}{Q}}_{k-1}}{1-{\mathrm{\β}}_{Q_{k-1}}}+\frac{Q_{k-1}}{{\mathrm{\β}}_{Q_{k-1}}},{\mathrm{\β}}_{Q_{k-1}}\∈(0,1)---\left(9\right).$

对于非线性观测方程z_k＝h(x_k)+v_k做上述计算步骤，计算虚拟观测噪声误差椭球

${\stackrel{\‾}{v}}_k\∈E(0,{\hat{R}}_k)\⊃E(0,R_k)\⊕E(0,{\stackrel{\‾}{R}}_k)---\left(8^{,}\right)$

是由椭球的线性化误差和过程噪声相加获得的，其中涉及到两个椭球的直和计算

${\hat{R}}_k=\frac{{\stackrel{\‾}{R}}_k}{1-{\mathrm{\β}}_{R_k}}+\frac{R_k}{{\mathrm{\β}}_{R_k}},{\mathrm{\β}}_{R_k}\∈(0,1)---\left(9^{,}\right)$

得到虚拟观测噪声椭球得到虚拟观测噪声椭球其中表示k时刻虚拟观测噪声包络矩阵，表示观测噪声包络矩阵R_k的尺度因子参数，表示过程噪声包络矩阵Q_k-1的尺度因子参数。

所述利用线性化椭球集员滤波算法的预测步骤计算预测状态椭球边界的方法是：线性化预测椭球和虚拟过程噪声直和计算过程

${\hat{x}}_{k,k-1}=f\left({\hat{x}}_{k-1}\right)---\left(10\right)$

$P_{k,k-1}=A_{k-1}\frac{P_{k-1}}{1-{\mathrm{\β}}_{k-1}}{A}_{k-1}^T+\frac{Q_{k-1}}{{\mathrm{\β}}_{k-1}}---\left(11\right)$

其中，是***过程方程的一阶差分算子矩阵，β_k-1表示k-1时刻***状态的尺度因子参数，P_k-1表示k-1时刻***状态变量误差包络矩阵，P_k,k-1表示k时刻的***状态变量一步预测误差包络矩阵；

得到预测状态椭球

所述利用线性椭球集员滤波算法的更新步骤更新状态椭球边界的方法是：将预测状态椭球和观测集合做直和交集计算：

$W_k=H_k\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}{H}_k^T+\frac{{\hat{R}}_k}{{\mathrm{\ρ}}_k},{\mathrm{\ρ}}_k\∈(0,1)---\left(12\right)$

$K_k=\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}{H}_k^T{W}_k^{-1}---\left(13\right)$

其中，是观测方程的一阶差分算子矩阵，y_k表示观测向量，K_k表示线性椭球集员滤波算法的增益矩阵，ρ_k为预测误差包络矩阵P_k,k-1的调节尺度因子参数。

所述利用线性椭球集员滤波算法的状态估计步骤完成***状态变量k时刻的估计计算和估计方差矩阵计算，从而完成组合导航***初始对准参数的估计计算任务的方法是：

${\hat{x}}_k={\hat{x}}_{k,k-1}+K_k\[y_k-h\left({\hat{x}}_{k,k-1}\right)\]---\left(14\right)$

${\tilde{P}}_k=\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}-\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}{H}_k^T{W}_k^{-1}{H}_k\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}---\left(15\right)$

$P_k={\mathrm{\δ}}_k{\tilde{P}}_k---\left(16\right)$

其中

${\mathrm{\δ}}_k=1-{\[y_k-h\left({\hat{x}}_{k,k-1}\right)\]}^T{W}_k^{-1}\[y_k-h\left({\hat{x}}_{k,k-1}\right)\]---\left(17\right)$

表示k时刻***状态变量估计误差包络矩阵计算的中间算子。

本发明利用Stirling插值多项式逼近计算实现椭球集员滤波方法，有效减小了基于Taylor级数的扩展表达式计算的复杂性和计算量，并且能够有效改善扩展集员滤波算法的计算精度；且其二阶Stirling插值多项式计算相对简单，计算效率可以得到有效的提高，能够满足SINS导航***初始对准的快速计算要求，完成对舰船SINS导航***初始对准大方位失准角模型状态参数的估计计算任务。

附图说明

下面结合附图说明对本发明作进一步的说明。

图1是本发明的***结构流程图。

图2是本发明的详细计算流程图。

图3是本发明的舰船载体机动转弯运行轨迹图。

图4是本发明的SINS导航***姿态速度状态参数估计数据图。

图5是本发明的SINS导航***惯性测量单元的状态参数估计数据图。

图6是本发明的SINS导航***的状态参数估计误差数据图。

具体实施方式

下面结合附图和具体实施例详细阐述一下本发明。

一种基于Stirling插值多项式逼近方法的非线性椭球集员滤波方法，即其SINS***的非线性误差模型状态参数估计方法，其步骤如下：

步骤一：建立SINS组合导航***的非线性误差状态方程和观测方程。

建立SINS***初始对准非线性误差模型方程组，包括非线性误差***的状态方程和观测方程：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+w_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.---\left(1\right)$

其中，x_k∈Rⁿ和z_k∈R^m分别表示k时刻的状态变量和观测向量，f(·)和h(·)是已知的非线性二阶可导函数。w_k∈Rⁿ和v_k∈R^m分别表示k时刻的过程噪声和观测噪声，其随时间变化，且满足未知但有界(UBB)的假设条件，m和n分别表示状态变量和观测向量的维数。记w_k∈(0,Q_k)和v_k∈(0,R_k)，Q_k为过程噪声包络矩阵，Rk为观测噪声包络矩阵，且 ε为大于0的误差界。组合导航***状态变量的初始状态x₀属于一个已知的有界集合X₀，即x₀∈X₀，该集合可以由***状态的先验知识确定。对于给定的测量序列向量那么k时刻的椭球集员滤波算法的状态可行集合为X_k。状态可行集合X_k由所有可能的状态点组成，这些状态点与所有可获取的信息，包括***模型、噪声假设和初始状态集合相一致。

定义椭球集合E(a,P)＝{x∈Rⁿ|(x-a)^TP^-1(x-a)≤1}，其中，a表示椭球集合的中心，P为满足正定性的椭球包络矩阵。定义***初始状态估计椭球集合为那么k-1时刻估计得到的***状态椭球集合为

步骤二：计算k-1时刻***状态参数向量的状态分量的不确定区间。

根据k-1时刻的***状态向量参数的估计值和估计方差矩阵来确定当前时刻***状态变量的不确定区间，其中，k的取值为1,2,…。k-1时刻***状态参数向量的状态分量的不确定区间为：其中i＝1,2,…,n，表示k-1时刻椭球包络矩阵Pk-1的第i个对角元素，s表示插值的步长，表示k-1时刻的状态变量的估计中心点。

步骤三：基于Stirling插值多项式逼近法对组合导航***的非线性误差状态方程和观测方程实施线性化处理操作，确定Lagrange余子的取值区间。

以当前k-1时刻的***状态变量估计值实施Stirling插值多项式扩展，取其二阶差分算子作为***非线性过程方程的线性化近似表达式。

SINS组合导航***的非线性误差状态方程x_k-1＝f(x_k-1)+w_k-1，基于区间分析技术利用Stirling插值多项式获得线性化生成的Lagrange余子的最大区间，以k-1时刻状态变量的估计点做Stirling插值多项式逼近获得***状态方程的线性化表达式为：

${\stackrel{\‾}{x}}_k=f\left({\hat{x}}_{k-1}\right)+D_{\mathrm{\Δ}x}f\left({\hat{x}}_{k-1}\right)+\frac{1}{2!}{D}_{\mathrm{\Δ}x}^{2}f\left({\hat{x}}_{k-1}\right)+...---\left(2\right)$

其中，为差分算子，定义为

$D_{\mathrm{\Δ}x}f\left({\hat{x}}_{k-1}\right)=\frac{1}{s}\[\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δx}}_p{\mathrm{\μ}}_p{\mathrm{\δ}}_p\]f\left({\hat{x}}_{k-1}\right)---\left(3\right)$

$D_{\mathrm{\Δ}x}^{2}f\left({\hat{x}}_{k-1}\right)=\frac{1}{s^2}\[\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δx}}_p^2{\mathrm{\δ}}_p^2+\underset{p=1}{\overset{n}{\mathrm{\Σ}}}\underset{q=1(p\≠q)}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δx}}_p{\mathrm{\Δx}}_q\left({\mathrm{\μ}}_p{\mathrm{\δ}}_p\right)\left({\mathrm{\μ}}_q{\mathrm{\δ}}_q\right)\]f\left({\hat{x}}_{k-1}\right)---\left(4\right)$

其中，μ_p为点观测向量预测的偏差算子，δ_p为观测向量预测的平均算子，分别表示为

${\mathrm{\μ}}_p=f({\hat{x}}_{k-1}+\frac{s}{2}{e}_p)-f({\hat{x}}_{k-1}-\frac{s}{2}{e}_p),{\mathrm{\δ}}_p=\frac{1}{2}\[f({\hat{x}}_{k-1}+\frac{s}{2}{e}_p)+f({\hat{x}}_{k-1}-\frac{s}{2}{e}_p)\]---\left(5\right)$

其中，e_p为沿轴向的单位向量，s为插值步长。

从Stirling插值多项式逼近表达式式(2)可以看出，Stirling插值展开式的计算精度高于Taylor级数展开式，且其精度可由插值步长s来控制。取Stirling插值多项式式(2)的前两项作为非线性***状态过程函数的线性化近似，那么Lagrange余子的取值区间为：

$X_{R_2}(\mathrm{\Δ}x,{\hat{x}}_{k-1})=D_{\mathrm{\Δ}x}^{2}f\left({\hat{x}}_{k-1}\right)=\frac{1}{s^2}\[\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δx}}_p^2{\mathrm{\δ}}_p^2+\underset{p=1}{\overset{n}{\mathrm{\Σ}}}\underset{q=1(p\≠q)}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δx}}_p{\mathrm{\Δx}}_q\left({\mathrm{\μ}}_p{\mathrm{\δ}}_p\right)\left({\mathrm{\μ}}_q{\mathrm{\δ}}_q\right)\]f\left({\hat{x}}_{k-1}\right)---\left(6\right)$

其中，下标R₂表示二阶差分算子余子。

SINS组合导航***的非线性误差的观测方程z_k＝h(x_k)+v_k，基于区间分析技术利用Stirling插值多项式获得线性化生成的Lagrange余子的最大区间，以k-1时刻状态预测估计中心点做Stirling插值多项式逼近获得观测过程方程的线性化表达式：

${\stackrel{\‾}{z}}_{k,k-1}=h\left({\hat{x}}_{k,k-1}\right)+D_{\mathrm{\Δ}z}h\left({\hat{x}}_{k,k-1}\right)+\frac{1}{2!}{D}_{\mathrm{\Δ}z}^{2}h\left({\hat{x}}_{k,k-1}\right)+...---\left(2^{,}\right)$

其中，项称为差分算子，定义为

$D_{\mathrm{\Δ}z}h\left({\hat{x}}_{k,k-1}\right)=\frac{1}{s}\[\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δz}}_p{\mathrm{\μ}}_p{\mathrm{\δ}}_p\]h\left({\hat{x}}_{k.k-1}\right)---\left(3^{,}\right)$

$D_{\mathrm{\Δ}z}^{2}h\left({\hat{x}}_{k,k-1}\right)=\frac{1}{s^2}\[\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δz}}_p^2{\mathrm{\δ}}_p^2+\underset{p=1}{\overset{n}{\mathrm{\Σ}}}\underset{q=1(p\≠q)}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δz}}_p{\mathrm{\Δz}}_q\left({\mathrm{\μ}}_p{\mathrm{\δ}}_p\right)\left({\mathrm{\μ}}_q{\mathrm{\δ}}_q\right)\]h\left({\hat{x}}_{k,k-1}\right)---\left(4^{,}\right)$

式中μ_p为观测向量预测的偏差算子，δ_p为观测向量预测的平均算子，分别表示为

${\mathrm{\μ}}_p=h({\hat{x}}_{k,k-1}+\frac{s}{2}{e}_p)-h({\hat{x}}_{k,k-1}-\frac{s}{2}{e}_p),$

${\mathrm{\δ}}_p=\frac{1}{2}\[h({\hat{x}}_{k,k-1}+\frac{s}{2}{e}_p)+h({\hat{x}}_{k,k-1}-\frac{s}{2}{e}_p)\]---\left(5^{,}\right)$

其中，e_p为沿轴向的单位向量，s为插值步长；取Stirling插值多项式式(2’)的前两项作为非线性观测方程线性化近似，那么Lagrange余子的取值区间可表达为

$Z_{R_2}(\mathrm{\Δ}z,z_{k,k-1})=D_{\mathrm{\Δ}z}^{2}h\left({\hat{x}}_{k,k-1}\right)=\frac{1}{s^2}\[\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δz}}_p^2{\mathrm{\δ}}_p^2+\underset{p=1}{\overset{n}{\mathrm{\Σ}}}\underset{q=1(p\≠q)}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Δz}}_p{\mathrm{\Δz}}_q\left({\mathrm{\μ}}_p{\mathrm{\δ}}_p\right)\left({\mathrm{\μ}}_q{\mathrm{\δ}}_q\right)\]h\left({\hat{x}}_{k,k-1}\right)---\left(6^{,}\right)$

其中，R₂表示二阶差分算子余子符号。

步骤四：计算线性化误差边界，利用椭球将线性化误差外包得到非线性误差状态方程和观测方程的线性化误差的外包椭球。

利用Stirling插值多项式逼近的线性化操作获得二阶Stirling差分算子作为Lagrange余子，计算线性化误差边界，用椭球将状态方程的线性化误差外包

${\stackrel{\‾}{Q}}_{k-1}^{i,i}=2{\[X_{R_2}(\mathrm{\Δ}x,{\hat{x}}_{k-1})\]}^2,{\stackrel{\‾}{Q}}_{k-1}^{i,j}=0,(i\≠j)---\left(7\right)$

获得状态方程的线性化误差的外包椭球为其中，表示***状态方程线性化误差外包椭球包络矩阵，表示***状态方程线性化误差外包椭球包络矩阵的对角线元素。

用椭球将观测方程的线性化误差外包

${\stackrel{\‾}{R}}_{k-1}^{i,i}=2{\[Z_{R_2}(\mathrm{\Δ}z,{\stackrel{\‾}{z}}_{k,k-1})\]}^2,{\stackrel{\‾}{R}}_{k-1}^{i,j}=0,(i\≠j)---\left(7^{,}\right)$

获得观测方程的线性化误差的外包椭球为其中，为观测方程线性化误差外包椭球包络矩阵，表示观测方程线性化误差外包椭球包络矩阵的对角线元素。

步骤五：计算虚拟过程状态噪声误差椭球和虚拟观测噪声椭球。

涉及到Stirling插值多项式逼近的线性化误差和过程噪声相加的两个椭球直和运算；通过线性化误差和过程噪声的直和计算获得虚拟噪声误差椭球。

计算虚拟过程的状态噪声误差椭球为：

${\stackrel{\‾}{w}}_{k-1}\∈E(0,{\hat{Q}}_{k-1})\⊃E(0,Q_{k-1})\⊕E(0,{\stackrel{\‾}{Q}}_{k-1})---\left(8\right)$

其中，表示k-1时刻***虚拟过程噪声包络矩阵，是由椭球的线性化误差和过程噪声相加获得的，涉及到两个椭球的直和计算：

${\hat{Q}}_{k-1}=\frac{{\stackrel{\‾}{Q}}_{k-1}}{1-{\mathrm{\β}}_{Q_{k-1}}}+\frac{Q_{k-1}}{{\mathrm{\β}}_{Q_{k-1}}},{\mathrm{\β}}_{Q_{k-1}}\∈(0,1)---\left(9\right)$

对于非线性观测方程z_k＝h(x_k)+v_k做上述计算步骤，计算虚拟观测噪声误差椭球

${\stackrel{\‾}{v}}_k\∈E(0,{\hat{R}}_k)\⊃E(0,R_k)\⊕E(0,{\stackrel{\‾}{R}}_k)---\left(8^{,}\right)$

是由椭球的线性化误差和过程噪声相加获得的，其中涉及到两个椭球的直和计算

${\hat{R}}_k=\frac{{\stackrel{\‾}{R}}_k}{1-{\mathrm{\β}}_{R_k}}+\frac{R_k}{{\mathrm{\β}}_{R_k}},{\mathrm{\β}}_{R_k}\∈(0,1)---\left(9^{,}\right)$

得到虚拟观测噪声椭球得到虚拟观测噪声椭球其中，表示k时刻虚拟观测噪声包络矩阵，表示观测噪声包络矩阵R_k的尺度因子参数，表示过程噪声包络矩阵Q_k-1的尺度因子参数。

步骤六：利用线性化椭球集员滤波算法的预测步骤计算预测状态椭球边界。

其中涉及到线性化预测椭球和虚拟过程噪声椭球的直和计算过程；利用k-1时刻的***状态变量估计值代入***过程方程，获得状态变量线性化预测值，及其外包预测椭球，开展线性化预测椭球和虚拟过程噪声椭球的直和运算，获得***状态变量的预测椭球边界。

线性化预测椭球和虚拟过程噪声直和计算过程

${\hat{x}}_{k,k-1}=f\left({\hat{x}}_{k-1}\right)---\left(10\right)$

$P_{k,k-1}=A_{k-1}\frac{P_{k-1}}{1-{\mathrm{\β}}_{k-1}}{A}_{k-1}^T+\frac{Q_{k-1}}{{\mathrm{\β}}_{k-1}}---\left(11\right)$

其中，是***过程方程的一阶差分算子矩阵，β_k-1表示k-1时刻***状态的尺度因子参数，P_k-1表示k-1时刻***状态变量误差包络矩阵，P_k,k-1表示k时刻的***状态变量一步预测误差包络矩阵；得到预测状态椭球

步骤七：利用线性椭球集员滤波算法的更新步骤更新状态椭球边界。

其中涉及到预测状态椭球和观测向量集合的交集计算；利用***观测向量序列开展预测状态椭球和观测向量带的交集计算。

将预测状态椭球和观测集合做直和交集计算：

$W_k=H_k\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}{H}_k^T+\frac{{\hat{R}}_k}{{\mathrm{\ρ}}_k},{\mathrm{\ρ}}_k\∈(0,1)---\left(12\right)$

$K_k=\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}{H}_k^T{W}_k^{-1}---\left(13\right)$

其中，是观测方程的一阶差分算子矩阵，y_k表示观测向量，K_k表示线性椭球集员滤波算法的增益矩阵，ρ_k为预测误差包络矩阵P_k,k-1的调节尺度因子参数。

步骤八：利用线性椭球集员滤波算法的状态估计步骤完成***状态变量k时刻的估计计算和估计方差矩阵计算，从而完成SINS组合导航***初始对准参数的估计计算任务。

${\hat{x}}_k={\hat{x}}_{k,k-1}+K_k\[y_k-h\left({\hat{x}}_{k,k-1}\right)\]---\left(14\right)$

${\tilde{P}}_k=\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}-\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}{H}_k^T{W}_k^{-1}{H}_k\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}---\left(15\right)$

$P_k={\mathrm{\δ}}_k{\tilde{P}}_k---\left(16\right)$

其中

${\mathrm{\δ}}_k=1-{\[y_k-h\left({\hat{x}}_{k,k-1}\right)\]}^T{W}_k^{-1}\[y_k-h\left({\hat{x}}_{k,k-1}\right)\]---\left(17\right)$

表示k时刻***状态变量估计误差包络矩阵计算的中间算子。

本发明的优势在于采用Stirling插值多项式实施线性化操作，有效避免Taylor级数展开式的一阶Jacobian矩阵和二阶Hessian矩阵的复杂计算，降低了算法的计算复杂度；利用插值步长s能够控制计算精度；相比于Taylor级数扩展的传统非线性集员滤波算法，本发明的计算精度比较高。

在本发明中，引入了四个参数，插值步长s和三个调节尺度因子参数β_k-1和ρ_k，其数值确定方法如下：

对于插值步长s，一般情况下若***状态向量满足Gauss分布时，为了满足这一条件，每次迭代计算的***状态向量的估计误差包络矩阵P都实施Cholsky分解，P＝SS^T，其中，S表示估计误差包络矩阵P的Cholsky分解因子，从而对***状态向量开展解耦变换操作，使其满足Gauss分布条件。

调节尺度因子参数和β_k-1涉及到两个椭球直和运算的外包椭球最优化问题，这里选取外包椭球的最小化计算方法，该方法求解形式简单，相比较于最小化外包椭球体积的优化准则，该方法性能鲁棒性更强。即有从而可以采用式子获得最优的尺度因子参数和β_k-1，P₁，P₂分别泛指前述的线性化误差包络矩阵和过程噪声包络矩阵或者观测噪声包络矩阵。

尺度因子参数需要过程噪声包络椭球E(0,Q_k-1)和***状态方程线性化误差外包椭球包络椭球的直和计算，那么其计算准则式为其最优计算式为

对于尺度因子参数β_k-1，需要状态方程虚拟过程的线性化误差的外包椭球和状态向量预测椭球的直和计算，考虑观测向量更新条件下的方差矩阵计算式为

$P_k={\mathrm{\δ}}_{k-1}\[I_{n\×n}-\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}{H}_k^T{W}_k^{-1}{H}_k\]\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}$

从而可以得到尺度因子参数β_k-1的计算公式为

${\mathrm{\β}}_{k-1}=\frac{\sqrt{tr\left(A_k{P}_k{A}_k^T\right)}}{\sqrt{tr\left({\hat{Q}}_k\right)}+\sqrt{tr\left(A_k{P}_k{A}_k^T\right)}}$

在迭代计算过程中，观测集合S_y形式一般都比较复杂，从而导致***状态向量方差矩阵P_k的计算复杂性，无论采用最小化椭球体积法还是最小化椭球迹准则，都使尺度因子参数ρ_k的优化计算很困难，甚至无法获得解析解，若采用数值计算方法的话计算复杂度很高。在本发明中采用最小化性能指标δ_k上界形式来计算

${\mathrm{\ρ}}_k=\arg \underset{{\mathrm{\ρ}}_k\∈(0,1)}{min}sup\left({\mathrm{\δ}}_k\right)$

这样可以获得调节尺度因子参数ρ_k的一种次优计算式

${\mathrm{\ρ}}_k=\frac{\sqrt{c_m}}{\sqrt{p_m}+\sqrt{c_m}}\∈(0,1)$

其中，p_m是矩阵的最大奇异值，c_m是矩阵的最大奇异值。

具体实施例：采用本发明开展对舰船SINS导航***的初始对准大方位失准角模型状态参数的估计计算任务。

载体SINS姿态误差方程为

$\stackrel{\·}{y}=(I-C_n^{n^{\′}}){\mathrm{\ω}}_{in}^n+{\mathrm{\δ\ω}}_{in}^n-C_b^n{\mathrm{\ϵ}}^b$

其中，y＝[φ_p φ_r φ_y]^T是舰船载体失准角向量，代表陀螺随机常值漂移和随机噪声，载体速度误差方程为：

$\mathrm{\δ}\stackrel{\·}{V}=(C_n^{n^{\′}}-I)f^n+{\mathrm{\δV}}^n(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)+V^n(2{\mathrm{\δ\ω}}_{ie}^n+{\mathrm{\δ\ω}}_{en}^n)+C_b^n{\▿}^b$

其中，δ表示两个向量分量之差、载体在导航坐标系中相对于地球系的旋转角速度、载体在惯性系中相对于导航坐标系的旋转角速度、Vⁿ分别导航坐标系中的载体运动速度，δV＝[δV_e δV_n]^T表示水面舰船载体速度误差向量，V_e表示东向速度，V_n表示北向速度；代表加速度计偏差向量，可以看作常值偏差和随机噪声；表示SINS***计算坐标系n′和导航坐标系n之间的姿态误差矩阵，由SINS姿态误差失准角φ可表达为

$C_n^{n^{\′}}=\left[\begin{array}{ccc}{\mathrm{cos\φ}}_y& {\mathrm{sin\φ}}_y& -{\mathrm{\φ}}_r\\ -{\mathrm{sin\φ}}_y& {\mathrm{cos\φ}}_y& {\mathrm{\φ}}_p\\ {\mathrm{\φ}}_r{\mathrm{cos\φ}}_y+{\mathrm{\φ}}_p{\mathrm{sin\φ}}_y& {\mathrm{\φ}}_r{\mathrm{sin\φ}}_y-{\mathrm{\φ}}_p{\mathrm{cos\φ}}_y& 1\end{array}\right].$

考虑都舰船载体水面运动情形有东向速度和北向速度误差方程。舰船水面运动中初始对准***状态向量

$X={\[{\mathrm{\δV}}_e,{\mathrm{\δV}}_n,{\mathrm{\φ}}_p,{\mathrm{\φ}}_r,{\mathrm{\φ}}_y,{\▿}_x,{\▿}_y,{\mathrm{\ϵ}}_x,{\mathrm{\ϵ}}_y,{\mathrm{\ϵ}}_z\]}^T$

那么考虑SINS初始对准***非线性方程

$\stackrel{\·}{X}=f\left(x\right)+w$

其中，f(·)为：

$f\left(x\right)=\left\{\begin{array}{c}{f}_x({\mathrm{cos\φ}}_y-1)+f_y{\mathrm{sin\φ}}_y+{\mathrm{\δV}}_x\frac{V_y}{R_e}tL+{\mathrm{\δV}}_y(2{\mathrm{\ω}}_{ie}\sin L+\frac{V_x}{R_e}tL)+{\▿}_x\\ -f_x{\mathrm{sin\φ}}_y+({\mathrm{cos\φ}}_y-1)f_y+f_u{\mathrm{\φ}}_p-2{\mathrm{\δV}}_x({\mathrm{\ω}}_{ie}\sin L+\frac{V_x}{R_e}tL)+{\▿}_y\\ -(1-{\mathrm{cos\φ}}_y)\frac{V_y}{R_n}-{\mathrm{s\φ}}_y({\mathrm{\ω}}_{ie}\cos L+\frac{V_x}{R_e})+{\mathrm{\φ}}_r({\mathrm{\ω}}_{ie}\sin L+\frac{V_x}{R_e}\tan L)-\frac{{\mathrm{\δV}}_y}{R_n}-{\mathrm{\ϵ}}_x\\ -\frac{V_y}{R_n}{\mathrm{sin\φ}}_y+(1-{\mathrm{cos\φ}}_y)({\mathrm{\ω}}_{ie}\cos L+\frac{V_x}{R_e})-{\mathrm{\φ}}_p({\mathrm{\ω}}_{ie}\sin L+\frac{V_x}{R_e}\tan L)+\frac{{\mathrm{\δV}}_x}{R_e}-{\mathrm{\ϵ}}_y\\ ({\mathrm{\φ}}_r{\mathrm{cos\φ}}_y+{\mathrm{\φ}}_p{\mathrm{sin\φ}}_y)\frac{V_y}{R_n}+({\mathrm{\φ}}_p{\mathrm{cos\φ}}_y-{\mathrm{\φ}}_r{\mathrm{sin\φ}}_y)({\mathrm{\ω}}_{ie}\cos L+\frac{V_x}{R_e})+\frac{{\mathrm{\δV}}_x}{R_e}\tan L-{\mathrm{\ϵ}}_z\\ 0_{5\×1}\end{array}\right.$

其中，f(x)＝0_5×1表示两向加速度计零偏误差微分方程和三个轴向的陀螺仪常值偏移误差微分方程。

考虑东向速度和北向速度作为SINS***观测向量获得SINS***观测方程

Z＝Hx+v

其中，H＝[I_2×2 0_2×8]。

假设SINS***中陀螺仪常值漂移ε向量和加速度计零偏误差向量分别符合一阶Markov模型噪声，其外定界椭球为Ε(0,Q_ε)和导航速度观测方程中的速度误差噪声满足外定界椭球Ε(0,R)。在动基座条件下进行SINS***初始对准状态参数估计计算仿真，舰船载体在海面上做机动转弯运行，其运行轨迹如图3所示。初始速度误差为0，航向角误差-45°，横摇角误差为2.4°，纵摇角误差为2.4°。陀螺仪常值漂移ε的均方差为0.02°/h，时间常数为100s，加速度计零偏误差向量的均方误差为50μg，时间常数为100s，速度观测误差均方差为0.5m/s，惯导积分周期为0.01s。***仿真时间为300秒，从而获得了SINS***姿态速度状态和惯性测量单元的(IMU组件)状态参数估计数据图如图4所示，陀螺仪和加速度计参数估计数据图如图5所示，以及图6所示的***状态变量和陀螺仪以及加速度计参数的算法估计误差数据。

实施过程中采用均方误差指标其中，N为Monte Carlo次数。对***状态变量参数估计的均方误差指标数据越小，表明算法计算精度越高，计算性能越好。仿真时间为300s，本发明仿真进行了500次的Monte Carlo仿真来衡量其的计算效能。

从图3所示的计算的SINS***姿态角估计数据以及图6所示的计算的姿态角估计误差可以看出，本发明计算误差逐步减小，在300秒内能够稳定收敛，计算效率相对较高。从图3的两向速度估计以及图6所示的速度计算误差也可以看出算法的计算优势。从图5所示的陀螺仪和加速度计参数估计以及图6的陀螺仪和加速度计参数估计计算误差数据可以看出，本发明能够准确估计出***IMU组件参数，并且估计误差快速收敛，显示出本算法较好的计算效能。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (9)

1.一种基于Stirling插值多项式逼近的椭球集员滤波方法，其特征在于，其步骤如下：

步骤一：建立组合导航***的非线性误差状态方程和观测方程；

步骤二：计算k-1时刻***状态参数向量的状态分量的不确定区间；

步骤三：基于Stirling插值多项式逼近法对组合导航***的非线性误差状态方程和观测方程实施线性化处理操作，确定Lagrange余子的取值区间；

步骤四：计算线性化误差边界，利用椭球将线性化误差外包得到非线性误差状态方程和观测方程的线性化误差的外包椭球；

步骤五：计算虚拟过程状态噪声误差椭球和虚拟观测噪声椭球；

步骤六：利用线性化椭球集员滤波算法的预测步骤计算预测状态椭球边界；

步骤七：利用线性椭球集员滤波算法的更新步骤更新状态椭球边界；

步骤八：利用线性椭球集员滤波算法的状态估计步骤完成***状态变量k时刻的估计计算和估计方差矩阵计算，从而完成组合导航***初始对准参数的估计计算任务。

2.根据权利要求1所述的基于Stirling插值多项式逼近的椭球集员滤波方法，其特征在于，所述组合导航***的非线性误差状态方程和观测方程为：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+w_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.---\left(1\right)$

其中，x_k∈Rⁿ和z_k∈R^m分别表示k时刻的状态变量和观测向量，f(·)和h(·)是非线性二阶可导函数，w_k∈Rⁿ和v_k∈R^m分别表示k时刻的过程噪声和观测噪声，m和n分别表示状态变量和观测向量的维数，记w_k∈(0,Q_k)和v_k∈(0,R_k)，Q_k为过程噪声包络矩阵，R_k为观测噪声包络矩阵，且ε为大于0的误差界；

组合导航***状态变量的初始状态x₀属于一个已知的有界集合X₀，即x₀∈X₀，对于给定的测量序列向量那么k时刻的椭球集员滤波算法的状态可行集合为X_k；

定义椭球集合E(a,P)＝{x∈Rⁿ|(x-a)^TP^-1(x-a)≤1}，其中，a表示椭球集合的中心，P为满足正定性的椭球包络矩阵，定义***初始状态估计椭球集合为那么k-1时刻估计得到的***状态椭球集合为

3.根据权利要求2所述的基于Stirling插值多项式逼近的椭球集员滤波方法，其特征在于，所述k-1时刻***状态参数向量的状态分量的不确定区间为：

其中i＝1,2,…,n，表示k-1时刻椭球包络矩阵P_k-1的第i个对角元素，s表示插值步长，表示k-1时刻的状态变量的估计点。

4.根据权利要求3所述的基于Stirling插值多项式逼近的椭球集员滤波方法，其特征在于，所述基于Stirling插值多项式逼近法对组合导航***的非线性误差状态方程和观测方程实施线性化处理操作，确定Lagrange余子的取值区间的方法是：基于区间分析技术利用Stirling插值多项式获得线性化生成的Lagrange余子的最大区间，以k-1时刻状态变量的估计点做Stirling插值多项式逼近获得***状态方程的线性化表达式为：

${\stackrel{\‾}{x}}_k=f\left({\hat{x}}_{k-1}\right)+D_{\mathrm{\Δ}x}f\left({\hat{x}}_{k-1}\right)+\frac{1}{2!}{D}_{\mathrm{\Δ}x}^{2}f\left({\hat{x}}_{k-1}\right)+...---\left(2\right)$

其中，为差分算子，定义为

$D_{\mathrm{\Δ}x}f\left({\hat{x}}_{k-1}\right)=\frac{1}{s}\[\underset{p=1}{\overset{n}{\Σ}}{\mathrm{\Δx}}_p{\mathrm{\μ}}_p{\mathrm{\δ}}_p\]f\left({\hat{x}}_{k-1}\right)---\left(3\right)$

$D_{\mathrm{\Δ}x}^{2}f\left({\hat{x}}_{k-1}\right)=\frac{1}{s^2}\[\underset{p=1}{\overset{n}{\Σ}}{\mathrm{\Δx}}_p^2{\mathrm{\δ}}_p^2+\underset{p=1}{\overset{n}{\Σ}}\underset{q=1(p\≠q)}{\overset{n}{\Σ}}{\mathrm{\Δx}}_p{\mathrm{\Δx}}_q\left({\mathrm{\μ}}_p{\mathrm{\δ}}_p\right)\left({\mathrm{\μ}}_q{\mathrm{\δ}}_q\right)\]f\left({\hat{x}}_{k-1}\right)---\left(4\right)$

其中，μ_p为观测向量预测的偏差算子，δ_p为观测向量预测的平均算子，分别表示为

${\mathrm{\μ}}_p=f({\hat{x}}_{k-1}+\frac{s}{2}{e}_p)-f({\hat{x}}_{k-1}-\frac{s}{2}{e}_p),{\mathrm{\δ}}_p=\frac{1}{2}\[f({\hat{x}}_{k-1}+\frac{s}{2}{e}_p)+f({\hat{x}}_{k-1}-\frac{s}{2}{e}_p)\]---\left(5\right)$

其中，e_p为沿轴向的单位向量，s为插值步长；取Stirling插值多项式的前两项作为非线性***状态过程函数的线性化近似，那么Lagrange余子的取值区间为：

$X_{R_2}(\mathrm{\Δ}x,{\hat{x}}_{k-1})=D_{\mathrm{\Δ}x}^{2}f\left({\hat{x}}_{k-1}\right)=\frac{1}{s^2}\[\underset{p=1}{\overset{n}{\Σ}}{\mathrm{\Δx}}_p^2{\mathrm{\δ}}_p^2+\underset{p=1}{\overset{n}{\Σ}}\underset{q=1(p\≠q)}{\overset{n}{\Σ}}{\mathrm{\Δx}}_p{\mathrm{\Δx}}_q\left({\mathrm{\μ}}_p{\mathrm{\δ}}_p\right)\left({\mathrm{\μ}}_q{\mathrm{\δ}}_q\right)\]f\left({\hat{x}}_{k-1}\right)---\left(6\right)$

其中，R₂表示二阶差分算子余子符号；

基于区间分析技术利用Stirling插值多项式获得线性化生成的Lagrange余子的最大区间，以k-1时刻状态变量的一步预测估计点做Stirling插值多项式逼近获得观测过程方程的线性化表达式

${\stackrel{\‾}{z}}_{k,k-1}=h\left({\hat{x}}_{k,k-1}\right)+D_{\mathrm{\Δ}z}h\left({\hat{x}}_{k,k-1}\right)+\frac{1}{2!}{D}_{\mathrm{\Δ}z}^{2}h\left({\hat{x}}_{k,k-1}\right)+...---\left(2^{,}\right)$

其中，项称为差分算子，定义为

$D_{\mathrm{\Δ}z}h\left({\hat{x}}_{k,k-1}\right)=\frac{1}{s}\[\underset{p=1}{\overset{n}{\Σ}}{\mathrm{\Δz}}_p{\mathrm{\μ}}_p{\mathrm{\δ}}_p\]h\left({\hat{x}}_{k.k-1}\right)---\left(3^{,}\right)$

$D_{\mathrm{\Δ}z}^{2}h\left({\hat{x}}_{k,k-1}\right)=\frac{1}{s^2}\[\underset{p=1}{\overset{n}{\Σ}}{\mathrm{\Δz}}_p^2{\mathrm{\δ}}_p^2+\underset{p=1}{\overset{n}{\Σ}}\underset{q=1(p\≠q)}{\overset{n}{\Σ}}{\mathrm{\Δz}}_p{\mathrm{\Δz}}_q\left({\mathrm{\μ}}_p{\mathrm{\δ}}_p\right)\left({\mathrm{\μ}}_q{\mathrm{\δ}}_q\right)\]h\left({\hat{x}}_{k,k-1}\right)---\left(4^{,}\right)$

式中μ_p为观测向量预测的偏差算子，δ_p为观测向量预测的平均算子，分别表示为

${\mathrm{\μ}}_p=h({\hat{x}}_{k,k-1}+\frac{s}{2}{e}_p)-h({\hat{x}}_{k,k-1}-\frac{s}{2}{e}_p),{\mathrm{\δ}}_p=\frac{1}{2}\[h({\hat{x}}_{k,k-1}+\frac{s}{2}{e}_p)+h({\hat{x}}_{k,k-1}-\frac{s}{2}{e}_p)\]---\left(5^{,}\right)$

其中e_p为沿轴向的单位向量，s为插值步长；

取Stirling插值多项式的前两项作为非线性观测方程线性化近似，那么Lagrange余子的取值区间可表达为

$Z_{R_2}(\mathrm{\Δ}z,z_{k,k-1})=D_{\mathrm{\Δ}z}^{2}h\left({\hat{x}}_{k,k-1}\right)=\frac{1}{s^2}\[\underset{p=1}{\overset{n}{\Σ}}{\mathrm{\Δz}}_p^2{\mathrm{\δ}}_p^2+\underset{p=1}{\overset{n}{\Σ}}\underset{q=1(p\≠q)}{\overset{n}{\Σ}}{\mathrm{\Δz}}_p{\mathrm{\Δz}}_q\left({\mathrm{\μ}}_p{\mathrm{\δ}}_p\right)\left({\mathrm{\μ}}_q{\mathrm{\δ}}_q\right)\]h\left({\hat{x}}_{k,k-1}\right)---\left(6^{,}\right).$

5.根据权利要求4所述的基于Stirling插值多项式逼近的椭球集员滤波方法，其特征在于，所述计算线性化误差边界，利用椭球将线性化误差外包得到非线性误差状态方程和观测方程的方法是：利用Stirling插值多项式逼近的线性化操作获得二阶Stirling差分算子作为Lagrange余子的计算线性化误差边界，用椭球将状态方程的线性化误差外包

$\begin{array}{cc}{\stackrel{\‾}{Q}}_{k-1}^{i,i}=2{\[X_{R_2}(\mathrm{\Δ}x,{\hat{x}}_{k-1})\]}^2,{\stackrel{\‾}{Q}}_{k-1}^{i,j}=0& (i\≠j)\end{array}---\left(7\right)$

获得状态方程的线性化误差的外包椭球为其中，表示***状态方程线性化误差外包椭球包络矩阵，表示***状态方程线性化误差外包椭球包络矩阵的对角线元素；

用椭球将观测方程的线性化误差外包

$\begin{array}{cc}{\stackrel{\‾}{R}}_{k-1}^{i,i}=2{\[Z_{R_2}(\mathrm{\Δ}z,{\stackrel{\‾}{z}}_{k,k-1})\]}^2,{\stackrel{\‾}{R}}_{k-1}^{i,j}=0& (i\≠j)\end{array}---\left(7^{,}\right)$

获得观测方程的线性化误差的外包椭球为其中_， 为观测方程线性化误差外包椭球包络矩阵、表示观测方程线性化误差外包椭球包络矩阵的对角线元素。

6.根据权利要求5所述的基于Stirling插值多项式逼近的椭球集员滤波方法，其特征在于，所述计算虚拟过程状态噪声误差椭球和虚拟观测噪声椭球的方法是：计算虚拟过程的状态噪声误差椭球为：

${\stackrel{\‾}{w}}_{k-1}\∈E(0,{\hat{Q}}_{k-1})\⊃E(0,Q_{k-1})\⊕E(0,{\stackrel{\‾}{Q}}_{k-1})---\left(8\right)$

其中，表示k-1时刻***虚拟过程噪声包络矩阵，是由椭球的线性化误差和过程噪声相加获得的，涉及到两个椭球的直和计算：

$\begin{array}{cc}{\hat{Q}}_{k-1}=\frac{{\stackrel{\‾}{Q}}_{k-1}}{1-{\mathrm{\β}}_{Q_{k-1}}}+\frac{Q_{k-1}}{{\mathrm{\β}}_{Q_{k-1}}}& {\mathrm{\β}}_{Q_{k-1}}\∈(0,1)\end{array}---\left(9\right)$

对于非线性观测方程z_k＝h(x_k)+v_k做上述计算步骤，计算虚拟观测噪声误差椭球

${\stackrel{\‾}{v}}_k\∈E(0,{\hat{R}}_k)\⊃E(0,R_k)\⊕E(0,{\stackrel{\‾}{R}}_k)---\left(8^{,}\right)$

是由椭球的线性化误差和过程噪声相加获得的，其中涉及到两个椭球的直和计算

$\begin{array}{cc}{\hat{R}}_k=\frac{{\stackrel{\‾}{R}}_k}{1-{\mathrm{\β}}_{R_k}}+\frac{R_k}{{\mathrm{\β}}_{R_k}}& {\mathrm{\β}}_{R_k}\∈(0,1)\end{array}---\left(9^{,}\right)$

得到虚拟观测噪声椭球得到虚拟观测噪声椭球其中表示k时刻虚拟观测噪声包络矩阵，表示观测噪声包络矩阵R_k的尺度因子参数，表示过程噪声包络矩阵Q_k-1的尺度因子参数。

7.根据权利要求6所述的基于Stirling插值多项式逼近的椭球集员滤波方法，其特征在于，所述利用线性化椭球集员滤波算法的预测步骤计算预测状态椭球边界的方法是：线性化预测椭球和虚拟过程噪声直和计算过程

${\hat{x}}_{k,k-1}=f\left({\hat{x}}_{k-1}\right)---\left(10\right)$

$P_{k,k-1}=A_{k-1}\frac{P_{k-1}}{1-{\mathrm{\β}}_{k-1}}{A}_{k-1}^T+\frac{Q_{k-1}}{{\mathrm{\β}}_{k-1}}---\left(11\right)$

其中，是***过程方程的一阶差分算子矩阵，β_k-1表示k-1时刻***状态的尺度因子参数，P_k-1表示k-1时刻***状态变量误差包络矩阵，P_k,k-1表示k时刻的***状态变量一步预测误差包络矩阵；

得到预测状态椭球

8.根据权利要求7所述的基于Stirling插值多项式逼近的椭球集员滤波方法，其特征在于，所述利用线性椭球集员滤波算法的更新步骤更新状态椭球边界的方法是：将预测状态椭球和观测集合做直和交集计算：

$\begin{array}{cc}{W}_k=H_k\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}{H}_k^T+\frac{{\hat{R}}_k}{{\mathrm{\ρ}}_k}& {\mathrm{\ρ}}_k\∈(0,1)\end{array}---\left(12\right)$

$K_k=\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}{H}_k^T{W}_k^{-1}---\left(13\right)$

其中，是观测方程的一阶差分算子矩阵，y_k表示观测向量，K_k表示线性椭球集员滤波算法的增益矩阵，ρ_k为预测误差包络矩阵P_k,k-1的调节尺度因子参数。

9.根据权利要求8所述的基于Stirling插值多项式逼近的椭球集员滤波方法，其特征在于，所述利用线性椭球集员滤波算法的状态估计步骤完成***状态变量k时刻的估计计算和估计方差矩阵计算，从而完成组合导航***初始对准参数的估计计算任务的方法是：

${\hat{x}}_k={\hat{x}}_{k,k-1}+K_k\[y_k-h\left({\hat{x}}_{k,k-1}\right)\]---\left(14\right)$

${\tilde{P}}_k=\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}-\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}{H}_k^T{W}_k^{-1}{H}_k\frac{P_{k,k-1}}{1-{\mathrm{\ρ}}_k}---\left(15\right)$

$P_k={\mathrm{\δ}}_k{\tilde{P}}_k---\left(16\right)$

其中

${\mathrm{\δ}}_k=1-{\[y_k-h\left({\hat{x}}_{k,k-1}\right)\]}^T{W}_k^{-1}\[y_k-h\left({\hat{x}}_{k,k-1}\right)\]---\left(17\right)$

表示k时刻***状态变量估计误差包络矩阵计算的中间算子。