一种惯性导航系统误差模型的降维RTS椭球集员平滑方法

摘要

本发明提出了一种惯性导航系统误差模型的降维RTS椭球集员平滑方法，用以解决现有椭球集员平滑方法计算复杂度大的问题。本发明基于惯性导航系统误差模型观测量噪声不满秩情形，对惯性导航系统误差模型方程实施降维操作，进而实现一种降维的惯性导航系统等价变换模型方程，达到减小系统状态变量计算量的目的；利用椭球集员平滑器算法实现等价变换惯性导航系统误差方程的系统状态向量的滤波平滑计算，进一步改善系统状态变量参数最优估计的精度要求。本发明具有较好的计算优势和计算效能，可应用于运动载体惯性导航系统误差模型中，实现惯性导航系统误差模型状态参数最优滤波平滑计算。

说明书

技术领域

本发明涉及导航制导与控制的技术领域，尤其涉及一种惯性导航系统误差模型的降维RTS椭球集员平滑方法，应用于运动载体惯性导航系统误差模型中，实现惯性导航系统误差模型状态参数最优的滤波平滑计算。

背景技术

根据估计时刻用到的观测量信息情况，最优估计理论分为预测、滤波和平滑三种类型，其中滤波计算是利用当前时刻以及以前时刻的所有观测信息对当前系统状态变量进行估计计算，而平滑方法除了利用滤波计算用到的观测信息，还要用到当前时刻以后的部分或者所有观测信息，因此理论上平滑方法是一种离线处理方法，在滤波计算基础上对系统状态变量进一步做出改善，从而获得更加精确的计算数据结果。

平滑算法和滤波算法一样，其理论基础也是基于Bayesians最优滤波理论，假设系统状态变量满足高斯分布。平滑估计算法一般可分为固定点平滑、固定滞后平滑和固定区间平滑等，其中固定区间平滑是利用某一时间区间内的所有观测信息对所有状态变量进行估计计算的一种方法，其应用范围最为广泛，而Rauch-Tung-Striebel平滑算法就是一种固定区间平滑计算方法，但是传统的平滑算法都是针对线性系统状态变量开展滤波后的平滑操作，但是近年来也有很多种非线性平滑算法，如其中应用最广泛的基于Taylor级数扩展表达式的一阶或者二阶RTS非线性平滑算法，被称为扩展RTS平滑算法，以及基于Sigma点非线性RTS平滑算法、中心差分RTS平滑算法、Gauss-Hermite数值积分逼近的RTS平滑算法和容积RTS平滑算法等。

发明内容

针对现有扩展RTS平滑方法计算复杂度高的技术问题，本发明提出一种惯性导航系统误差模型的降维RTS椭球集员平滑方法，在椭球集员滤波理论基础上，利用椭球集员计算的UBB特性，改善平滑算法计算效能，具有较好的计算优势与计算效能。

为了达到上述目的，本发明的技术方案是这样实现的：一种惯性导航系统误差模型的降维RTS椭球集员平滑方法，其步骤如下：

步骤一：构建惯性导航系统误差模型的状态方程和观测方程；

步骤二：基于观测矩阵不满秩情形，利用非奇异矩阵对惯性导航系统误差模型实施降维等价变换，获得维数减小的惯性导航系统等价变换线性系统的模型方程；

步骤三：对等价变换线性系统的状态变量参数开展椭球集员滤波迭代计算，获得各时刻的等价的状态变量的滤波估计椭球形状；经由n＝T步迭代计算获得第T步的最优滤波椭球形状，确定等价变换线性系统的状态变量的估计均值和估计方差矩阵，并存储各个时刻的估计椭球形状数据；

步骤四：从n＝T开始，对步骤三获取的各个时刻滤波数据开展逆向平滑操作，基于等价变换线性系统的方程，根据第T步滤波椭球数据计算平滑算法非零噪声的观测向量的预测椭球，进而确定预测均值及其预测方差矩阵；

步骤五：令n＝T-1，根据第T步的估计数据开展等价变换线性系统的状态变量的RTS椭球平滑计算，获得第T-1步的等价变换系统的状态变量的平滑椭球形状数据，确定平滑增益矩阵；进而确定等价变换线性系统的状态变量的平滑均值和方差矩阵；直至计算n＝0步的椭球平滑数据，从而完成等价变换惯性导航系统的状态参数变量的平滑计算任务。

所述步骤一中惯性导航系统误差模型的状态方程和观测方程为：

其中，是第n步的系统状态变量，是第n步的系统观测变量，和分别表示系统的高斯过程噪声和观测噪声，n＝1,2,…,T，T表示迭代计算次数；和分别表示n_x、n_y、n_u、n_v的实数空间；F_n表示状态转移矩阵、G_n表示过程噪声矩阵、H_n为观测矩阵、J_n表示观测噪声矩阵；且u＝{u_n}_n∈N和w＝{w_n}_n∈N都零均值独立或者联合独立于系统状态变量初值x₀，系统状态变量x_n表现为马尔科夫链过程，(x_n,y_n)是一个具有独立噪声的隐马科夫链过程，f(·)和h(·)分别表示系统的动态模型函数和观测模型函数。

所述步骤二中获得降维等价变换线性系统的方法为：构造非奇异矩阵M_n满足：

其中，m是等价变换线性系统的状态变量的维数，且m≤n_x，0_m表示m×m的零矩阵，R_n表示具有m个零特征值的观测噪声方差矩阵，I_r表示r维单位矩阵；

令那么惯性导航系统误差模型的观测变量分解为：

其中，p表示观测方程中无噪声部分向量的维数，r表示有噪声部分向量的维数，和分别表示无噪声部分观测分量、有噪声部分观测分量；和分别表示观测矩阵经由非奇异矩阵变换后分解的对应矩阵和的观测矩阵分量；矩阵0的下标表示m×1的矩阵；

选择一个(n_x-m)×n_x的非奇异矩阵U_n对惯性导航系统误差模型的状态变量实施变换：

其中，表示经由非奇异矩阵变换后的系统状态变量；上式是可逆的，且其左边的等价变换矩阵为利用等价变换矩阵T_n和非奇异矩阵M_n对惯性导航系统误差模型做出等价变换，获得等价系统为：

其中，

根据状态变量实施变换和惯性导航系统误差模型的状态方程，整理获得：

因此，获得降维的等价变换线性系统：

其中，

定义联合系统状态变量那么获得联合系统状态变量满足初始椭球分布为其噪声项整理为：满足椭球形状分布其中，是等价变换线性系统的状态变量的椭球方差矩阵，是等价变换线性系统的观测变量的椭球方差，是等价变换线性系统的观测变量的椭球协方差矩阵。

从n＝T-1开始，利用第n步的椭球估计形状数据和P_nn，计算第n+1步导航系统等价观测变量的椭球形状中心平滑均值为：

计算等价变换线性系统的观测变量的平滑方差矩阵，从而确定系统状态变量的预测椭球，利用椭球直和计算公式：

其中，p_n∈[0,+∞)为优化参数。

所述优化参数p_n用于选择椭球的最小尺寸，选择最小化椭球迹来计算最优椭球，计算最优p_n值为：

计算平滑增益：获得等价变换线性系统的状态变量的平滑椭球形状数据，包括平滑均值及平滑方差的计算：

其中，参数λ_n+1表示为：参数Λ_n+1为：

所述参数λ_n和Λ_n+1从λ_N＝0、Λ_N＝0递推计算，其递推表达式为：

其中，参数β_n的表达式为：

q_n∈[0,∞)为尺度因子参数。

所述尺度因子参数q_n计算最小化椭球利用椭球最小迹规则尺度因子参数q_n满足：

其中，是矩阵的特征值，构造对角元素为的对角矩阵L，那么由特征值对应的特征向量构造的矩阵D可以实施分解操作，d_ii(·)表示矩阵的对角元素，m是等价变换系统状态变量的维数。

本发明的有益效果：基于惯性导航系统误差模型观测量噪声不满秩情形，对惯性导航系统误差模型方程实施降维操作，进而实现一种降维的惯性导航系统等价变换模型方程，达到减小系统状态变量计算量的目的；利用椭球集员平滑器算法实现等价变换惯性导航系统误差方程的系统状态向量的滤波平滑计算，从而达到进一步改善系统状态变量参数最优估计的精度要求。经由运动载体惯性导航系统仿真实验，并与椭球集员滤波算法对比，验证了本发明的计算优势和计算效能。

附图说明

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

图1为本发明的结构示意图。

图2是运动载体惯性导航系统运动轨迹图。

图3是运动载体位置误差数据曲线。

图4是运动载体速度误差数据曲线。

图5是运动载体姿态失准角数据曲线。

具体实施方式

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

本发明提出的一种惯性导航系统误差模型的降维RTS椭球集员平滑方法，经由系统线性函数的降维计算获得系统线性函数的等价变换系统，进而对等价变换系统开展椭球集员最优平滑迭代计算，最终获得原系统状态变量的最优平滑结果。以惯性导航系统误差模型离散化的随机状态空间模型，包括状态方程和观测方程，

其中，是第n步的系统状态变量，是第n步的系统观测变量，和分别表示系统的高斯过程噪声和观测噪声，n＝1,2,…,T，T表示迭代计算次数；和分别表示n_x、n_y、n_u、n_v的实数空间；F_n表示状态转移矩阵、G_n表示过程噪声矩阵、H_n为观测矩阵、J_n表示观测噪声矩阵；。并且u＝{u_n}_n∈N和w＝{w_n}_n∈N都零均值独立或者联合独立于系统状态变量初值x₀，系统状态变量x_n表现为马尔科夫链(MarkovChain,，MC)过程，(x_n,y_n)是一个具有独立噪声的隐马科夫链(Hidden>

平滑算法一般都是和滤波计算交互迭代使用，用于改善滤波获得结果的再平滑计算过程，平滑计算在数学上就是在给定所有观测向量{y_n}，T>n基础上的系统状态变量的条件概率分布p(x_n|y₁:_T)，n＝1,2,…,T集合，在本发明中利用高斯分布假设开展RTS椭球集员平滑方法的推理过程，也就是系统状态变量满足高斯正态分布：

p(x_n|y_1:T)＝N(x_n|m_n|T,P_n|T)(2)

其中，m_n|T是系统状态变量的均值向量，P_n|T是第n步平滑计算获得的逼近高斯分布系统的状态变量的方差矩阵。但是在滤波和平滑操作中都需要计算系统线性函数的一阶和二阶矩。针对满足高斯分布的泛指线性向量函数g(x)，其均值、方差和协方差分别为：

这里给出Bayesian平滑的理论计算步骤，假设已经获得了系统第n步的状态变量的滤波解的高斯逼近：

p(x_n|y_1:n)＝N(x_n|m_n|n,P_n|n)(6)

那么泛指线性函数的高斯一阶矩(期望值)可表示为：

平滑方法式从第n＝T步的滤波结果开始平滑计算，直至n＝0为止，从而高斯分布的Bayesian平滑计算框架为：

其中，m_n|n表示n时刻(第n步)的系统状态变量的估计值、m_n+1|n表示k+1时刻(第k+1步)的系统状态变量的预测值、P_n|n表示n时刻的系统状态变量的估计方差、P_n+1|n表示k+1时刻的系统状态变量的预测方差，而则表示其逆矩阵、P_n|T表示自T步骤开始的第n次的平滑迭代方差、P_n+1|T表示n+1次的平滑方差、Q_n表示n时刻的系统噪声方差、C_n+1|n表示平滑的协方差矩阵、G_k表示平滑增益矩阵，而表示平滑增益矩阵的转置矩阵。

最终获得系统状态变量逼近计算的平滑分布：

p(x_n|y_1:T)＝N(x_n|m_n|T,P_n|T)(9)。

在本发明中考虑一种特殊情形，就是观测噪声方差矩阵R_n若是零矩阵情形，其实这种情形在很多实际应用场合中都会出现，尤其是考虑有色加性观测噪声。若考虑一类自回归观测噪声，满足：

w_n＝A_nw_n-1+ξ_n(10)

初始噪声为w₀＝ξ₀，其中，噪声变量ξ_n是零均值的，且状态变量初值x₀、过程噪声{u_n}_n≥0和噪声变量{ξ_n}_n≥0是独立的，那么可以把线性系统状态变量扩展为那么误差模型的状态空间模型可整理为无观测噪声系统：

其中，A_n表示噪声映射矩阵。这里假设矩阵J_n是n_y×n_y的单位阵，且噪声变量ξ_n满足方差矩阵对线性系统的状态空间模型再次做出变换整理获得：

其中，等效噪声是独立的，且其方差

在系统(1)及变换系统(11)和(12)中，状态变量初值x₀、过程噪声{u_n}_n≥0和噪声变量{ξ_n}_n≥0是独立的，暗含着过程噪声{u_n}_n≥0和{w_n}_n≥0也是独立的，但是针对同源过程噪声和观测噪声，其部分向量可能是相关的。可以假设u_n和w_n+p(p>0)是相关的，并且这里考虑观测噪声方差矩阵R_n是半正定矩阵情形，有R_n≥0，从而考虑部分无噪声的观测变量情形，可以令其中部分变量是无噪声变量，从而可以对观测噪声进行分解操作：

其中，从而可以构建扩展系统状态变量依次扩展状态变量可以获得线性系统(1)的变换模型(11)的具体表达式：

下面依次开展线性系统(1)的降维计算操作，若观测噪声方差矩阵R_n≥0，其具有r＝rank(R_n)∈{0,1，…,n_y}的性质，可以依次将线性系统的状态变量x_n的维数利用m＝n_y-r降低到r阶，从而实现线性系统状态变量椭球集员平滑操作的计算量得到降低，减小算法计算量，优化椭球集员平滑方法的目的。根据观测噪声方差矩阵秩性质，R_n具有m个零特征值，那么存在着一个非奇异矩阵M_n满足：

其中，0_m表示m×m的零矩阵，令那么从线性系统(1)可以获得观测变量分解为：

其中，这些变量或者矩阵都被分为无噪声部分和有噪声部分，一旦无噪声部分观测变量已知的话，相应的m个系统状态变量x_n就是已知的了，因此仅仅对有噪声观测变量部分开展平滑操作即可完成线性系统状态变量的迭代计算，这样可以明显降低平滑算法的计算量，提高和改善算法的计算效能。公式中0的下标表示m×1的矩阵。

很明显，在转换观测方程(14)中，m≤n_x，且可以看出可以选择一个(n_x-m)×n_x的非奇异矩阵U_n实施变换：

应该说明的是式(15)是可逆的，且其左边矩阵可表达为从而可以利用矩阵T_n和非奇异矩阵M_n对线性系统(1)做出等价变换，获得等价系统为：

且其中

另外根据式(15)和线性系统(1)中的过程方程，可以整理获得：

从而综合式(16)和式(17)，可以获得降维等价变换线性系统：

其中，定义

那么可以针对降维等价变换系统模型(18)开展椭球集员RTS平滑计算处理。

本发明采用观测变量的无噪声和有噪声分解策略对惯性导航系统问题实施降维操作，获得等价降维惯性导航系统模型方程，对等价系统模型方程实施椭球集员滤波算法迭代计算，获得系统状态变量的T步滤波数据并保存，根据获得的滤波计算数据开展反向等价惯性导航系统状态向量的滤波平滑计算，从而达到进一步改善惯性导航系统状态变量参数最优估计的精度要求。

如图1所示，一种惯性导航系统误差模型的降维RTS椭球集员平滑方法，实施步骤为：首先建立惯性导航系统误差模型的系统方程和观测方程；开展惯性导航系统方程的等价变换，获得惯性导航系统等价变换线性系统的模型方程，进而开展状态变量参数的椭球集员滤波计算，经由n＝T步迭代计算获得第T步的最优滤波结果估计均值m_T|T和估计方差矩阵P_T|T，并存储各个时刻的估计数据m_n|n和P_n|n；然后从n＝T-1开始，利用各时刻滤波估计数据作为观测数据和等价变换线性系统的模型方程开展椭球集员RTS平滑计算，获得平滑算法的预测均值和预测方差矩阵P_n+1|n，构造平滑预测椭球接着计算RTS椭球集员平滑增益G_k，获取等价变换线性系统的状态方程和观测方程的平滑均值及平滑方差矩阵P_n|0:T，从而获得最终的等价变换系统状态变量的平滑椭球从而完成等价变换惯性导航系统问题的系统状态参数变量的估计计算任务。具体步骤为：

步骤一：构建惯性导航系统误差模型的状态方程和观测方程，如公式(1)所示。

步骤二：基于观测矩阵不满秩情形，利用非奇异矩阵对惯性导航系统误差模型实施降维等价变换，获得维数减小的惯性导航系统等价变换线性系统的模型方程。

根据惯性导航系统的观测向量的噪声分布情况对其实施降维操作，获得降维等价变换线性系统的模型方程如式(18)所示，其中利用了满足式(14)的非奇异矩阵M_n进行等价变换；等价变换矩阵T_n为：

同时，定义联合系统状态变量那么获得联合系统状态变量满足初始椭球分布为其噪声项整理为：

满足椭球形状分布其中，是等价变换线性系统的状态变量的椭球方差矩阵，是变换模型方程的观测变量的椭球方差，是变换模型方程的观测变量的椭球协方差矩阵。

步骤三：对等价变换线性系统的状态变量参数开展椭球集员滤波迭代计算，获得各时刻的等价的状态变量的滤波估计椭球形状；经由n＝T步迭代计算获得第T步的最优滤波椭球形状，确定等价变换线性系统的状态变量的估计均值和估计方差矩阵，并存储各个时刻的估计椭球形状数据。

根据降维等价变换线性系统的方程模型(18)，对其做出椭球集合计算操作。假设经由n＝T步的椭球集员滤波迭代计算后获得系统状态变量的最优滤波结果为估计均值m_T|T和估计方差矩阵P_T|T，并构造各个时刻的系统状态变量的椭球形状E(m_n|n,P_n|n)，并存储各个时刻的系统状态变量估计椭球形状E(m_n|n,P_n|n)，利用系统状态变量滤波获得估计均值作为系统状态变量平滑计算的观测数据开展椭球集员平滑计算。

步骤四：从n＝T开始，对步骤三获取的各个时刻滤波数据开展逆向平滑操作，基于等价变换线性系统的方程，根据第T步滤波椭球数据计算平滑算法非零噪声的观测向量的预测椭球，进而确定预测均值及其预测方差矩阵。

从n＝T-1开始，利用第n步的椭球估计形状数据和P_n|n，计算第n+1步导航系统等价观测变量的椭球形状中心平滑均值为，

步骤五：令n＝T-1，根据第T步的估计数据开展等价变换线性系统的状态变量的RTS椭球平滑计算，获得第T-1步的等价变换系统的状态变量的平滑椭球形状数据，确定平滑增益矩阵；进而确定等价变换线性系统的状态变量的平滑均值和方差矩阵；直至计算n＝0步的椭球平滑数据，从而完成等价变换惯性导航系统的系统状态参数变量的平滑计算任务。

计算等价变换线性系统联合状态变量中的观测变量的平滑方差矩阵，从而确定系统状态变量的预测椭球，利用椭球直和计算公式：

其中，优化参数p_n∈[0,+∞)用于选择椭球的最小尺寸，如最小迹或者最小容积。本发明选择最小化椭球迹来计算最优椭球，计算最优p_n值为：

计算平滑增益：

获得等价变换线性系统的状态变量的平滑椭球形状数据，就是平滑均值及其平滑方差计算：

其中，参数λ_n+1表示为：

参数Λ_n+1表达式为：

其实式(27)和(28)表达的参数λ_n和Λ_n可以从λ_N＝0、Λ_N＝0递推计算，其递推表达式为：

其中，引入了参数β_n的表达式为：

这里引入了尺度因子参数q_n∈[0,∞)来计算最小化椭球利用椭球最小迹规则尺度因子参数q_n满足：

其中，是矩阵的特征值，构造对角元素为的对角矩阵L，那么由特征值对应的特征向量构造的矩阵D可以实施分解操作，d_ii(·)表示矩阵的对角元素，m是等价变换系统状态变量的维数。

经由每一步的后向递推计算获得参数λ_n和Λ_n的数据，代入式(25)和(26)进行等价变换线性系统的状态变量的平滑均值和平滑方差矩阵计算，最终获得等价变换线性系统的N步平滑计算的椭球形状从而完成RTS固定区间椭球集员平滑算法的迭代计算过程。

具体实例：考虑运动载体的惯性导航系统问题，在笛卡尔坐标系下可以给出惯性导航系统方程为：

这里惯性导航系统状态向量为其中，δp＝[δL,δλ,δh]^T表示运动载体位置误差，表示运动载体姿态失准角，ε^b表示载体系中陀螺仪常值误差，表示载体系中加速度计零位偏差，δv^b表示运动载体速度误差。

假设陀螺仪和加速度计的输出为和那么相应的误差项为

其中，矩阵中的δk_gii,(i＝x,y,z)是陀螺仪尺度因子误差，δk_gij,(i,j＝x,y,z,i≠j)陀螺仪相对于理想载体轴的失准角，

δk_aii,(i＝x,y,z)是加速度计的尺度因子误差，δk_aij,(i,j＝x,y,z,i≠j)是加速度计相对于载体轴的失准角。系统噪声为噪声向量w_n是高斯过程噪声，v_k～N(0,Q_k)，其中Q_k表示噪声方差。

本实施例中采用位置和速度观测获得观测方程为：

y＝Hx+v (36)

其中，H＝[0_6×3>6×6>6×6]，v_n是观测白噪声，满足分布v_n～N(0,R_n)，其中，R_n表示观测噪声的方差。由此展开仿真验证工作，获得如图2所示的载体运行轨迹图，如图3所示的运动载体位置的误差图，如图4所示运动载体速度误差图和如图5所示运动载体姿态失准角平滑图。和椭球集员滤波数据进行比较，很明显滤波和平滑算法中RTS椭球集员平滑算法与运动载体真实运行数据拟合程度较好，而滤波算法的拟合程度差一些；并且RTS椭球集员平滑算法的计算标准差比较小，误差数据曲线平滑稳定，而椭球集员滤波算法的数据变化剧烈，明显误差数据比较大，通过利用这两种平滑和滤波算法开展运动载体系统仿真实验，获得的实验数据说明，本发明的计算效能优于椭球集员滤波算法，表现出了RTS椭球集员平滑器算法的计算优势。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。