本发明主要涉及航天导航技术领域,涉及一种非完备外测数据在实时滤波中的使用方法,具体涉及一种非完备外测数据在ukf及ekf实时滤波计算中的使用方法。
背景技术:
航天器飞行过程的轨道测量数据中,扩频及s、c波段统一测控设备的外测测量数据是一类非常重要的观测数据。但此类数据中测距、测速、测角等信息在实际采集及传输过程中不在一帧数据中存在,从而给需要同时使用这些数据的实时滤波定位计算带来一些问题,即通常需要先将此类数据通过时标的插值对齐及数据拼接转换成信息完备(指测距、测角均有的数据,因一组测距和测角数据可转换成东北天坐标系下的位置分量)的数据帧再进行使用。而这种数据的插值对齐及拼帧操作不但增加了观测数据预处理的复杂度,而且不可避免地带来一些精度损失。而且有些场合下,某些测量设备未发送上行信号,此时仅能输出测角数据,另外,地面光学设备仅可给出对航天器的测角信息,此时就无法得到完备的观测数据。
目前对非完备外测数据在定位计算中的使用多集中于下面几种方法:1)多站几何方法,如使用三站测距、双站测角等进行定位计算;2)仅对某一类非完备外测数据(如仅测角、仅测距)针对性建立滤波观测方程;3)在单位矢量法短弧定轨中使用;4)通过对非完备数据中缺失的测量元素采用外推预测值补齐的方法组成完备数据帧再进行使用;5)对非完备数据进行插值和拼帧,形成完备观测数据。
这些方法存在的局限性主要有:1)不能在滤波定位中实时自适应使用各类非完备外测数据(如仅测距、仅测速、仅测角等);2)几何方法受测量数据精度影响显著;3)预测值补足方法需要较精确的预报模型;4)插值拼帧法增加了数据处理复杂度。
技术实现要素:
本发明的目的是提供一种非完备外测数据在实时滤波中的使用方法,能够在ukf、ekf滤波定位(定轨)计算过程中自适应使用各种不同类型非完备外测观测数据。
本发明采用的第一种技术方案是,一种非完备外测数据在实时滤波中的使用方法,具体按照下述步骤进行:
步骤1,对ukf滤波进行初始状态估计;
步骤2,将当前帧外测数据中的每个测量元素进行标识赋值,得到变维的观测矢量;
对当前ukf滤波系统状态进行ukf采样,得到多个采样点,分别计算每采样点的权重wx(i)和协方差权值wp(i);
步骤3,对系统状态进行ukf采样,得到多个采样点,并分别计算每采样点的权重wx(i)和协方差权值wp(i);
步骤4,对每个采样点均进行状态外推计算,并计算系统状态预测均值
步骤5,计算状态外推后各采样点的观测预测值
步骤6,根据变维观测预测协方差矩阵和变维相关协方差矩阵得到滤波增益矩阵;
步骤7,根据所述滤波增益矩阵对当前ukf滤波的系统状态预测均值和状态协方差矩阵进行更新。
本发明第一种技术方案的特点还在于:
滤波器的观测模型建立在测站地平坐标系下,轨道运动状态模型建立在j2000地心惯性系下。
步骤1中采用轨道外推或多点定轨的方法估计初始时间t0时刻的初始位置矢量r0和初始速度矢量
步骤2中具体按照下述方法进行:
步骤2.1,取当前帧的外测观测数据,判断外测观测数据中每个测量元素的有效性;
步骤2.2,将有效的测量元素,按照测距、方位角、仰角、测速的顺序,得到变维的观测矢量。
本发明采用的另一种技术方案是:
一种非完备外测数据在实时滤波中的使用方法,具体按照下述步骤进行:
步骤a,对ekf滤波进行初始状态估计;
步骤b,计算ekf滤波状态转移估计,得到ekf滤波的状态转移矩阵;
步骤c,计算状态向量的最优预测,得到状态预测值;
计算状态协方差矩阵的最优预测
步骤d,将当前帧将当前帧外测数据中的每个测量元素进行标识赋值,得到变维的观测矢量,将观测矢量对状态向量求偏导得到变维的观测矩阵;
步骤e,计算变维的观测残差向量yk+1;
步骤f,计算最优增益矩阵kk+1;
步骤d,进行状态及协方差更新。
本发明另一种技术方案的特点还在于:
滤波器的观测模型建立在测站地平坐标系下,轨道运动状态模型建立在j2000地心惯性系下。
步骤d中具体按照下述方法进行:
步骤2.1,取当前帧的外测观测数据,判断外测观测数据中每个测量元素的有效性;
步骤2.2,将有效的测量元素,按照测距、方位角、仰角、测速的顺序,得到变维的观测矢量。
本发明的有益效果是
1)本发明一种非完备外测数据在实时滤波中的使用方法,通过将观测模型直接建立在测站地平坐标系下,在滤波计算中能针对各个测站、各个不同测量元素的误差特性实时进行滤波的增益改变,从而有效避免较差质量数据对系统状态的影响;
2)本发明一种非完备外测数据在实时滤波中的使用方法,能够在一个滤波器中直接将各类非完备及完备外测数据帧混合使用,避免了一个滤波器中只能使用一种类型数据的情况。
3)本发明一种非完备外测数据在实时滤波中的使用方法,通过对各个观测值进行滤波方差检验的方法,可在一定范围内去除野值数据,使得算法具有一定的野值剔除及容错能力。
4)本发明一种非完备外测数据在实时滤波中的使用方法,实现思路同样适用于容积卡尔曼滤波(ckf)、中心差分卡尔曼滤波(cdkf)等算法下的非完备外测数据使用(类似ukf)。
附图说明
图1是本发明一种非完备外测数据在实时滤波中的使用方法中,一种非完备外测数据在ukf实时滤波中的使用方法的计算流程图;
图2是发明一种非完备外测数据在实时滤波中的使用方法中,一种非完备外测数据在ekf实时滤波中的使用方法的计算流程图。
具体实施方式
下面结合附图和具体实施方式对本发明进行详细说明。
一种非完备外测数据在实时ukf滤波中的使用方法,如图1所示,具体按照下述步骤进行:
步骤1,将滤波器的观测模型直接建立在测站地平坐标系,轨道运动状态模型建立在j2000地心惯性系下,则:
若将某时刻的外测测量数据中的测距、方位角、仰角、测速信息分别标记为ρ、a、e、
[ρxρyρz]=mi(r-rs)(1)
对测距、方位角、仰角、测速等测元值有
a=arctan(ρx/ρy)(3)
即可将地心惯性系的运动状态矢量转换到测站地平系下的外测测量量。
对ukf滤波进行初始状态估计:
采用轨道外推或多点定轨的方法估计初始时间t0时刻的初始位置矢量r0和初始速度矢量
步骤2,将当前帧外测数据中的每个测量元素进行标识赋值,得到变维的观测矢量,具体按照下述方法进行:
步骤2.1,取当前帧的外测观测数据,判断外测观测数据中每个测量元素的有效性;
步骤2.2,基于各个测量元素的有效标志,按照测距、方位角、仰角、测速的顺序,得到变维的观测矢量,具体按照下述方法进行:
在当前帧的外观测数据中增加有效性字段,如表1,使用一个字节的比特位表示各个测量元素的有效性:
表1比特位与所代表元素有效标识对应关系
如表1所示,定义b0位表示测距ρ的有效性,b1位表示方位角a的有效性,b2位表示仰角e有效性,b3位表示测速
并根据比特位赋值为1的测量元素得到变维的观测矢量,具体方法为:观测矢量维数等于有效标志字节中的有效位个数,其构成按照测距、方位角、仰角、测速的顺序,将每个测量元素的观测值写入观测矢量中,某元素有效位为0时不写入观测矢量,这样即可得到变维的观测矢量。
如某帧数据有效标志字节的二进制表示为00001110,则观测矢量为
步骤3,对系统状态进行ukf采样,得到多个采样点,并分别计算每采样点的权重wx(i)和协方差权值wp(i)。
步骤4,对每个采样点均进行状态外推计算,并计算系统状态预测均值
其中,
步骤5,计算状态外推后各采样点的观测预测值
式中,h为观测矩阵,观测模型直接建立在测站地平坐标系下,以更好适应非完备外测数据情况。
步骤6,计算观测预测协方差矩阵pzz(变维矩阵)、状态向量与观测向量之间相关协方差矩阵pxz,观测预测协方差矩阵pzz和相关协方差矩阵pxz均为变维矩阵:
其中,r为变维的观测噪声协方差矩阵。在当前外观测数据帧中,若仅有测距ρ有效,观测噪声协方差矩阵为:
步骤6,由观测预测协方差矩阵和观测向量与状态量间相关协方差矩阵计算滤波增益矩阵;
步骤7,对当前ukf滤波的系统状态和状态协方差矩阵进行更新。
式中,
式中,
从ukf的算法步骤看出,其计算过程主要为矩阵与向量运算,因而根据外测数据中各类测元信息设置的可用性标志,可动态调整各种矩阵及矢量的维数,以实现非完备观测信息对指定维上的状态协方差矩阵的更新,从而在一个滤波器中将各类不同类型的非完备观测数据进行混合使用。
一种非完备外测数据在实时滤波中的使用方法,在ekf滤波框架下的计算如图2所示,具体按照下述步骤进行:
步骤a,采用轨道外推或多点定轨的方法对ekf滤波进行初始状态估计。
步骤b,计算ekf滤波状态转移估计,得到ekf滤波的状态转移矩阵;
步骤c,计算状态向量的最优预测,得到状态预测值:
xk+1=φ(tk,tk+1)·xk+w(tk,tk+1)(14)
其中,φ(tk,tk+1)为状态向量进行外推过程中,k时刻和k+1时刻间的状态转移矩阵,w(tk,tk+1)为状态模型噪声。
计算状态估计协方差矩阵p的最优预测为:
式中,q为状态噪声协方差。
步骤d,根据有效标识,计算变维的观测矩阵h,观测矩阵由观测向量对状态向量求偏导得到,具体计算参见ekf滤波定位算法
步骤e,计算变维的观测残差向量yk+1
其中,zk+1为k+1时刻的观测矢量,h为观测矩阵。
其中观测矢量是变维的观测矢量,首先判断当前外测观测数据帧中每个测量元素的有效性;将有效的测量元素,按照测距、方位角、仰角、测速的顺序放入观测向量中,即可得到变维的观测矢量。
步骤f:计算最优增益矩阵kk+1:
其中,r为观测噪声的协方差矩阵。
步骤g:进行状态及协方差更新。
滤波状态更新方程为
协方差矩阵更新方程为: