导航数据的滤波方法、装置、计算机设备及存储介质与流程

文档序号:15489278发布日期:2018-09-21 20:18阅读:131来源:国知局

本发明涉及车辆组合导航系统,更具体地说是指导航数据的滤波方法、装置、计算机设备及存储介质。



背景技术:

惯性系统的理论基础是牛顿力学定律,系统的运行只需要加速度和角速度信息的融合,不需要接收其他辅助信息,也不会产生外向辐射,具有屏蔽并且不产生干扰的特点,能在较短时间内提供高精度相对位置信息。自进入21世纪,基于mems(micro-electro-mechanicalsystem)技术的惯性传感器被成功应用于消费市场,mems是微机电系统,也叫做微电子机械系统、微系统、微机械等,指尺寸在几毫米乃至更小的高科技装置,但是把惯性传感器应用于导航系统需要解决误差在计算过程中迅速积累的问题。

多种成熟的卫星导航系统共存的背景下,借助于差分gps等辅助卫星定位技术,理想情况下,汽车能通过卫星导航系统获取全方位、全天候、较高精度的绝对位置信息,但城市环境中卫星信号易受建筑物遮挡,产生较大的定位误差。惯性系统与卫星系统的各自特性使得惯性系统/卫星系统组合导航系统能为车辆在城市环境下的精确定位提供有效的解决方案,两种子系统能通过相互冗余并修正的方式,共同提升位置估计的精度。由于子系统存在自身的误差,所以开发组合导航系统需要把两个子系统的数据进行融合并对误差进行补偿,目前最普遍采用的卡尔曼滤波算法及多种改进算法都是基于最小均方误差准则,此类方法能实现对高斯噪声的最优滤波性能,而无法捕获非高斯噪声的高阶项,另外,还可以采用非卡尔曼滤波迭代框架的滤波算法进行数据融合和误差进行补偿,如粒子滤波和神经网络滤波算法,都需要较大的计算量,不适用于状态量维数较高的组合导航系统。

因此,有必要设计一种新的方法,以实现对非高斯噪声高阶项的有效捕获,提高误差的估计准确率,提升定位精度。



技术实现要素:

本发明的目的在于克服现有技术的缺陷,提供导航数据的滤波方法、装置、计算机设备及存储介质。

为实现上述目的,第一方面,本发明实施例提供了一种导航数据的滤波方法,其包括:

建立惯性系统参数误差模型;

初始化惯性系统参数误差模型,设置下一滤波周期的时间序号为指定值;

对时间序号为指定值减一的状态量进行无迹变换,获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值;

根据时间序号为指定值的状态量预测值对时间序号为指定值的量测值进行无迹变换,获取时间序号为指定值的量测值估计值、时间序号为指定值的量测值协方差的估计值、时间序号为指定值的状态量与量测值的互协方差;

根据时间序号为指定值的状态量协方差预测值和时间序号为指定值的状态量与量测值的互协方差构造伪观测矩阵,利用伪观测矩阵对非线性量测方程进行线性化改写,获取改写公式;

根据改写公式计算高斯核的系数;

利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值;

更新时间序号为指定值的状态量协方差;

反馈状态量后验估计值至惯性系统内,补偿惯性系统的参数;

设置指定值等于指定值加一,并返回所述对时间序号为指定值减一的状态量进行无迹变换,获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值的步骤。

第二方面,本发明实施例还提供了一种导航数据的滤波装置,其包括:

模型建立单元,用于建立惯性系统参数误差模型;

初始化单元,用于初始化惯性系统参数误差模型,设置下一滤波周期的时间序号为指定值;

预测值获取单元,用于对时间序号为指定值减一的状态量进行无迹变换,获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值;

估计值获取单元,用于根据时间序号为指定值减一的状态量预测值对时间序号为指定值的量测值进行无迹变换,获取时间序号为指定值的量测值估计值、时间序号为指定值的量测值协方差的估计值、时间序号为指定值的状态量与量测值的互协方差;

改写公式获取单元,用于根据时间序号为指定值的状态量协方差预测值和时间序号为指定值的状态量与量测值的互协方差构造伪观测矩阵,利用伪观测矩阵对非线性量测方程进行线性化改写,获取改写公式;

系数获取单元,用于根据改写公式计算高斯核的系数;

后验估计值获取单元,用于利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值;

方差更新单元,用于更新时间序号为指定值的状态量协方差;

参数补偿单元,用于反馈状态量后验估计值至惯性系统内,补偿惯性系统的参数;

设置单元,用于设置指定值等于指定值加一。

第三方面,本发明实施例还提供了一种计算机设备,其包括存储器及处理器,所述存储器上存储有计算机程序,所述处理器执行所述计算机程序时实现上述方法。

第四方面,本发明实施例还提供了一种计算机可读存储介质,所述存储介质存储有计算机程序,所述计算机程序包括程序指令,所述程序指令当被处理器执行时可实现上述方法。

本发明与现有技术相比的有益效果是:本发明通过在对导航系统的状态量误差进行估计时,引入最大相关熵准则对传统无迹卡尔曼滤波算法进行改进,利用相关熵能捕获非高斯噪声高阶项,对系统误差进行精确估计,并通过反馈状态量后验估计值,进行校正方式,对系统误差进行补偿,实现导航参数的高精度输出,提高误差的估计准确率,提升定位精度,具备高效的滤波性能。

下面结合附图和具体实施例对本发明作进一步描述。

附图说明

图1为本发明具体实施例提供的导航数据的滤波方法的示意流程图;

图2为本发明具体实施例提供的导航数据的滤波方法的子步骤示意流程图;

图3为本发明具体实施例提供的导航数据的滤波方法的子步骤示意流程图;

图4为本发明具体实施例提供的导航数据的滤波方法的子步骤示意流程图;

图5为本发明具体实施例提供的位置估计误差对比的示意图;

图6为本发明具体实施例提供的导航数据的滤波装置的示意性框图;

图7为本发明具体实施例提供的模型建立单元的示意性框图;

图8为本发明具体实施例提供的初始化单元的示意性框图;

图9为本发明具体实施例提供的后验估计值获取单元的示意性框图;

图10为本发明具体实施例提供的一种计算机设备的示意性框图。

具体实施方式

为了更充分理解本发明的技术内容,下面结合具体实施例对本发明的技术方案进一步介绍和说明,但不局限于此。

应当理解,当在本说明书和所附权利要求书中使用时,术语“包括”和“包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在,但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。

还应当理解,在此本申请说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本申请。如在本申请说明书和所附权利要求书中所使用的那样,除非上下文清楚地指明其它情况,否则单数形式的“一”、“一个”及“该”意在包括复数形式。

还应当进一步理解,在本申请说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合,并且包括这些组合。

请参阅图1,图1为本发明具体实施例提供的导航数据的滤波方法的示意流程图;该滤波方法应用于服务器内,以滤波平台的形式存在;该服务器与导航装置进行数据交互,以使得导航装置的导航数据可以进行误差补偿,提高定位精度。

图1为本发明具体实施例提供的导航数据的滤波方法的示意流程图,如图1所示,该方法包括s110~s200。

s110、建立惯性系统参数误差模型。

在本发明实施例中,惯性系统参数误差模型是指由惯性系统中的各个参数的误差按照特定的返程组合形成的模型。

如图2所示,在一实施例中,上述的s110可包括步骤s111~s113。

s111、利用惯性系统的参数误差构造状态方程;

s112、利用惯性系统与卫星系统的测量信息的差值建立量测方程;

s113、根据状态方程和量测方程组成惯性系统参数误差模型。

其中,状态方程是指惯性系统的姿态角误差、速度误差、位置误差、陀螺仪常值零偏和加速度计常值零偏组合形成的方程,量测方程是惯性系统与卫星系统的测量信息的差值之间的线性关系组成的方程。避免了卡尔曼滤波算法在组合导航系统中的非线性不适用问题。

模型中状态量xk包含惯性系统的姿态角误差、速度误差、位置误差、陀螺仪常值零偏和加速度计常值零偏,此处的状态量为n维,系统模型包含状态方程和量测方程;

惯性系统参数误差模型表示为其中;系统状态量xk表示为xk=[φeφnφuδveδvnδvuδlδλδhbwxbwybwzbfxbfybfz];φe、φn、φu表示计算过程中导航坐标系内角度计算误差;δve、δvn、δvu表示计算过程中导航坐标系内速度计算误差;δl、δλ、δh表示计算过程中地心惯性坐标系内的位置计算误差;bwx、bwy、bwz表示载体坐标系中陀螺仪在三个轴向的固定漂移;bfx、bfy、bfz表示载体坐标系中加速度器在三个轴向的固定漂移;fk-1和hk分别为15*15的状态转移矩阵和6*15的量测矩阵,n=15;qk-1表示系统噪声,统计特性为高斯特性,其协方差为qk;rk表示测量噪声,此处设置为高斯混合噪声,统计特性为非高斯特性,协方差表示为rk;yk为量测值。

s120、初始化惯性系统参数误差模型,设置下一滤波周期的时间序号为指定值。

对系统进行初始化,以完成卡尔曼滤波的迭代。在本实施例中,时间序号用k表示,此时设定的下一滤波周期的时间序号为k=1。

在一实施例中,如图3所示,上述的s120可包括有s121~s122。

s121、时间序号为零的状态量及时间序号为零的协方差进行初始化;

s122、设置下一滤波周期的时间序号为一。

对系统进行初始化,设置时间序号为k=0的系统状态值,令惯性导航系统采集惯性数据,卫星导航系统接收卫星信号,惯性系统进行惯导数据解算,并令时间序列的序号k=1,先获取零时刻的系统状态值,作为初始数据,再根据初始数据对惯性系统参数误差模型进行参数初始化。

具体地,对k=0时刻系统的初始化状态量x0设置为x0=0及初始化状态量对应的初始方差设置为p0=0;并设置

对组合导航系统中导航参数的误差估计,在采集了加速度计数据和陀螺仪数据后,进行了惯性系统的导航解算,包括速度和位置信息,导航输出数据为惯性导航解算数据在进行误差补偿后的修正值,也就是服务器进行所有计算后,将计算结果对导航系统进行误差补偿修正,形成最终的结果。

s130、对时间序号为指定值减一的状态量进行无迹变换,获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值。

状态量预测值指的是在某一时刻,依据惯性系统参数误差模型得出的状态量;状态量协方差预测值是指在某一时刻,依据状态量预测值得出进行方差计算,获取的数值。

在本实施例中,具体是对k-1时刻的状态量xk-1应用无迹变换,求出状态量及其协方差的先验预测值和pk|k-1;对于无迹变换过程,采用以下方式:

计算2n+1个sigma点χk-1(卡尔曼滤波采样点),计算公式如下:

式中λ是一个复合比例因子,规定λ=α2(n+ε)-n;其中通常设置0<α≤1,ε是一个比例因子,一般选择为3-n。

利用状态转换矩阵对sigma点(卡尔曼滤波采样点)进行非线性变换,公式如下:

通过变换后的点计算先验预测值和pk|k-1,计算公式如下:

qk是惯性系统噪声的协方差,是状态均值对应的权值,是关联协方差矩阵对应的权值,计算方式如下:

其中,i=1,…,2n,β与xk的先验知识相关,通常情况下取β=2。

基于无迹卡尔曼滤波算法框架,避免了卡尔曼滤波算法在惯性系统与卫星系统组合的导航系统中的非线性不适用问题。

s140、根据时间序号为指定值的状态量预测值对时间序号为指定值的量测值进行无迹变换,获取时间序号为指定值的量测值估计值、时间序号为指定值的量测值协方差的估计值、时间序号为指定值的状态量与量测值的互协方差。

量测值估计值是由状态量预测值代入惯性系统参数误差模型内,获取的量测值;量测值协方差的估计值是指对量测值估计值进行方差计算,获取的方差值;状态量与量测值的互协方差是指某一时刻的某一状态量对应的量测值进行互协方差计算后,获取的数值。

在本实施例中,上述的量测值协方差的估计值采用以下公式计算:其中,rk是测量噪声的协方差,是时间序号为指定值的量测值的估计值;py,k|k-1是时间序号为指定值的量测值协方差的估计值;是关联协方差矩阵对应的权值。

该步骤中的无迹变换是采用非线性变换矩阵是观测矩阵,用观测矩阵代替步骤s130中的状态转移矩阵,进行与s130步骤类似的无迹变换。

s150、根据时间序号为指定值的状态量协方差预测值和时间序号为指定值的状态量与量测值的互协方差构造伪观测矩阵,利用伪观测矩阵对非线性量测方程进行线性化改写,获取改写公式。

在本实施例中,伪观测矩阵采用以下公式构造:其中,pk|k-1是时间序号为指定值的状态量协方差预测值,pxy,k|k-1是时间序号为指定值的状态量与量测值的互协方差。

在本实施例中,改写公式为其中,是时间序号为指定值的状态量预测值,pk|k-1是时间序号为指定值的状态量协方差预测值;是伪观测矩阵,是时间序号为指定值的状态量预测值,py,k|k-1是时间序号为指定值的量测值协方差的估计值。

s160、根据改写公式计算高斯核的系数;

在本实施例中,该高斯核的系数在计算该高斯核的系数时,首先参考加权最小二乘法求解卡尔曼滤波方程的过程,列写代价函数,代价函数的设计需要体现相关熵,利用高斯核函数的使用来体现相关熵,代价函数表示为:

为计算代价函数所表示的最大值,对代价函数计算关于的微分,并令其为零。

使用最大相关熵准则来计算状态量估计值,由于高斯核函数及其核宽参数的作用,使得无迹卡尔曼滤波的迭代过程遵循最大相关熵准则,对非高斯噪声的统计二阶项和更高阶项实现有效捕获。

s170、利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值。

状态量后验估计值是指某一时刻的最优状态量估计值。

在一实施例中,如图4所示,上述的步骤s170可包括有s171~s172。

s171、利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取卡尔曼滤波增益;其中,该卡尔曼滤波增益

s172、根据卡尔曼滤波增益获取时间序号为指定值的状态量后验估计值。该状态量后验估计值为该数据是k时刻的最优状态量估计值。

s180、更新时间序号为指定值的状态量协方差;具体地,将时间序号为指定值的状态量协方差更新为

s190、反馈状态量后验估计值至惯性系统内,补偿惯性系统的参数。

具体地,利用惯性系统所解算的数据减去对应时刻的状态量后验估计值,形成补偿数据,并把补偿数据作为下一时刻惯性系统的解算基础。

s200、设置指定值等于指定值加一,并返回步骤s120。

以此不断完成卡尔曼滤波的迭代,形成对导航误差估计进行不断更新,保证了组合导航系统滤波的实时性和有效性。

当整个导航系统断电时,该流程结束,也就是不再进入循环。

举一个例子,以松组合方式的sins/gnss(惯性系统/卫星系统)组合导航系统为例,导航初始点为西安市某点,经纬度分别设置在北纬34.14°,东经108.54°,陀螺仪三轴向的常值偏差都设置为0.03deg/h,随机漂移设置为0.001deg/sqrt(h),加速度计常值偏差设置为0.0001g,随机偏差设置为0.0005g/sqrt(hz)。卫星系统采用gps模拟系统,位置误差方差为5m;惯性系统传感器的采集频率为100hz,gps更新频率为1hz。进行两组实验,其中一组是利用本实施例提供的滤波方法在三个方向(东、北、天)位置估计误差量,另一组为传统基于最小均方误差的无迹卡尔曼滤波算法(unscentedkalmanfilter,ukf)对本例数据的三个方向上的位置估计误差量。实验结果如图5所示,从总体结果看,本方法在位置估计精度上高于传统无迹卡尔曼滤波算法。

上述的导航数据的滤波方法,通过在对导航系统的状态量误差进行估计时,引入最大相关熵准则对传统无迹卡尔曼滤波算法进行改进,利用相关熵能捕获非高斯噪声高阶项,,对系统误差进行精确估计,并通过反馈状态量后验估计值,进行校正方式,对系统误差进行补偿,实现导航参数的高精度输出,提高误差的估计准确率,提升定位精度,具备高效的滤波性能。

请参阅图6,图6是本发明具体实施例提供的导航数据的滤波装置300的示意性框图;如图6所示,该装置包括:

模型建立单元310,用于建立惯性系统参数误差模型。

初始化单元320,用于初始化惯性系统参数误差模型,设置下一滤波周期的时间序号为指定值。

预测值获取单元330,用于对时间序号为指定值减一的状态量进行无迹变换,获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值。

估计值获取单元340,用于根据时间序号为指定值的状态量预测值对时间序号为指定值的量测值进行无迹变换,获取时间序号为指定值的量测值估计值、时间序号为指定值的量测值协方差的估计值、时间序号为指定值的状态量与量测值的互协方差。

改写公式获取单元350,用于根据时间序号为指定值的状态量协方差预测值和时间序号为指定值的状态量与量测值的互协方差构造伪观测矩阵,利用伪观测矩阵对非线性量测方程进行线性化改写,获取改写公式。

系数获取单元360,用于根据改写公式计算高斯核的系数。

后验估计值获取单元370,用于利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值。

方差更新单元380,用于更新时间序号为指定值的状态量协方差。

参数补偿单元390,用于反馈状态量后验估计值至惯性系统内,补偿惯性系统的参数。

设置单元391,用于设置指定值等于指定值加一。

在一实施例中,如图7所示,上述的模型建立单元310包括有:

状态方程构建模块311,用于利用惯性系统的参数误差构造状态方程。

量测方程构建模块312,用于利用惯性系统与卫星系统的测量信息的差值建立量测方程。

组合模块313,用于根据状态方程和量测方程组成惯性系统参数误差模型。

在一实施例中,如图8所示,上述的初始化单元320包括有:

数据初始化模块321,用于时间序号为零的状态量及时间序号为零的协方差进行初始化;

序号设置模块322,用于设置下一滤波周期的时间序号为一。

在一实施例中,如图9所示,上述的后验估计值获取单元370包括有:

增益获取模块371,用于利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取卡尔曼滤波增益;

数值获取模块372,用于根据卡尔曼滤波增益获取时间序号为指定值的状态量后验估计值。

需要说明的是,所属领域的技术人员可以清楚地了解到,上述导航数据的滤波装置300和各单元的具体实现过程,可以参考前述方法实施例中的相应描述,为了描述的方便和简洁,在此不再赘述。

上述导航数据的滤波装置300可以实现为一种计算机程序的形式,该计算机程序可以在如图10所示的计算机设备上运行。

请参阅图10,图10是本申请实施例提供的一种计算机设备的示意性框图。该计算机设备500是服务器,具体地,该参阅图10,该计算机设备500包括通过系统总线501连接的处理器502、存储器和网络接口505,其中,存储器可以包括非易失性存储介质503和内存储器504。

该非易失性存储介质503可存储操作系统5031和计算机程序5032。该计算机程序5032包括程序指令,该程序指令被执行时,可使得处理器502执行一种导航数据的滤波方法。

该处理器502用于提供计算和控制能力,以支撑整个计算机设备500的运行。

该内存储器504为非易失性存储介质503中的计算机程序5032的运行提供环境,该计算机程序5032被处理器502执行时,可使得处理器502执行一种导航数据的滤波方法。

该网络接口505用于与其它设备进行网络通信。本领域技术人员可以理解,图8中示出的结构,仅仅是与本申请方案相关的部分结构的框图,并不构成对本申请方案所应用于其上的计算机设备500的限定,具体的计算机设备500可以包括比图中所示更多或更少的部件,或者组合某些部件,或者具有不同的部件布置。

其中,所述处理器502用于运行存储在存储器中的计算机程序5032,以实现如下步骤:

建立惯性系统参数误差模型;

初始化惯性系统参数误差模型,设置下一滤波周期的时间序号为指定值;

对时间序号为指定值减一的状态量进行无迹变换,获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值;

根据时间序号为指定值的状态量预测值对时间序号为指定值的量测值进行无迹变换,获取时间序号为指定值的量测值估计值、时间序号为指定值的量测值协方差的估计值、时间序号为指定值的状态量与量测值的互协方差;

根据时间序号为指定值的时刻的状态量协方差预测值和时间序号为指定值的状态量与量测值的互协方差构造伪观测矩阵,利用伪观测矩阵对非线性量测方程进行线性化改写,获取改写公式;

根据改写公式计算高斯核的系数;

利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值;

更新时间序号为指定值的状态量协方差;

反馈状态量后验估计值至惯性系统内,补偿惯性系统的参数;

设置指定值等于指定值加一,并返回所述对时间序号为指定值减一的状态量进行无迹变换,获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值的步骤。

在一实施例中,处理器502在实现所述建立惯性系统参数误差模型步骤时,具体实现如下步骤:

利用惯性系统的参数误差构造状态方程;

利用惯性系统与卫星系统的测量信息的差值建立量测方程;

根据状态方程和量测方程组成惯性系统参数误差模型。

在一实施例中,处理器502在实现所述初始化惯性系统参数误差模型,设置下一滤波周期的时间序号为指定值步骤时,具体实现如下步骤:

时间序号为零的状态量及时间序号为零的协方差进行初始化;

设置下一滤波周期的时间序号为一。

上述的所述量测值协方差的估计值采用以下公式计算其中,rk是测量噪声的协方差,是时间序号为指定值的量测值估计值;py,k|k-1是时间序号为指定值的量测值协方差的估计值;是关联协方差矩阵对应的权值。

上述的伪观测矩阵采用以下公式构造:其中,pk|k-1是时间序号为指定值的状态量协方差预测值,pxy,k|k-1是时间序号为指定值的状态量与量测值的互协方差。

上述的改写公式为其中,是时间序号为指定值的状态量预测值,pk|k-1是时间序号为指定值的状态量协方差预测值;是伪观测矩阵,是时间序号为指定值的状态量预测值,py,k|k-1是时间序号为指定值的量测值协方差的估计值。

在一实施例中,处理器502在实现所述利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值步骤时,具体实现如下步骤:

利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取卡尔曼滤波增益;

根据卡尔曼滤波增益获取时间序号为指定值的状态量后验估计值。

应当理解,在本申请实施例中,处理器502可以是中央处理单元(centralprocessingunit,cpu),该处理器502还可以是其他通用处理器、数字信号处理器(digitalsignalprocessor,dsp)、专用集成电路(applicationspecificintegratedcircuit,asic)、现成可编程门阵列(field-programmablegatearray,fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。其中,通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

本领域普通技术人员可以理解的是实现上述实施例的方法中的全部或部分流程,是可以通过计算机程序来指令相关的硬件来完成。该计算机程序包括程序指令,计算机程序可存储于一存储介质中,该存储介质为计算机可读存储介质。该程序指令被该计算机系统中的至少一个处理器执行,以实现上述方法的实施例的流程步骤。

因此,本发明还提供一种存储介质。该存储介质可以为计算机可读存储介质。该存储介质存储有计算机程序,其中计算机程序包括程序指令。该程序指令被处理器执行时使处理器执行如下步骤:

建立惯性系统参数误差模型;

初始化惯性系统参数误差模型,设置下一滤波周期的时间序号为指定值;

对时间序号为指定值减一的状态量进行无迹变换,获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值;

根据时间序号为指定值的状态量预测值对时间序号为指定值的量测值进行无迹变换,获取时间序号为指定值的量测值估计值、时间序号为指定值的量测值协方差的估计值、时间序号为指定值的状态量与量测值的互协方差;

根据时间序号为指定值的时刻的状态量协方差预测值和时间序号为指定值的状态量与量测值的互协方差构造伪观测矩阵,利用伪观测矩阵对非线性量测方程进行线性化改写,获取改写公式;

根据改写公式计算高斯核的系数;

利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值;

更新时间序号为指定值的状态量协方差;

反馈状态量后验估计值至惯性系统内,补偿惯性系统的参数;

设置指定值等于指定值加一,并返回所述对时间序号为指定值减一的状态量进行无迹变换,获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值的步骤。

在一实施例中,处理器502所述处理器在执行所述程序指令而实现所述建立惯性系统参数误差模型步骤时,具体实现如下步骤:

利用惯性系统的参数误差构造状态方程;

利用惯性系统与卫星系统的测量信息的差值建立量测方程;

根据状态方程和量测方程组成惯性系统参数误差模型。

在一实施例中,处理器502所述处理器在执行所述程序指令而实现所述初始化惯性系统参数误差模型,设置下一滤波周期的时间序号为指定值步骤时,具体实现如下步骤:

时间序号为零的状态量及时间序号为零的协方差进行初始化;

设置下一滤波周期的时间序号为一。

上述的量测值协方差的估计值采用以下公式计算:其中,rk是测量噪声的协方差,是时间序号为指定值的量测值估计值;py,k|k-1是时间序号为指定值的量测值协方差的估计值;是关联协方差矩阵对应的权值。

上述的伪观测矩阵采用以下公式构造:其中,pk|k-1是时间序号为指定值的状态量协方差预测值,pxy,k|k-1是时间序号为指定值的状态量与量测值的互协方差。

上述的改写公式为其中,是时间序号为指定值的状态量预测值,pk|k-1是时间序号为指定值的状态量协方差预测值;是伪观测矩阵,是时间序号为指定值的状态量预测值,py,k|k-1是时间序号为指定值的量测值协方差的估计值。

在一实施例中,处理器502所述处理器在执行所述程序指令而实现所述利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值步骤时,具体实现如下步骤:

利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取卡尔曼滤波增益;

根据卡尔曼滤波增益获取时间序号为指定值的状态量后验估计值。

所述存储介质可以是u盘、移动硬盘、只读存储器(read-onlymemory,rom)、磁碟或者光盘等各种可以存储程序代码的计算机可读存储介质。

本领域普通技术人员可以意识到,结合本文中所公开的实施例描述的各示例的单元及算法步骤,能够以电子硬件、计算机软件或者二者的结合来实现,为了清楚地说明硬件和软件的可互换性,在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行,取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能,但是这种实现不应认为超出本发明的范围。

在本发明所提供的几个实施例中,应该理解到,所揭露的装置和方法,可以通过其它的方式实现。例如,以上所描述的装置实施例仅仅是示意性的。例如,各个单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式。例如多个单元或组件可以结合或者可以集成到另一个系统,或一些特征可以忽略,或不执行。

本发明实施例方法中的步骤可以根据实际需要进行顺序调整、合并和删减。本发明实施例装置中的单元可以根据实际需要进行合并、划分和删减。另外,在本发明各个实施例中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以是两个或两个以上单元集成在一个单元中。

该集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个存储介质中。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分,或者该技术方案的全部或部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可以是个人计算机,终端,或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。

上述仅以实施例来进一步说明本发明的技术内容,以便于读者更容易理解,但不代表本发明的实施方式仅限于此,任何依本发明所做的技术延伸或再创造,均受本发明的保护。本发明的保护范围以权利要求书为准。

当前第1页1 2 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1