CN109752006B - 一种非完备外测数据在实时滤波中的使用方法 - Google Patents
一种非完备外测数据在实时滤波中的使用方法 Download PDFInfo
- Publication number
- CN109752006B CN109752006B CN201811407764.7A CN201811407764A CN109752006B CN 109752006 B CN109752006 B CN 109752006B CN 201811407764 A CN201811407764 A CN 201811407764A CN 109752006 B CN109752006 B CN 109752006B
- Authority
- CN
- China
- Prior art keywords
- data
- observation
- state
- filtering
- measurement
- 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.)
- Active
Links
- 238000005259 measurement Methods 0.000 title claims abstract description 72
- 238000001914 filtration Methods 0.000 title claims abstract description 48
- 238000000034 method Methods 0.000 title claims abstract description 44
- 239000013598 vector Substances 0.000 claims abstract description 50
- 239000011159 matrix material Substances 0.000 claims abstract description 47
- 238000004364 calculation method Methods 0.000 claims abstract description 16
- 238000005070 sampling Methods 0.000 claims description 25
- 238000013213 extrapolation Methods 0.000 claims description 12
- 230000008859 change Effects 0.000 abstract description 2
- 238000005516 engineering process Methods 0.000 abstract 1
- 230000007704 transition Effects 0.000 description 5
- 230000008569 process Effects 0.000 description 4
- FGUUSXIOTUKUDN-IBGZPJMESA-N C1(=CC=CC=C1)N1C2=C(NC([C@H](C1)NC=1OC(=NN=1)C1=CC=CC=C1)=O)C=CC=C2 Chemical compound C1(=CC=CC=C1)N1C2=C(NC([C@H](C1)NC=1OC(=NN=1)C1=CC=CC=C1)=O)C=CC=C2 FGUUSXIOTUKUDN-IBGZPJMESA-N 0.000 description 2
- GNFTZDOKVXKIBK-UHFFFAOYSA-N 3-(2-methoxyethoxy)benzohydrazide Chemical compound COCCOC1=CC=CC(C(=O)NN)=C1 GNFTZDOKVXKIBK-UHFFFAOYSA-N 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000009432 framing Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000001228 spectrum Methods 0.000 description 1
- 230000001502 supplementing effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Landscapes
- Radar Systems Or Details Thereof (AREA)
Abstract
本发明公开了一种非完备外测数据在实时滤波中的使用方法，将滤波器的观测模型直接建立在测站地平坐标系下；定义统一的结构表示每帧数据中各个测量元素的有效性；在滤波计算中根据当前数据帧测量元素个数对与观测量相关的矩阵及向量采取自适应变维处理等技术来实现各类完备、非完备数据的混合使用。本发明的使用方法在滤波计算中能针对各个测站、各个不同测量元素的误差特性实时进行滤波的增益改变，从而有效避免较差质量数据对系统状态的影响。
Description
技术领域
本发明主要涉及航天导航技术领域，涉及一种非完备外测数据在实时滤波中的使用方法，具体涉及一种非完备外测数据在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，对每个采样点均进行状态外推计算，并计算系统状态预测均值
与状态协方差矩阵PXX；
步骤5，计算状态外推后各采样点的观测预测值
和UKF滤波的观测预测均值
其中各采样点的观测预测值
和UKF滤波的观测预测均值
均根据有效状态标志进行变维处理
步骤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、
J2000地心惯性系下航天器位置矢量标记为r＝(x,y,z)，速度矢量标记为
测站原点大地坐标记为λ,φ,h；在地心惯性系下的测站位置、速度分别记为Rs,
(可由测站大地坐标算出)；地心惯性系到测站地平系转换矩阵为MI，则测站地平坐标系下航天器位置坐标为
[ρ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位表示测速
的有效性，给每个比特位初始化赋值为0，若某测量元素有效时，有效标志字节中表示该元素的比特位赋值为1。
并根据比特位赋值为1的测量元素得到变维的观测矢量，具体方法为：观测矢量维数等于有效标志字节中的有效位个数，其构成按照测距、方位角、仰角、测速的顺序，将每个测量元素的观测值写入观测矢量中，某元素有效位为0时不写入观测矢量，这样即可得到变维的观测矢量。
如某帧数据有效标志字节的二进制表示为00001110，则观测矢量为
若为00001001，则观测矢量为
若为00000110，则观测矢量为[A E]；若为00000001,则观测矢量为[ρ]；若标志位为00001111，则观测矢量为
其它情况同理。
步骤3，对系统状态进行UKF采样，得到多个采样点，并分别计算每采样点的权重WX(i)和协方差权值WP(i)。
步骤4，对每个采样点均进行状态外推计算，并计算系统状态预测均值
与状态协方差矩阵PXX：
其中，
为状态向量X的sigma采样点序列，Q为状态噪声协方差矩阵。
步骤5，计算状态外推后各采样点的观测预测值
和UKF滤波的观测预测均值
其中各采样点的观测预测值
和UKF滤波的观测预测均值
均根据有效状态标志进行变维处理：
式中，h为观测矩阵，观测模型直接建立在测站地平坐标系下，以更好适应非完备外测数据情况。
步骤6，计算观测预测协方差矩阵PZZ(变维矩阵)、状态向量与观测向量之间相关协方差矩阵PXZ，观测预测协方差矩阵PZZ和相关协方差矩阵PXZ均为变维矩阵：
其中，R为变维的观测噪声协方差矩阵。在当前外观测数据帧中，若仅有测距ρ有效，观测噪声协方差矩阵为：
其中
为ρ的方差；若只有方位角A和仰角E有效时，观测噪声协方差矩阵为
当只有测速数据
有效时，观测噪声协方差矩阵为
当只有测距ρ、测速数据
有效时，观测噪声协方差矩阵为
步骤6，由观测预测协方差矩阵和观测向量与状态量间相关协方差矩阵计算滤波增益矩阵；
步骤7，对当前UKF滤波的系统状态和状态协方差矩阵进行更新。
式中，
表示更新后的当前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：进行状态及协方差更新。
滤波状态更新方程为
协方差矩阵更新方程为：
Claims (4)
1.一种非完备外测数据在实时滤波中的使用方法，其特征在于，具体按照下述步骤进行：
步骤1，对UKF滤波进行初始状态估计；
步骤2，将当前帧外测数据中的每个测量元素进行标识赋值，得到变维的观测矢量；
对当前UKF滤波系统状态进行UKF采样，得到多个采样点，分别计算每采样点的权重WX(i)和协方差权值WP(i)；
步骤3，对系统状态进行UKF采样，得到多个采样点，并分别计算每采样点的权重WX(i)和协方差权值WP(i)；
步骤4，对每个采样点均进行状态外推计算，并计算系统状态预测均值
与状态协方差矩阵PXX；
步骤5，计算状态外推后各采样点的观测预测值
和UKF滤波的观测预测均值
其中各采样点的观测预测值
和UKF滤波的观测预测均值
均根据有效状态标志进行变维处理；
步骤6，根据变维观测预测协方差矩阵和变维相关协方差矩阵得到滤波增益矩阵；
步骤7，根据所述滤波增益矩阵对当前UKF滤波的系统状态预测均值和状态协方差矩阵进行更新。
2.根据权利要求1所述的一种非完备外测数据在实时滤波中的使用方法，其特征在于，滤波器的观测模型建立在测站地平坐标系下，轨道运动状态模型建立在J2000地心惯性系下。
3.根据权利要求1所述的一种非完备外测数据在实时滤波中的使用方法，其特征在于，所述步骤1中采用轨道外推或多点定轨的方法估计初始时间t0时刻的初始位置矢量r0和初始速度矢量
4.根据权利要求1所述的一种非完备外测数据在实时滤波中的使用方法，其特征在于，所述步骤2中具体按照下述方法进行：
步骤2.1，取当前帧的外测观测数据，判断外测观测数据中每个测量元素的有效性；
步骤2.2，将有效的测量元素，按照测距、方位角、仰角、测速的顺序，得到变维的观测矢量。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811407764.7A CN109752006B (zh) | 2018-11-23 | 2018-11-23 | 一种非完备外测数据在实时滤波中的使用方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811407764.7A CN109752006B (zh) | 2018-11-23 | 2018-11-23 | 一种非完备外测数据在实时滤波中的使用方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109752006A CN109752006A (zh) | 2019-05-14 |
| CN109752006B true CN109752006B (zh) | 2022-09-09 |
Family
ID=66402560
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811407764.7A Active CN109752006B (zh) | 2018-11-23 | 2018-11-23 | 一种非完备外测数据在实时滤波中的使用方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109752006B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112595319B (zh) * | 2020-11-18 | 2023-09-22 | 中国西安卫星测控中心 | 一种模型自适应补偿的返回弹道估计方法 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103438892B (zh) * | 2013-09-16 | 2015-09-30 | 哈尔滨工业大学 | 一种改进的基于ekf的天文自主定轨算法 |
| CN105549049B (zh) * | 2015-12-04 | 2018-10-02 | 西北农林科技大学 | 一种应用于gps导航的自适应卡尔曼滤波算法 |
| CN107421550B (zh) * | 2017-07-25 | 2020-08-28 | 北京航空航天大学 | 一种基于星间测距的地球-Lagrange联合星座自主定轨方法 |
| CN108120994B (zh) * | 2017-10-30 | 2020-02-21 | 千寻位置网络（浙江）有限公司 | 一种基于星载gnss的geo卫星实时定轨方法 |
| CN108844536B (zh) * | 2018-04-04 | 2020-07-03 | 中国科学院国家空间科学中心 | 一种基于量测噪声协方差矩阵估计的地磁导航方法 |
| CN108692729B (zh) * | 2018-05-04 | 2019-05-24 | 北京空间飞行器总体设计部 | 一种空间非合作目标相对导航协方差自适应修正滤波方法 |
-
2018
- 2018-11-23 CN CN201811407764.7A patent/CN109752006B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN109752006A (zh) | 2019-05-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105371840B (zh) | 一种惯性/视觉里程计/激光雷达的组合导航方法 | |
| CN108802707A (zh) | 改进的用于目标跟踪的卡尔曼滤波方法 | |
| CN110779518B (zh) | 一种具有全局收敛性的水下航行器单信标定位方法 | |
| CN111156987A (zh) | 基于残差补偿多速率ckf的惯性/天文组合导航方法 | |
| CN106568442B (zh) | 一种具有鲁棒特性的协同导航滤波方法 | |
| CN108981696B (zh) | 一种sins任意失准角无奇异快速传递对准方法 | |
| CN107229037B (zh) | 移动平台传感器量测数据扩维空间配准方法 | |
| CN110779519B (zh) | 一种具有全局收敛性的水下航行器单信标定位方法 | |
| CN112986977B (zh) | 一种克服雷达扩展卡尔曼航迹滤波发散的方法 | |
| CN110749891A (zh) | 一种可估计未知有效声速的自适应水下单信标定位方法 | |
| CN110933597A (zh) | 一种基于蓝牙的多无人车协同容错导航定位方法及系统 | |
| CN107860382B (zh) | 一种在地磁异常情况下应用ahrs测量姿态的方法 | |
| CN111623779A (zh) | 一种适用于噪声特性未知的时变系统自适应级联滤波方法 | |
| CN112985384B (zh) | 一种抗干扰磁航向角优化系统 | |
| CN109752006B (zh) | 一种非完备外测数据在实时滤波中的使用方法 | |
| CN109470251A (zh) | 一种用于组合导航系统中的部分反馈滤波方法 | |
| CN116608859A (zh) | 一种基于阈值处理的自适应无迹卡尔曼滤波的导航方法、存储介质和设备 | |
| Vasconcelos et al. | Discrete time-varying attitude complementary filter | |
| CN112595319B (zh) | 一种模型自适应补偿的返回弹道估计方法 | |
| CN110736468A (zh) | 自适应运动学模型辅助的航天器姿态估计方法 | |
| CN115560757A (zh) | 随机姿态误差条件下基于神经网络的无人机直接定位校正方法 | |
| Filaretov et al. | Formation control of AUV on the base of visual tracking of AUV-leader | |
| CN109856624B (zh) | 一种针对单雷达直线航迹线的目标状态估计方法 | |
| CN110263400B (zh) | 一种无人艇非线性动力学模型的积分辨识方法 | |
| Li et al. | Exploring the Potential of Deep Learning Aided Kalman Filter for GNSS/INS Integration: A Study on 2D Simulation Datasets |
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 |