CN108507593A - 一种惯性导航系统误差模型的降维rts椭球集员平滑方法 - Google Patents
一种惯性导航系统误差模型的降维rts椭球集员平滑方法 Download PDFInfo
- Publication number
- CN108507593A CN108507593A CN201810309501.6A CN201810309501A CN108507593A CN 108507593 A CN108507593 A CN 108507593A CN 201810309501 A CN201810309501 A CN 201810309501A CN 108507593 A CN108507593 A CN 108507593A
- Authority
- CN
- China
- Prior art keywords
- matrix
- ellipsoid
- ins errors
- dimensionality reduction
- smoothing
- 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.)
- Granted
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)
- Complex Calculations (AREA)
Abstract
本发明提出了一种惯性导航系统误差模型的降维RTS椭球集员平滑方法,用以解决现有椭球集员平滑方法计算复杂度大的问题。本发明基于惯性导航系统误差模型观测量噪声不满秩情形,对惯性导航系统误差模型方程实施降维操作,进而实现一种降维的惯性导航系统等价变换模型方程,达到减小系统状态变量计算量的目的;利用椭球集员平滑器算法实现等价变换惯性导航系统误差方程的系统状态向量的滤波平滑计算,进一步改善系统状态变量参数最优估计的精度要求。本发明具有较好的计算优势和计算效能,可应用于运动载体惯性导航系统误差模型中,实现惯性导航系统误差模型状态参数最优滤波平滑计算。
Description
技术领域
本发明涉及导航制导与控制的技术领域,尤其涉及一种惯性导航系统误差模型的降维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表示迭代计算次数;和分别表示nx、ny、nu、nv的实数空间;Fn表示状态转移矩阵、Gn表示过程噪声矩阵、Hn为观测矩阵、Jn表示观测噪声矩阵;且u={un}n∈N和w={wn}n∈N都零均值独立或者联合独立于系统状态变量初值x0,系统状态变量xn表现为马尔科夫链过程,(xn,yn)是一个具有独立噪声的隐马科夫链过程,f(·)和h(·)分别表示系统的动态模型函数和观测模型函数。
所述步骤二中获得降维等价变换线性系统的方法为:构造非奇异矩阵Mn满足:
其中,m是等价变换线性系统的状态变量的维数,且m≤nx,0m表示m×m的零矩阵,Rn表示具有m个零特征值的观测噪声方差矩阵,Ir表示r维单位矩阵;
令那么惯性导航系统误差模型的观测变量分解为:
其中,p表示观测方程中无噪声部分向量的维数,r表示有噪声部分向量的维数,和分别表示无噪声部分观测分量、有噪声部分观测分量;和分别表示观测矩阵经由非奇异矩阵变换后分解的对应矩阵和的观测矩阵分量;矩阵0的下标表示m×1的矩阵;
选择一个(nx-m)×nx的非奇异矩阵Un对惯性导航系统误差模型的状态变量实施变换:
其中,表示经由非奇异矩阵变换后的系统状态变量;上式是可逆的,且其左边的等价变换矩阵为利用等价变换矩阵Tn和非奇异矩阵Mn对惯性导航系统误差模型做出等价变换,获得等价系统为:
其中,
根据状态变量实施变换和惯性导航系统误差模型的状态方程,整理获得:
因此,获得降维的等价变换线性系统:
其中,
定义联合系统状态变量那么获得联合系统状态变量满足初始椭球分布为其噪声项整理为:满足椭球形状分布其中,是等价变换线性系统的状态变量的椭球方差矩阵,是等价变换线性系统的观测变量的椭球方差,是等价变换线性系统的观测变量的椭球协方差矩阵。
从n=T-1开始,利用第n步的椭球估计形状数据和Pnn,计算第n+1步导航系统等价观测变量的椭球形状中心平滑均值为:
计算等价变换线性系统的观测变量的平滑方差矩阵,从而确定系统状态变量的预测椭球,利用椭球直和计算公式:
其中,pn∈[0,+∞)为优化参数。
所述优化参数pn用于选择椭球的最小尺寸,选择最小化椭球迹来计算最优椭球,计算最优pn值为:
计算平滑增益:获得等价变换线性系统的状态变量的平滑椭球形状数据,包括平滑均值及平滑方差的计算:
其中,参数λn+1表示为:参数Λn+1为:
所述参数λn和Λn+1从λN=0、ΛN=0递推计算,其递推表达式为:
其中,参数βn的表达式为:
qn∈[0,∞)为尺度因子参数。
所述尺度因子参数qn计算最小化椭球利用椭球最小迹规则尺度因子参数qn满足:
其中,是矩阵的特征值,构造对角元素为的对角矩阵L,那么由特征值对应的特征向量构造的矩阵D可以实施分解操作,dii(·)表示矩阵的对角元素,m是等价变换系统状态变量的维数。
本发明的有益效果:基于惯性导航系统误差模型观测量噪声不满秩情形,对惯性导航系统误差模型方程实施降维操作,进而实现一种降维的惯性导航系统等价变换模型方程,达到减小系统状态变量计算量的目的;利用椭球集员平滑器算法实现等价变换惯性导航系统误差方程的系统状态向量的滤波平滑计算,从而达到进一步改善系统状态变量参数最优估计的精度要求。经由运动载体惯性导航系统仿真实验,并与椭球集员滤波算法对比,验证了本发明的计算优势和计算效能。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明的结构示意图。
图2是运动载体惯性导航系统运动轨迹图。
图3是运动载体位置误差数据曲线。
图4是运动载体速度误差数据曲线。
图5是运动载体姿态失准角数据曲线。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
本发明提出的一种惯性导航系统误差模型的降维RTS椭球集员平滑方法,经由系统线性函数的降维计算获得系统线性函数的等价变换系统,进而对等价变换系统开展椭球集员最优平滑迭代计算,最终获得原系统状态变量的最优平滑结果。以惯性导航系统误差模型离散化的随机状态空间模型,包括状态方程和观测方程,
其中,是第n步的系统状态变量,是第n步的系统观测变量,和分别表示系统的高斯过程噪声和观测噪声,n=1,2,…,T,T表示迭代计算次数;和分别表示nx、ny、nu、nv的实数空间;Fn表示状态转移矩阵、Gn表示过程噪声矩阵、Hn为观测矩阵、Jn表示观测噪声矩阵;。并且u={un}n∈N和w={wn}n∈N都零均值独立或者联合独立于系统状态变量初值x0,系统状态变量xn表现为马尔科夫链(MarkovChain,,MC)过程,(xn,yn)是一个具有独立噪声的隐马科夫链(Hidden Markov Chain,HMC-IN)过程,模型中的f(·)和h(·)分别表示系统的动态模型函数和观测模型函数。
平滑算法一般都是和滤波计算交互迭代使用,用于改善滤波获得结果的再平滑计算过程,平滑计算在数学上就是在给定所有观测向量{yn},T>n基础上的系统状态变量的条件概率分布p(xn|y1:T),n=1,2,…,T集合,在本发明中利用高斯分布假设开展RTS椭球集员平滑方法的推理过程,也就是系统状态变量满足高斯正态分布:
p(xn|y1:T)=N(xn|mn|T,Pn|T) (2)
其中,mn|T是系统状态变量的均值向量,Pn|T是第n步平滑计算获得的逼近高斯分布系统的状态变量的方差矩阵。但是在滤波和平滑操作中都需要计算系统线性函数的一阶和二阶矩。针对满足高斯分布的泛指线性向量函数g(x),其均值、方差和协方差分别为:
这里给出Bayesian平滑的理论计算步骤,假设已经获得了系统第n步的状态变量的滤波解的高斯逼近:
p(xn|y1:n)=N(xn|mn|n,Pn|n) (6)
那么泛指线性函数的高斯一阶矩(期望值)可表示为:
平滑方法式从第n=T步的滤波结果开始平滑计算,直至n=0为止,从而高斯分布的Bayesian平滑计算框架为:
其中,mn|n表示n时刻(第n步)的系统状态变量的估计值、mn+1|n表示k+1时刻(第k+1步)的系统状态变量的预测值、Pn|n表示n时刻的系统状态变量的估计方差、Pn+1|n表示k+1时刻的系统状态变量的预测方差,而则表示其逆矩阵、Pn|T表示自T步骤开始的第n次的平滑迭代方差、Pn+1|T表示n+1次的平滑方差、Qn表示n时刻的系统噪声方差、Cn+1|n表示平滑的协方差矩阵、Gk表示平滑增益矩阵,而表示平滑增益矩阵的转置矩阵。
最终获得系统状态变量逼近计算的平滑分布:
p(xn|y1:T)=N(xn|mn|T,Pn|T) (9)。
在本发明中考虑一种特殊情形,就是观测噪声方差矩阵Rn若是零矩阵情形,其实这种情形在很多实际应用场合中都会出现,尤其是考虑有色加性观测噪声。若考虑一类自回归观测噪声,满足:
wn=Anwn-1+ξn (10)
初始噪声为w0=ξ0,其中,噪声变量ξn是零均值的,且状态变量初值x0、过程噪声{un}n≥0和噪声变量{ξn}n≥0是独立的,那么可以把线性系统状态变量扩展为那么误差模型的状态空间模型可整理为无观测噪声系统:
其中,An表示噪声映射矩阵。这里假设矩阵Jn是ny×ny的单位阵,且噪声变量ξn满足方差矩阵对线性系统的状态空间模型再次做出变换整理获得:
其中,等效噪声是独立的,且其方差
在系统(1)及变换系统(11)和(12)中,状态变量初值x0、过程噪声{un}n≥0和噪声变量{ξn}n≥0是独立的,暗含着过程噪声{un}n≥0和{wn}n≥0也是独立的,但是针对同源过程噪声和观测噪声,其部分向量可能是相关的。可以假设un和wn+p(p>0)是相关的,并且这里考虑观测噪声方差矩阵Rn是半正定矩阵情形,有Rn≥0,从而考虑部分无噪声的观测变量情形,可以令其中部分变量是无噪声变量,从而可以对观测噪声进行分解操作:
其中,从而可以构建扩展系统状态变量依次扩展状态变量可以获得线性系统(1)的变换模型(11)的具体表达式:
下面依次开展线性系统(1)的降维计算操作,若观测噪声方差矩阵Rn≥0,其具有r=rank(Rn)∈{0,1,…,ny}的性质,可以依次将线性系统的状态变量xn的维数利用m=ny-r降低到r阶,从而实现线性系统状态变量椭球集员平滑操作的计算量得到降低,减小算法计算量,优化椭球集员平滑方法的目的。根据观测噪声方差矩阵秩性质,Rn具有m个零特征值,那么存在着一个非奇异矩阵Mn满足:
其中,0m表示m×m的零矩阵,令那么从线性系统(1)可以获得观测变量分解为:
其中,这些变量或者矩阵都被分为无噪声部分和有噪声部分,一旦无噪声部分观测变量已知的话,相应的m个系统状态变量xn就是已知的了,因此仅仅对有噪声观测变量部分开展平滑操作即可完成线性系统状态变量的迭代计算,这样可以明显降低平滑算法的计算量,提高和改善算法的计算效能。公式中0的下标表示m×1的矩阵。
很明显,在转换观测方程(14)中,m≤nx,且可以看出可以选择一个(nx-m)×nx的非奇异矩阵Un实施变换:
应该说明的是式(15)是可逆的,且其左边矩阵可表达为从而可以利用矩阵Tn和非奇异矩阵Mn对线性系统(1)做出等价变换,获得等价系统为:
且其中
另外根据式(15)和线性系统(1)中的过程方程,可以整理获得:
从而综合式(16)和式(17),可以获得降维等价变换线性系统:
其中,定义
那么可以针对降维等价变换系统模型(18)开展椭球集员RTS平滑计算处理。
本发明采用观测变量的无噪声和有噪声分解策略对惯性导航系统问题实施降维操作,获得等价降维惯性导航系统模型方程,对等价系统模型方程实施椭球集员滤波算法迭代计算,获得系统状态变量的T步滤波数据并保存,根据获得的滤波计算数据开展反向等价惯性导航系统状态向量的滤波平滑计算,从而达到进一步改善惯性导航系统状态变量参数最优估计的精度要求。
如图1所示,一种惯性导航系统误差模型的降维RTS椭球集员平滑方法,实施步骤为:首先建立惯性导航系统误差模型的系统方程和观测方程;开展惯性导航系统方程的等价变换,获得惯性导航系统等价变换线性系统的模型方程,进而开展状态变量参数的椭球集员滤波计算,经由n=T步迭代计算获得第T步的最优滤波结果估计均值mT|T和估计方差矩阵PT|T,并存储各个时刻的估计数据mn|n和Pn|n;然后从n=T-1开始,利用各时刻滤波估计数据作为观测数据和等价变换线性系统的模型方程开展椭球集员RTS平滑计算,获得平滑算法的预测均值和预测方差矩阵Pn+1|n,构造平滑预测椭球接着计算RTS椭球集员平滑增益Gk,获取等价变换线性系统的状态方程和观测方程的平滑均值及平滑方差矩阵Pn|0:T,从而获得最终的等价变换系统状态变量的平滑椭球从而完成等价变换惯性导航系统问题的系统状态参数变量的估计计算任务。具体步骤为:
步骤一:构建惯性导航系统误差模型的状态方程和观测方程,如公式(1)所示。
步骤二:基于观测矩阵不满秩情形,利用非奇异矩阵对惯性导航系统误差模型实施降维等价变换,获得维数减小的惯性导航系统等价变换线性系统的模型方程。
根据惯性导航系统的观测向量的噪声分布情况对其实施降维操作,获得降维等价变换线性系统的模型方程如式(18)所示,其中利用了满足式(14)的非奇异矩阵Mn进行等价变换;等价变换矩阵Tn为:
同时,定义联合系统状态变量那么获得联合系统状态变量满足初始椭球分布为其噪声项整理为:
满足椭球形状分布其中,是等价变换线性系统的状态变量的椭球方差矩阵,是变换模型方程的观测变量的椭球方差,是变换模型方程的观测变量的椭球协方差矩阵。
步骤三:对等价变换线性系统的状态变量参数开展椭球集员滤波迭代计算,获得各时刻的等价的状态变量的滤波估计椭球形状;经由n=T步迭代计算获得第T步的最优滤波椭球形状,确定等价变换线性系统的状态变量的估计均值和估计方差矩阵,并存储各个时刻的估计椭球形状数据。
根据降维等价变换线性系统的方程模型(18),对其做出椭球集合计算操作。假设经由n=T步的椭球集员滤波迭代计算后获得系统状态变量的最优滤波结果为估计均值mT|T和估计方差矩阵PT|T,并构造各个时刻的系统状态变量的椭球形状E(mn|n,Pn|n),并存储各个时刻的系统状态变量估计椭球形状E(mn|n,Pn|n),利用系统状态变量滤波获得估计均值作为系统状态变量平滑计算的观测数据开展椭球集员平滑计算。
步骤四:从n=T开始,对步骤三获取的各个时刻滤波数据开展逆向平滑操作,基于等价变换线性系统的方程,根据第T步滤波椭球数据计算平滑算法非零噪声的观测向量的预测椭球,进而确定预测均值及其预测方差矩阵。
从n=T-1开始,利用第n步的椭球估计形状数据和Pn|n,计算第n+1步导航系统等价观测变量的椭球形状中心平滑均值为,
步骤五:令n=T-1,根据第T步的估计数据开展等价变换线性系统的状态变量的RTS椭球平滑计算,获得第T-1步的等价变换系统的状态变量的平滑椭球形状数据,确定平滑增益矩阵;进而确定等价变换线性系统的状态变量的平滑均值和方差矩阵;直至计算n=0步的椭球平滑数据,从而完成等价变换惯性导航系统的系统状态参数变量的平滑计算任务。
计算等价变换线性系统联合状态变量中的观测变量的平滑方差矩阵,从而确定系统状态变量的预测椭球,利用椭球直和计算公式:
其中,优化参数pn∈[0,+∞)用于选择椭球的最小尺寸,如最小迹或者最小容积。本发明选择最小化椭球迹来计算最优椭球,计算最优pn值为:
计算平滑增益:
获得等价变换线性系统的状态变量的平滑椭球形状数据,就是平滑均值及其平滑方差计算:
其中,参数λn+1表示为:
参数Λn+1表达式为:
其实式(27)和(28)表达的参数λn和Λn可以从λN=0、ΛN=0递推计算,其递推表达式为:
其中,引入了参数βn的表达式为:
这里引入了尺度因子参数qn∈[0,∞)来计算最小化椭球利用椭球最小迹规则尺度因子参数qn满足:
其中,是矩阵的特征值,构造对角元素为的对角矩阵L,那么由特征值对应的特征向量构造的矩阵D可以实施分解操作,dii(·)表示矩阵的对角元素,m是等价变换系统状态变量的维数。
经由每一步的后向递推计算获得参数λn和Λn的数据,代入式(25)和(26)进行等价变换线性系统的状态变量的平滑均值和平滑方差矩阵计算,最终获得等价变换线性系统的N步平滑计算的椭球形状从而完成RTS固定区间椭球集员平滑算法的迭代计算过程。
具体实例:考虑运动载体的惯性导航系统问题,在笛卡尔坐标系下可以给出惯性导航系统方程为:
这里惯性导航系统状态向量为其中,δp=[δL,δλ,δh]T表示运动载体位置误差,表示运动载体姿态失准角,εb表示载体系中陀螺仪常值误差,表示载体系中加速度计零位偏差,δvb表示运动载体速度误差。
假设陀螺仪和加速度计的输出为和那么相应的误差项为
其中,矩阵中的δkgii,(i=x,y,z)是陀螺仪尺度因子误差,δkgij,(i,j=x,y,z,i≠j)陀螺仪相对于理想载体轴的失准角,
δkaii,(i=x,y,z)是加速度计的尺度因子误差,δkaij,(i,j=x,y,z,i≠j)是加速度计相对于载体轴的失准角。系统噪声为噪声向量wn是高斯过程噪声,vk~N(0,Qk),其中Qk表示噪声方差。
本实施例中采用位置和速度观测获得观测方程为:
y=Hx+v (36)
其中,H=[06×3 I6×6 06×6],vn是观测白噪声,满足分布vn~N(0,Rn),其中,Rn表示观测噪声的方差。由此展开仿真验证工作,获得如图2所示的载体运行轨迹图,如图3所示的运动载体位置的误差图,如图4所示运动载体速度误差图和如图5所示运动载体姿态失准角平滑图。和椭球集员滤波数据进行比较,很明显滤波和平滑算法中RTS椭球集员平滑算法与运动载体真实运行数据拟合程度较好,而滤波算法的拟合程度差一些;并且RTS椭球集员平滑算法的计算标准差比较小,误差数据曲线平滑稳定,而椭球集员滤波算法的数据变化剧烈,明显误差数据比较大,通过利用这两种平滑和滤波算法开展运动载体系统仿真实验,获得的实验数据说明,本发明的计算效能优于椭球集员滤波算法,表现出了RTS椭球集员平滑器算法的计算优势。
以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (9)
1.一种惯性导航系统误差模型的降维RTS椭球集员平滑方法,其特征在于,其步骤如下:
步骤一:构建惯性导航系统误差模型的状态方程和观测方程;
步骤二:基于观测矩阵不满秩情形,利用非奇异矩阵对惯性导航系统误差模型实施降维等价变换,获得维数减小的惯性导航系统等价变换线性系统的模型方程;
步骤三:对等价变换线性系统的状态变量参数开展椭球集员滤波迭代计算,获得各时刻的等价的状态变量的滤波估计椭球形状;经由n=T步迭代计算获得第T步的最优滤波椭球形状,确定等价变换线性系统的状态变量的估计均值和估计方差矩阵,并存储各个时刻的估计椭球形状数据;
步骤四:从n=T开始,对步骤三获取的各个时刻滤波数据开展逆向平滑操作,基于等价变换线性系统的方程,根据第T步滤波椭球数据计算平滑算法非零噪声的观测向量的预测椭球,进而确定预测均值及其预测方差矩阵;
步骤五:令n=T-1,根据第T步的估计数据开展等价变换线性系统的状态变量的RTS椭球平滑计算,获得第T-1步的等价变换系统的状态变量的平滑椭球形状数据,确定平滑增益矩阵;进而确定等价变换线性系统的状态变量的平滑均值和方差矩阵;直至计算n=0步的椭球平滑数据,从而完成等价变换惯性导航系统的状态参数变量的平滑计算任务。
2.根据权利要求1所述的惯性导航系统误差模型的降维RTS椭球集员平滑方法,其特征在于,所述步骤一中惯性导航系统误差模型的状态方程和观测方程为:
其中,是第n步的系统状态变量,是第n步的系统观测变量,和分别表示系统的高斯过程噪声和观测噪声,n=1,2,…,T,T表示迭代计算次数;和分别表示nx、ny、nu、nv的实数空间;Fn表示状态转移矩阵、Gn表示过程噪声矩阵、Hn为观测矩阵、Jn表示观测噪声矩阵;且u={un}n∈N和w={wn}n∈N都零均值独立或者联合独立于系统状态变量初值x0,系统状态变量xn表现为马尔科夫链过程,(xn,yn)是一个具有独立噪声的隐马科夫链过程,f(·)和h(·)分别表示系统的动态模型函数和观测模型函数。
3.根据权利要求2所述的惯性导航系统误差模型的降维RTS椭球集员平滑方法,其特征在于,所述步骤二中获得降维等价变换线性系统的方法为:构造非奇异矩阵Mn满足:
其中,m是等价变换线性系统的状态变量的维数,且m≤nx,0m表示m×m的零矩阵,Rn表示具有m个零特征值的观测噪声方差矩阵,Ir表示r维单位矩阵;
令那么惯性导航系统误差模型的观测变量分解为:
其中,p表示观测方程中无噪声部分向量的维数,r表示有噪声部分向量的维数,和分别表示无噪声部分观测分量、有噪声部分观测分量;和分别表示观测矩阵经由非奇异矩阵变换后分解的对应矩阵和的观测矩阵分量;矩阵0的下标表示m×1的矩阵;
选择一个(nx-m)×nx的非奇异矩阵Un对惯性导航系统误差模型的状态变量实施变换:
其中,表示经由非奇异矩阵变换后的系统状态变量;上式是可逆的,且其左边的等价变换矩阵为利用等价变换矩阵Tn和非奇异矩阵Mn对惯性导航系统误差模型做出等价变换,获得等价系统为:
其中,
根据状态变量实施变换和惯性导航系统误差模型的状态方程,整理获得:
因此,获得降维的等价变换线性系统:
其中,
4.根据权利要求1所述的惯性导航系统误差模型的降维RTS椭球集员平滑方法,其特征在于,定义联合系统状态变量那么获得联合系统状态变量满足初始椭球分布为其噪声项整理为:满足椭球形状分布其中,是等价变换线性系统的状态变量的椭球方差矩阵,是等价变换线性系统的观测变量的椭球方差,是等价变换线性系统的观测变量的椭球协方差矩阵。
5.根据权利要求3所述的惯性导航系统误差模型的降维RTS椭球集员平滑方法,其特征在于,从n=T-1开始,利用第n步的椭球估计形状数据和Pn|n,计算第n+1步导航系统等价观测变量的椭球形状中心平滑均值为:
计算等价变换线性系统的观测变量的平滑方差矩阵,从而确定系统状态变量的预测椭球,利用椭球直和计算公式:
其中,pn∈[0,+∞)为优化参数。
6.根据权利要求5所述的惯性导航系统误差模型的降维RTS椭球集员平滑方法,其特征在于,所述优化参数pn用于选择椭球的最小尺寸,选择最小化椭球迹来计算最优椭球,计算最优pn值为:
7.根据权利要求3所述的惯性导航系统误差模型的降维RTS椭球集员平滑方法,其特征在于,计算平滑增益:获得等价变换线性系统的状态变量的平滑椭球形状数据,包括平滑均值及平滑方差的计算:
其中,参数λn+1表示为:参数Λn+1为:
8.根据权利要求7所述的惯性导航系统误差模型的降维RTS椭球集员平滑方法,其特征在于,所述参数λn和Λn+1从λN=0、ΛN=0递推计算,其递推表达式为:
其中,参数βn的表达式为:qn∈[0,∞)为尺度因子参数。
9.根据权利要求7所述的惯性导航系统误差模型的降维RTS椭球集员平滑方法,其特征在于,所述尺度因子参数qn计算最小化椭球利用椭球最小迹规则尺度因子参数qn满足:
其中,是矩阵的特征值,构造对角元素为的对角矩阵L,那么由特征值对应的特征向量构造的矩阵D可以实施分解操作,dii(·)表示矩阵的对角元素,m是等价变换系统状态变量的维数。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810309501.6A CN108507593B (zh) | 2018-04-09 | 2018-04-09 | 一种惯性导航系统误差模型的降维rts椭球集员平滑方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810309501.6A CN108507593B (zh) | 2018-04-09 | 2018-04-09 | 一种惯性导航系统误差模型的降维rts椭球集员平滑方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108507593A true CN108507593A (zh) | 2018-09-07 |
CN108507593B CN108507593B (zh) | 2020-04-28 |
Family
ID=63380801
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810309501.6A Active CN108507593B (zh) | 2018-04-09 | 2018-04-09 | 一种惯性导航系统误差模型的降维rts椭球集员平滑方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108507593B (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111765889A (zh) * | 2020-04-30 | 2020-10-13 | 江苏高科石化股份有限公司 | 一种生产车间移动机器人基于多胞-椭球双滤波的位姿定位方法 |
CN113343436A (zh) * | 2021-05-20 | 2021-09-03 | 中国科学院国家空间科学中心 | 一种高斯混合协方差演化的碰撞概率计算方法及系统 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104795819A (zh) * | 2015-05-11 | 2015-07-22 | 重庆大学 | 一种基于强跟踪集员估计的电力系统状态估计方法 |
CN105222780A (zh) * | 2015-09-07 | 2016-01-06 | 郑州轻工业学院 | 一种基于Stirling插值多项式逼近的椭球集员滤波方法 |
CN106767780A (zh) * | 2016-11-28 | 2017-05-31 | 郑州轻工业学院 | 基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法 |
-
2018
- 2018-04-09 CN CN201810309501.6A patent/CN108507593B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104795819A (zh) * | 2015-05-11 | 2015-07-22 | 重庆大学 | 一种基于强跟踪集员估计的电力系统状态估计方法 |
CN105222780A (zh) * | 2015-09-07 | 2016-01-06 | 郑州轻工业学院 | 一种基于Stirling插值多项式逼近的椭球集员滤波方法 |
CN106767780A (zh) * | 2016-11-28 | 2017-05-31 | 郑州轻工业学院 | 基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法 |
Non-Patent Citations (4)
Title |
---|
N.NADARAJSH .ET AL: "IMM Forward Filtering and Backward Smoothing for Maneuvering Target Tracking", 《IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS》 * |
PATRICK NIMA RAANES .ET AL: "On the ensemble Rauch-Tung-Striebel smoother and its equivalence to the ensemble Kalman smoother", 《QUARTERLY JOURNAL OF THE ROYAL METEOROLOGICAL SOCIETY》 * |
沈强等: "用于MEMS陀螺阵列信号处理的OBE平滑算法", 《中国惯性技术学报》 * |
赵祚连等: "非线性系统的线性等价及应用", 《上海交通大学学报》 * |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111765889A (zh) * | 2020-04-30 | 2020-10-13 | 江苏高科石化股份有限公司 | 一种生产车间移动机器人基于多胞-椭球双滤波的位姿定位方法 |
CN111765889B (zh) * | 2020-04-30 | 2024-04-16 | 江苏高科石化股份有限公司 | 一种生产车间移动机器人基于多胞-椭球双滤波的位姿定位方法 |
CN113343436A (zh) * | 2021-05-20 | 2021-09-03 | 中国科学院国家空间科学中心 | 一种高斯混合协方差演化的碰撞概率计算方法及系统 |
Also Published As
Publication number | Publication date |
---|---|
CN108507593B (zh) | 2020-04-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106054170B (zh) | 一种约束条件下的机动目标跟踪方法 | |
CN105806338B (zh) | 基于三向卡尔曼滤波平滑器的gnss/ins组合定位定向算法 | |
CN101982732B (zh) | 一种基于esoqpf和ukf主从滤波的微小卫星姿态确定方法 | |
CN106767780B (zh) | 基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法 | |
CN106772524B (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
CN104964683B (zh) | 一种室内环境地图创建的闭环校正方法 | |
Nabaa et al. | Validation and comparison of coordinated turn aircraft maneuver models | |
CN108507593A (zh) | 一种惯性导航系统误差模型的降维rts椭球集员平滑方法 | |
Yousuf et al. | Information fusion of GPS, INS and odometer sensors for improving localization accuracy of mobile robots in indoor and outdoor applications | |
CN111025229B (zh) | 一种水下机器人纯方位目标估计方法 | |
CN112990549B (zh) | 一种空间非合作目标抵近绕飞观测轨迹优化方法 | |
CN115933641A (zh) | 基于模型预测控制指导深度强化学习的agv路径规划方法 | |
CN105891820A (zh) | 基于ukf和iufir的机动目标跟踪方法 | |
CN110967017A (zh) | 一种用于双移动机器人刚体协作搬运的协同定位方法 | |
CN111854741B (zh) | 一种gnss/ins紧组合滤波器及导航方法 | |
CN109764876A (zh) | 无人平台的多模态融合定位方法 | |
CN117521006A (zh) | 一种基于增量学习的因子图多源信息融合方法 | |
CN109115228B (zh) | 一种基于加权最小二乘容积卡尔曼滤波的目标定位方法 | |
Lai et al. | Generalizations of the complex-step derivative approximation | |
CN108681621B (zh) | 基于Chebyshev正交多项式扩展RTS Kalman平滑方法 | |
CN106599541A (zh) | 一种动态电力负荷模型的结构和参数在线辨识方法 | |
Poddar et al. | Tuning of GPS aided attitude estimation using evolutionary algorithms | |
Munts | Numerical Study of Different Variants of Dubins’ Car Model | |
CN110849392A (zh) | 一种机器人的里程计数据校正方法及机器人 | |
Araneda et al. | Statistical inference in mapping and localization for mobile robots |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |