基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法

摘要

本发明提出了一种基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法，用于解决基于Taylor级数线性近似的方法计算复杂、效率低下、计算精度低的问题；建立非线性捷联惯性导航系统方程和观测方程；计算第

说明书

技术领域

本发明涉及属于航空航天系统处理中导航制导与控制的技术领域，具体涉及一种基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法，实现惯性导航系统非线性系统误差模型状态参数最优滤波计算，可应用于惯性导航系统。

背景技术

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

实际上，非线性系统状态参数可行集合形状一般无法准确确定，甚至是非凸的，集员滤波方法常用的形状描述方法有椭球、区间、超平形体以及全对称多胞形等满足一定规则的几何集合来近似实际可行集，达到降低算法计算复杂度的目的。其中，椭球法具有仿射变换不变性，包络矩阵协方差意义以及便于优化计算等优点获得广泛应用。Schweppe和Bertsekas首先提出了可以利用外定界椭球集合来包含系统的真实状态，但是没有考虑椭球的最优化问题。在此基础上，Fogel和Huang给出了最优化定界椭球算法，得到了最小体积和最小迹椭球集合；Maksarov、Kurzhanski和Chernousko等人进一步发展了针对状态和参数估计计算的椭球计算技术；并且Lin针对特定应用情况提出了一种自适应的边界估计计算的椭球算法；Polyak推倒了用于具有模型不确定性系统的椭球算法，进一步扩展了椭球集员滤波算法的应用领域。

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

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

发明内容

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

本发明的技术方案是：一种基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法，其步骤如下：

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

步骤二：根据第k-1步迭代计算获得系统状态变量的均值和方差，确定第k-1步组合导航系统状态参数向量的状态分量的不确定区间，其中k＝1,2,···；

步骤三：基于Chebyshev多项式插值表达式对组合导航系统非线性误差的状态方程和观测方程实施Chebyshev多项式插值逼近计算处理，确定Lagrange余子的取值区间；

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

步骤五：计算虚拟过程的误差椭球，包括Chebyshev多项式逼近的不确定性误差和过程噪声相加的两个椭球直和运算；

步骤六：利用线性化椭球集员滤波算法的预测步骤计算预测状态椭球边界，包括线性化预测椭球和虚拟过程噪声椭球的直和计算；

步骤七：利用线性椭球集员滤波算法的更新步骤更新状态椭球边界，包括预测状态椭球和观测向量集合的交集计算；

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

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

其中，x_k∈Rⁿ表示k时刻的状态变量，z_k∈R^m表示k时刻的观测向量，f(·)和h(·)是已知的非线性二阶可导函数，w_k∈Rⁿ表示过程噪声，v_k∈R^m表示观测噪声，且|w_i,k|≤1,i＝1,2,…,n，|v_j,k|≤1,j＝1,2,…,m，记w_k∈(0,Q_k)和v_k∈(0,R_k)，Q_k表示第k步的系统状态噪声包络矩阵，R_k表示第k步的系统观测噪声包络矩阵；n表示系统状态变量的维数，m表示观测变量的维数；

系统初始状态x₀∈X₀，X₀为系统状态的先验知识确定的有界集合，对于给定的测量序列向量那么第k步椭球集员滤波算法的状态可行集合为X_k；定义椭球集合E(a,P)＝{x∈Rⁿ|(x-a)^TP^-1(x-a)≤1}，其中，a表示椭球集合的中心，P为满足正定性的椭球包络矩阵；系统初始状态x₀估计的椭球集合为那么第k-1步估计得到的系统状态椭球集合为其中，P₀表示初始系统状态变量的椭球包络矩阵，P_k-1表示状态变量第k-1步的椭球包络矩阵；

所述第k-1步组合导航系统状态参数向量的状态分量的不确定区间为：

其中，i＝1,2,…,n，表示椭球包络矩阵P_k的第(i,i)元素，表示第k-1步的状态变量的估计值，l是一个正实数，且l≥3。

所述确定Lagrange余子的取值区间的方法是：利用Chebyshev多项式逼近分别表达系统非线性的状态方程和观测方程，利用在逼近过程中产生的逼近误差获得Lagrange余子的最大区间：

根据捷联惯性导航系统非线性误差的状态方程x_k＝f(x_k-1)+w_k-1，利用Chebyshev插值多项式获得线性化逼近生成的Lagrange余子的极小化区间，以第k-1步状态变量的估计点作为Chebyshev插值多项式逼近系统状态方程的n阶Chebyshev插值表达式获得第k步状态变量的预测点

其中，表示第i项的Chebyshev多项式，A_i表示Chebyshev多项式的系数，表示Chebyshev多项式逼近的插值余项。

当系统状态变量区间时，插值余项为Chebyshev插值多项式高阶项，其表达式为：

根据Chebyshev插值多项式的性质，当插值节点取Chebyshev多项式的零点值时，插值余项获得极小值：

若第k-1步系统状态变量的取值区间为获得极小化的插值余项为：

基于捷联惯性组合导航系统非线性误差的观测方程z_k＝h(x_k)+v_k，利用Chebyshev插值多项式获得插值逼近计算生成的Lagrange余子的极小化区间，以第k步状态变量的预测点作为Chebyshev插值多项式逼近获得观测方程的插值逼近计算表达式：

其中，B_i是非线性观测方程的Chebyshev多项式系数，表示基于系统状态变量一步预测值的Chebyshev多项式，为极小化插值余项算子，且：

所述利用椭球将线性化误差外包得到非线性误差的状态方程和观测方程的线性化误差的外包椭球的方法是：

利用Chebyshev插值多项式逼近操作获得插值余项算子作为Lagrange余子，计算逼近误差边界，用椭球形状将状态方程的Chebyshev多项式逼近误差外包：

获得状态方程逼近误差的外包椭球为其中，表示Chebyshev多项式逼近的系统过程模型状态方程的不确定性噪声方差矩阵，表示系统Chebyshev多项式逼近中的不确定性噪声方差矩阵的对角元素；

用椭球形状将观测方程的Chebyshev多项式逼近误差外包：

获得观测方程的线性化误差的外包椭球为其中，表示Chebyshev多项式逼近的观测方程不确定性噪声的方差矩阵，表示Chebyshev多项式逼近中造成的观测方程不确定性噪声方差矩阵的对角元素。

所述计算虚拟过程的误差椭球的方法是：Chebyshev多项式插值逼近造成的不确定性误差椭球和过程噪声相加的两个椭球直和运算；通过逼近不确定性误差和过程噪声的直和计算获得虚拟噪声误差椭球；

对非线性过程的状态方程x_k＝f(x_k-1)+w_k-1计算虚拟过程的状态噪声误差椭球为

其中，Q_k-1表示第k-1步的系统状态噪声包络矩阵，是由椭球形状的系统Chebyshev多项式插值逼近计算的不确定性误差和过程噪声相加获得的，表示第k-1步的系统噪声误差椭球方差矩阵直和，且：

其中，为过程噪声方差直和计算的尺度因子，且

对于非线过程的性观测方程z_k＝h(x_k)+v_k计算虚拟观测噪声误差椭球

其中，表示获得的虚拟观测噪声方差矩阵直和，且：

其中，是观测噪声方差矩阵直和计算的尺度因子参数，

所述利用线性化椭球集员滤波算法的预测步骤计算预测状态椭球边界的方法是：利用第k-1步的系统状态变量估计值和Chebyshev多项式逼近法展开计算第k步的状态预测值，获得状态变量线性化预测值及其外包预测椭球，开展线性化预测椭球和虚拟过程噪声椭球的直和运算，获得系统状态变量的预测椭球边界；

在第k-1步获得系统状态变量估计值利用Chebyshev多项式逼近计算第k步的系统状态变量预测值，根据系统均值计算公式可有：

上式中设根据Chebyshev多项式性质可以进一步整理E_j项为：

其中，P_x,k-1表示第k-1步系统状态变量的椭球包络矩阵，Π(·)表示系统状态变量的概率分布特征，利用E₀＝1，直至E_n项表达式获得线性方程为：

其中，R是一个(n-1)×(n-1)的矩阵，且其元素满足

获得第k-1步的预测值表达式：

其中，是直到n的系统状态向量的非中心矩构造的向量，且：

表示Chebyshev多项式的系数构造的向量，且：

A_n＝[A₀，A₁，…，A_n]^T，

Π_n是由Chebyshev多项式系数构造的(n+1)×(n+1)的矩阵，且：

П_n＝[α_0,n,α_1,n,…,α_n,n]^T，

组成第i项Chebyshev多项式的所有的直到n次单项式的系数：

且有递推表达公式为：

其起始项α_0,n＝[1,0，…，0]^T和α_1,n＝[0，1,0，…，0]^T，经由前面两项系数向量可以递推出直到n的所有系数向量；对于系统状态变量的方差计算，可以经由方差矩阵的计算公式获得：

对上式中的均值进行化简得：从而获得系统状态变量的预测状态椭球

其中，A_k,n表示整理获得的Chebyshev多项式系数向量，表示系统状态变量在第k-1步的估计误差非中心矩向量，⊙表示Kronecker乘积，P_2n是一个(n+1)²×(2n+1)的矩阵，其表达式为：β_k-1为尺度因子参数。

所述利用线性椭球集员滤波算法的更新步骤更新状态椭球边界的方法是：利用系统观测向量序列开展预测状态椭球和观测向量集合的交集计算；

将预测状态椭球和观测向量集合做直和交集计算，其中观测集合为：

采用Chebyshev多项式Kalman滤波算法计算系统状态变量的观测更新计算过程，观测向量的一步预测计算为

其中，由系统状态变量的直到n的所有的非中心矩组成，是观测方程的Chebyshev多项式的系数组成的向量；

相应的观测方程的观测向量一步预测方差可计算为：

那么系统状态变量和观测向量的协方差可计算为

从而可以获得

其中，表示系统观测变量在第k-1步的预测误差非中心矩向量，表示直到2n阶次的系统观测变量在第k-1步的预测误差非中心矩向量，是一个(n+1)×(n+2)的矩阵，其表达式为

其中，表示系统状态变量的取值区间范围，若那么

10、根据权利要求8所述的基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法，其特征在于，所述利用线性椭球集员滤波算法的状态估计计算步骤完成系统状态变量k时刻的估计计算和估计方差矩阵计算的方法是：

其中，δ_k为算法健康度因子，其表达式为：表示k时刻系统状态变量估计误差包络矩阵计算的中间算子，且：

其中，z_k表示观测向量，Π_k,n是在第k步预测计算中由Chebyshev多项式系数构造的(n+1)×(n+1)的矩阵，W_k表示第k步的系统观测向量的一步预测误差矩阵，K_k表示滤波算法的增益矩阵，ρ_k为预测误差包络矩阵的调节尺度因子参数。

本发明采用Chebyshev多项式插值逼近的扩展椭球集员滤波算法对大方位失准角初始对准非线性误差模型的状态变量参数的估计计算，利用Chebyshev多项式具有极小化的逼近误差的优点，方位失准角收敛速度很快，估计误差获得快速收敛，并且数值计算的稳定性较好，没有出现估计计算数据发散现象，计算效能较好。本发明采用Chebyshev多项式插值实施线性化逼近操作，有效避免Taylor级数展开式的一阶Jacobian矩阵和二阶Hessian矩阵的复杂计算，降低了算法的计算复杂度；相比于Taylor级数扩展的传统非线性集员滤波方法，本发明计算精度较高，并且有效保证了扩展椭球集员滤波算法的计算稳定性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明Chebyshev多项式逼近非线性系统函数的矩信息计算的流程图。

图2是本发明的流程图。

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

图4是导航系统姿态失准角状态估计的误差数据曲线图(Chebyshev多项式法)。

图5是导航系统速度状态估计的误差数据曲线图(Chebyshev多项式法)。

图6是导航系统姿态失准角状态估计的误差数据曲线图(Taylor级数扩展法)。

图7是导航系统速度状态估计的误差数据曲线图(Taylor级数扩展法)。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1和2所示，一种基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法，首先建立非线性捷联惯性导航系统方程和观测方程；计算第k-1步的系统状态参数分量的不确定区间；基于Chebyshev多项式插值逼近对接连惯性导航系统的非线性方程和观测方程实施Chebyshev多项式逼近计算，确定Lagrange余子的取值区间；接着计算Chebyshev多项式插值逼近计算的误差边界，获取非线性系统方程和观测方程的Chebyshev多项式插值逼近计算误差的外包椭球，从而开展虚拟过程噪声误差椭球和虚拟观测噪声椭球计算；利用线性椭球集员滤波算法的预测步骤计算预测状态变量的椭球边界；利用线性椭球集员滤波算法的更新步骤更新计算状态椭球边界；再利用线性椭球集员滤波算法的状态估计步骤计算第k步的状态参数变量的估计值和估计方差矩阵，从而完成捷联惯性导航系统状态参数变量的估计计算任务。本发明利用Chebeshev多项式逼近计算实现非线性的扩展椭球集员滤波方法，有效减小了基于Taylor级数的扩展表达式计算的复杂性和计算量。

假设非线性捷联惯性导航系统方程为f(x)，其可以被切比雪夫(Chebyshev)多项式和来任意逼近，如

其中，T(x)＝{T_n(x)，n＝0,…,∞}表示包含变量x的切比雪夫多项式的各项，{A_i,i＝0,1,2,…}是切比雪夫多项式系数，其计算表达式为：

且切比雪夫多项式具有正交性，切比雪夫多项式是采用n次的切比雪夫多项式加权代数式来逼近任意的非线性函数，这些加权多项式满足正交特性，其正交性可表达为：

相邻的三个切比雪夫多项式具有递推关系，可以表达为：

并且切比雪夫多项式T_n(x)具有奇偶性，满足：

T_n(-x)＝(-1)ⁿT_n(x)，>

切比雪夫多项式满足T(x)∈[-1,1]取值区间，并且在此区间中T(x)具有n个不同的实数零点，可由这些零点k＝1,2,···,n实施切比雪夫多项式插值逼近计算操作。

根据Chebyshev多项式的奇偶性质及取值特性，Chebyshev多项式还可以写为：

其中，α_n,i表示n次Chebyshev多项式的第i阶单项式的系数，α_n,n-2i也是同样的意义，表示n次Chebyshev多项式的第n-2i阶单项式的系数，式中[n/2]表示取整数，从而也可以获得两项Chebyshev多项式的乘积表达式为

同时，Chebyshev多项式的函数在区间[-1,1]上交替出现n+1个极值点组，其最大值为1，最小值为-1。Chebyshev多项式的最高次项系数为2^n-1，n＝1,2,···。从而Chebyshev多项式存在着与零偏差最小的特性，并且其偏差为依此特性可以在Chebyshev多项式逼近非线性函数过程中获得多项式插值余项的极小值，这有助于有效改善扩展集员滤波算法的计算精度；且其计算相对简单，计算效率可以得到有效的提高，能够满足SINS导航系统初始对准的快速计算要求，完成对舰船SINS导航系统初始对准大方位失准角模型状态参数的估计计算任务。

在实际应用过程中，一般取有限项Chebyshev多项式插值逼近非线性系统函数，式(1)可以表达为有限项的Chebyshev多项式逼近为：

其中，R_n+1(x)表示非线性系统函数的Chebyshev多项式插值逼近计算的余项；其Chebyshev多项式系数表达式(2)可写为：

且满足收敛性质为了后面便于利用Chebyshev多项式展开系统状态变量的矩信息的插值逼近计算，在n+1个插值点上我们对Chebyshev多项式的正交性表达式(5)整理为：

另外应该说明的是为了计算方便性，Chebyshev多项式插值逼近表达式中Chebyshev多项式系数在插值点上的计算表达式为：

其中，δ_0,n表示Kroneckerδ函数，满足

还要说明的是一般情况下系统状态变量的取值区间不在[-1,1]区间，这时需要做系统状态变量的变量替换，若系统状态变量取值区间为[a,b]，一般可采用变量替换表达式：

那么，相应的系统变量x'∈[-1,1]，则系统替换变量的均值和方差分别可变换为：

其中，E[x]表示原变量x的均值，P^x表示原变量x的方差。系统状态变量经由变量替换后参与迭代计算，获得替换变量的矩信息计算结果后，替换变量再转换到原系统变量，其变换公式为

那么相应的插值节点变换为

由此我们可以开展Chebyshev多项式插值逼近非线性函数的计算工作。

下面详细阐述一下本发明的具体内容。

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

设计一种捷联惯性导航非线性系统状态方程和观测方程，如后面的应用实例所述，其可以总结为

其中，x_k∈Rⁿ表示系统第k步的状态向量和z_k∈R^m表示系统第k步的观测向量，这里说明系统状态变量x的下标k表示迭代的第k步，本发明申请书的下标表示都一致。f(·)为系统状态方程的函数，h(·)为系统观测方程的函数，f(·)和h(·)是已知的系统非线性可导函数。w_k∈Rⁿ表示第k步的过程噪声，v_k∈R^m表示第k步的观测噪声，其随时间变化，且满足未知但有界(UBB)的假设条件。记w_k∈(0,Q_k)和v_k∈(0,R_k)，Q_k表示第k步的系统状态噪声包络矩阵，R_k表示第k步的系统观测噪声包络矩阵为第k步的系统状态噪声误差矩阵，Q_k表示第k步的系统状态噪声包络矩阵，R_k表示第k步的系统观测噪声包络矩阵；n表示系统状态变量的维数，m表示观测变量的维数；且过程噪声满足|w_k,i|≤1,i＝1,2,…,n，观测噪声满足|v_k,j|≤1,j＝1,2,…,m；且n表示系统状态变量的维数，m表示观测变量的维数。捷联导航系统状态变量的初始状态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时刻非线性椭球集员滤波算法的迭代计算过程由步骤二至步骤八组成。

步骤二：据第k-1步迭代计算获得系统状态变量的均值和方差，确定第k-1步组合导航系统状态参数向量的状态分量的不确定区间。

根据第k-1步的系统状态向量的估计值和估计方差矩阵来确定当前时刻系统状态变量的不确定区间，第k-1步系统状态参数向量的状态分量的不确定区间为：其中i＝1,2,…,n，表示第k-1步椭球包络矩阵P_k-1的(i，i)元素，表示第k-1步的状态变量的估计值，l是一个正实数，它设置意义是保证第k-1步的系统状态参数估计值有99.7％的概率落在设定的状态变量取值区间之内，其一般取值为l≥3。

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

以当前第k-1步的系统状态变量估计值实施Chebyshev插值多项式扩展，取Chebyshev多项式插值误差，或者称之为插值余项，Lagrange余子项作为非线性系统方程状态变量的不确定区间。利用Chebyshev多项式逼近表达系统非线性状态方程，在逼近过程中产生逼近误差，确定利用Chebyshev多项式插值逼近获得的Lagrange余子的最大区间，以第k-1步状态估计点做Chebyshev插值多项式获得系统过程函数的逼近表达式。

根据捷联惯性导航系统的非线性误差状态方程x_k＝f(x_k-1)+w_k-1，根据Chebyshev多项式的插值余项极小化性质，利用Chebyshev插值多项式获得线性化逼近生成的Lagrange余子的极小化区间，以第k-1步状态变量的估计点作为Chebyshev插值多项式逼近系统状态方程的n阶Chebyshev插值表达式获得第k步状态变量的预测点

其中，表示第i项的Chebyshev多项式，A_i表示Chebyshev多项式系数，表示Chebyshev多项式逼近余项。。上式表示，当系统状态变量x_k-1∈[-1,1]区间时，Chebyshev插值多项式的前n项，其余高阶项统一定义为插值余项其表达式为：

根据Chebyshev插值多项式的性质，当插值节点取Chebyshev多项式的零点值时，插值余项获得极小值，也就是：

若第k-1步系统状态变量的取值区间为x_k-1∈[a_k-1,b_k-1]，经由式(12)变换可以获得极小化的插值逼近误差余项为：

Chebyshev插值多项式的这一性质对于提高和改善插值多项式逼近非线性系统函数的计算精度具有重要意义。

同样地，基于捷联惯性组合导航系统非线性误差的观测方程z_k＝h(x_k)+v_k，根据Chebyshev插值多项式性质，利用Chebyshev插值多项式获得插值逼近计算生成的Lagrange余子的极小化区间，以第k步状态变量的预测点作为Chebyshev插值多项式逼近获得观测方程的插值逼近计算表达式：

其中，B_i是非线性观测方程的Chebyshev多项式系数，表示基于系统状态变量一步预测值的Chebyshev多项式，为极小化插值余项算子，且：

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

利用Chebyshev插值多项式逼近操作获得插值余项算子作为Lagrange余子，计算逼近误差边界，用椭球形状将状态方程的Chebyshev多项式逼近误差外包：

获得状态方程逼近误差的外包椭球为其中，表示Chebyshev多项式逼近的系统过程模型状态方程的不确定性噪声方差矩阵，表示系统Chebyshev多项式逼近中的不确定性噪声方差矩阵的对角元素。

用椭球形状将观测方程的Chebyshev多项式逼近误差外包：

获得观测方程的线性化误差的外包椭球为其中，表示Chebyshev多项式逼近的观测方程不确定性噪声的方差矩阵，表示Chebyshev多项式逼近中造成的观测方程不确定性噪声方差矩阵的对角元素。

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

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

计算第k-1步的虚拟过程的状态噪声误差椭球为且

其中，表示第k-1步的系统噪声误差椭球方差矩阵直和，Q_k-1表示第k-1步计算的系统状态噪声包络矩阵，是由椭球形状的系统Chebyshev多项式插值逼近计算的不确定性误差和过程噪声相加获得的，涉及到两个椭球的直和计算：

其中，为过程噪声方差直和计算的尺度因子，且对于非线性观测方程z_k＝h(x_k)+v_k做上述计算步骤，计算虚拟观测噪声误差椭球

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

其中，是观测噪声方差矩阵直和计算的尺度因子参数，从而获得了得到系统观测噪声的虚拟噪声椭球其中，表示获得的虚拟观测噪声方差矩阵直和。

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

涉及到线性化预测椭球和虚拟过程噪声椭球的直和计算过程。利用第k-1步的系统状态变量估计值和Chebyshev多项式逼近法展开计算第k步的状态预测值，获得状态变量线性化预测值及其外包预测椭球，开展线性化预测椭球和虚拟过程噪声椭球的直和运算，获得系统状态变量的预测椭球边界。

线性化预测椭球和虚拟过程噪声直和计算过程。在第k-1步获得系统状态变量估计值利用Chebyshev多项式逼近计算第k步的系统状态变量预测值，根据系统均值计算公式可有：

定义根据Chebyshev多项式性质可以进一步整理E_j项为：

其中，P_x,k-1表示第k-1步系统状态变量的椭球包络矩阵，Π(·)表示系统状态变量的概率分布特征。从而可以利用E₀＝1，直至E_n项表达式获得一个线性方程为：

其中，R是一个(n-1)×(n-1)的矩阵，其元素满足矩阵R是一个下三角的稀疏矩阵。由此开展系统预测均值的计算任务。

那么，利用前面的公式可以很容易地获得第k-1步的预测值表达式：

其中，是直到n的系统状态向量的非中心矩构造的向量，定义为：

表示Chebyshev多项式的系数构造的向量，其定义为：

A_n＝[A₀，A₁，…，A_n]^T，>

Π_n是由Chebyshev多项式系数构造的(n+1)×(n+1)的矩阵，其定义为：

П_n＝[α_0,n,α_1,n,…,α_n,n]^T，

且有组成第i项Chebyshev多项式的所有的直到n次单项式的系数，

它还有递推表达公式为：

其起始项α_0,n＝[1,0，…，0]^T和α_1,n＝[0，1,0，…，0]^T，经由前面两项系数向量可以递推出直到n的所有系数向量。

对于系统状态变量的方差计算，可以经由方差矩阵的计算公式获得：

对于式(35)中的第二项是系统虚拟噪声的方差矩阵，下面讨论其第一项的计算方法。

其中可计算为：

其中，符号⊙表示Kronecker乘积，P_2n表示一个(n+1)²×(2n+1)的矩阵，其表达式为：由的所有乘积项组成的，A_k,n表示Chebyshev多项式系数构造的向量，表示系统状态变量在第k-1步的估计误差非中心矩向量。同时引入尺度因子参数β_k-1，从而可以获得虚拟过程噪声预测直和计算方差矩阵为：

从而获得系统状态变量的预测状态椭球

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

涉及到预测状态椭球和观测向量集合的交集计算，利用系统观测向量序列开展预测状态椭球和观测向量带的交集计算。

将预测状态椭球和观测集合S_y做直和交集计算，其中观测集合S_y为：

下面首先考虑采用Chebyshev多项式Kalman滤波算法计算系统状态变量的观测更新计算过程，观测向量的一步预测计算为：

其中，由系统状态变量的直到n的所有的非中心矩组成，是观测方程的Chebyshev多项式的系数组成的向量。相应的观测方程的观测向量一步预测方差可计算为：

其中，表示系统状态变量的第k步预测的直到2n的所有的非中心矩组成向量，那么系统状态变量和观测向量的协方差可计算为：

从而可以获得

其中，表示系统状态变量的第k步预测的直到n的所有的非中心矩组成向量，是一个(n+1)×(n+2)的矩阵，其表达式为：

其中，表示系统状态变量的取值区间范围，若那么

其中，z_k表示观测向量，Π_k,n是在第k步预测计算中的由Chebyshev多项式系数构造的(n+1)×(n+1)的矩阵，W_k表示第k步的系统观测向量的一步预测误差矩阵，K_k表示滤波算法的增益矩阵，ρ_k为预测误差包络矩阵的调节尺度因子参数。

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

其中，δ_k为算法健康度因子，其表达式为：

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

本发明的优势在于采用Chebyshev多项式插值实施线性化逼近操作，有效避免Taylor级数展开式的一阶Jacobian矩阵和二阶Hessian矩阵的复杂计算，降低了算法的计算复杂度；相比于Taylor级数扩展的传统非线性集员滤波算法，本发明的计算精度比较高，并且有效保证了扩展椭球集员滤波算法的计算稳定性。

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

尺度因子参数和β_k-1涉及到两个椭球直和运算的外包椭球最优化问题，这里选取外包椭球的最小化计算方法，该方法求解形式简单，相比较于最小化外包椭球体积的优化准则，该方法性能鲁棒性更强。即有从而可以采用式子获得最优的尺度因子参数和β_k-1，P₁和P₂表示泛指的任意两个方差矩阵。

尺度因子参数需要E(0,Q_k-1)和的直和计算，那么其计算准则式为其最优计算式为

对于尺度因子参数β_k-1，需要两个椭球和的直和计算，考虑观测向量更新条件下的方差矩阵计算式为：

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

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

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

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

具体实施算例：采用本发明开展对舰船SINS导航系统的初始对准大方位失准角模型状态参数的估计计算任务。该算例应用的大方位失准角初始对准模型可以参阅专著《非线性系统建模与滤波方法》一书，在这里仅作为本发明的验证实例，系统模型状态变量由三向失准角和两向速度变量组成随机系统状态向量，其方程为：

其中，系统状态变量为：噪声向量为并且系统观测向量设为东向和北向速度，获得线性观测函数，其观测矩阵为H＝[0_2×3>2×2]。

其中，SINS系统中导航解算更新采用如下式子(56)和(57)：

其中，表示由系统角速度向量组成的斜对称矩阵，gⁿ＝[0>T，表示系统IMU组件中加速度计的比力输出量，表示导航坐标系下的东向速度和北向速度组成的速度向量。仿真参数设置为：初始姿态误差角中滚转角和俯仰角分别设为3°和6°，方位失准角数值较大，设为27°；舰船初始东向航速为5m/s和北向航速为10m/s；陀螺仪漂移设为0.03°/h，随机漂移设为0.005°/h；加速度计零点偏差设为0.002gm/s²，随机噪声设为0.0005g>2。假设SINS系统中陀螺仪常值漂移ε向量和加速度计零偏误差向量分别符合一阶Markov模型噪声，其外定界椭球为Ε(0,Q_ε)和Q_ε表示系统IMU组件的陀螺仪随机漂移的椭球包络矩阵，Q_Δ表示系统IMU组件的加速度计随机漂移的椭球包络矩阵。导航速度观测方程中的速度误差噪声满足外定界椭球Ε(0,R)。在动基座条件下进行SINS系统初始对准状态参数估计计算仿真，舰船载体在海面上做机动转弯运行，其运行轨迹如图3所示，显示出了舰船初始所在位置坐标。

利用本发明的方法获得系统状态变量的估计计算结果，其中导航系统大方位失准角姿态失准角状态估计数据曲线、导航系统大方位失准角误差模型系统速度状态估计数据曲线、导航系统大方位失准角误差模型的陀螺仪参数估计误差数据曲线和本发明的导航系统大方位失准角误差模型的加速度计参数估计误差数据曲线分别如图4、图5、图6和图7所示。其中，图4显示的是惯导系统三个姿态失准角的估计误差数据，很明显可以看到，本发明对大方位失准角初始对准非线性误差模型的状态变量参数的估计计算其估计误差获得快速收敛，并且数值计算的稳定性较好，没有出现估计计算数据发散现象，这证明了本发明算法的优越计算效能，原因在于Chebyshev多项式逼近非线性函数过程中，相比于Taylor级数扩展来说，Chebyshev多项式具有极小化的逼近误差，从方位失准角估计数据也可以看出，方位失准角收敛速度很快，基本上可以和其他两向的姿态失准角同时收敛的，这充分体现了本发明的一个计算优势。同时，从图5的两向速度误差估计数据曲线也可以看出本发明算法的计算效能。从图6和图7的系统状态变量估计数据可知，本发明的数值计算效能优于Taylor级数扩展法。

本发明说明书中未作详细描述的内容属于本领域专业技术人员所公知的现有技术。应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或者变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。