CN109000638A - 一种小视场星敏感器量测延时滤波方法 - Google Patents
一种小视场星敏感器量测延时滤波方法 Download PDFInfo
- Publication number
- CN109000638A CN109000638A CN201810519570.XA CN201810519570A CN109000638A CN 109000638 A CN109000638 A CN 109000638A CN 201810519570 A CN201810519570 A CN 201810519570A CN 109000638 A CN109000638 A CN 109000638A
- Authority
- CN
- China
- Prior art keywords
- measurement
- state
- variance
- star sensor
- delay
- 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.)
- Pending
Links
- 238000005259 measurement Methods 0.000 title claims abstract description 85
- 238000001914 filtration Methods 0.000 title claims abstract description 64
- 238000000034 method Methods 0.000 title claims abstract description 48
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 19
- 238000013461 design Methods 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 48
- 238000010606 normalization Methods 0.000 claims description 12
- 239000013598 vector Substances 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 4
- PHTXVQQRWJXYPP-UHFFFAOYSA-N ethyltrifluoromethylaminoindane Chemical compound C1=C(C(F)(F)F)C=C2CC(NCC)CC2=C1 PHTXVQQRWJXYPP-UHFFFAOYSA-N 0.000 claims description 3
- 238000004064 recycling Methods 0.000 abstract 1
- 238000012545 processing Methods 0.000 description 6
- 239000000654 additive Substances 0.000 description 3
- 230000000996 additive effect Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 1
- 238000010835 comparative analysis Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012886 linear function Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
Classifications
-
- 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/02—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by astronomical means
- G01C21/025—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by astronomical means with the use of startrackers
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Astronomy & Astrophysics (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种小视场星敏感器量测延时滤波方法,属于飞行器姿态估计技术领域。本发明考虑到非线性姿态估计系统状态模型存在乘性噪声以及量测模型存在未知干扰的情况,建立了两种模型不确定存在下的飞行器姿态估计模型,同时基于扩展卡尔曼滤波的结构,利用鲁棒算法设计找到预测方差及估计方差的上界范围,再利用最小方差理论设计最优的滤波增益。本发明采用鲁棒扩展卡尔曼滤波对飞行器姿态确定,其本质是一种最小方差准则估计,不同于一般的频率域滤波器,本发明可以用于处理不确定性条件下进行姿态估计,具有实时性高、对先验信息依赖度低等优势,解决了星敏感器存在延时的问题,即使在信息不确定的情况下也能保证较高的姿态估计精度。
Description
技术领域
本发明属于飞行器姿态估计技术领域,具体涉及一种利用鲁棒扩展卡尔曼滤波的小视场星敏感器量测延时滤波方法。
背景技术
由陀螺和星敏感器组合的姿态估计系统由于定姿精度高被广泛地应用于飞行器中。欧拉角、修正罗德里格斯参数、方向余弦、四元数等是飞行器的主要姿态描述参数。四元数由于计算简单,无三角函数的运算,同时又能避免欧拉角的奇异性问题,因此,四元数常被作为姿态估计系统中的姿态描述参数。针对该系统的四元数姿态估计模型,许多基于扩展卡尔曼滤波(Extended Kalman Filter,EKF)的姿态估计方法被提出,如加性扩展卡尔曼滤波(Additive Extended Kalman Filter,AEKF)和乘性扩展卡尔曼滤波(MultiplicativeExtended Kalman Filter,MEKF)。然而,扩展卡尔曼滤波仅仅适合于带加性噪声的精确已知的系统模型。如果系统模型中存在模型不确定的情况,该算法的性能将会受到严重的影响。因此,许多带模型不确定的非线性滤波算法被发展,如H∞滤波,集值非线性滤波,鲁棒滤波设计等,其中,基于最小方差的鲁棒滤波设计被证明是一种用于解决系统带模型不确定情况的有效处理手段,然而大多数基于最小方差的鲁棒滤波都是针对系统只存在一种模型不确定的情况。如果系统中存在两种或是两种以上的模型不确定,上述的鲁棒滤波算法将会失效。
针对姿态估计系统,系统状态方程中存在状态与高斯白噪声耦合的乘性噪声,该噪声的方差未知,因此被看成是一种状态模型不确定,一种鲁棒扩展卡尔曼滤波(RobustExtended Kalman Filter,REKF)被提出。由于小视场星敏感器独有的优势将其用于研究小视场下姿态估计算法中。视场减小使得星敏感器捕获星点数目减少,导致星图识别准确率大幅下降,影响后续姿态估计的精度。为了解决星点丢失,我们对导航星表进行预处理,使得小视场星敏感器对暗星的探测能力增强。我们需要选取不同的星等的星点进行导航星表构建,构建的导航星表相比大视场星敏感器的星表要大,所以在星图识别匹配阶段时,星图识别的时间增加导致了延时,影响姿态信息输出的实时性。同时,由于光学系统以及确定姿态等步骤的繁琐性,也导致了姿态敏感器输出信息的同时带有延时现象。针对小视场星敏感器的特点,将陀螺仪与星敏感器组合进行姿态估计,同时考虑星图识别及数据处理中不可避免的延时现象,建立带有量测信息延时的不确定性系统误差模型。基于求取最小误差方差准则的鲁棒滤波算法被证实是一种处理模型不确定时有效算法,故在鲁棒滤波的基础上,给出一种改进的鲁棒扩展滤波算法(REKF)处理量测延迟的问题。当构建的导航星表内满足星图识别的最低三颗星数的要求后,通过鲁棒扩展卡尔曼滤波算法来解决量测延时问题;若构建的导航星表内仍不满足最低三颗星数的要求,即用鲁棒扩展卡尔曼滤波算法来解决量测延时问题。
发明内容
本发明的目的在于提供一种提高姿态估计的精度和增强系统的鲁棒性的小视场星敏感器量测延时滤波方法。
本发明的目的是这样实现的:
本发明公开了一种小视场星敏感器量测延时滤波方法,包括以下步骤:
(1)采集飞行器运动过程中陀螺与星敏感器的输出数据;
(2)建立量测延时下基于姿态四元数的飞行器非线性状态空间模型,包括建立飞行器姿态估计系统的状态方程及量测延时下飞行器姿态估计系统的量测方程;
(3)针对上述量测延时下的飞行器状态空间模型,已知k时刻的状态估计值和方差的上界,以扩展卡尔曼滤波为结构框架,利用鲁棒扩展卡尔曼滤波设计,进行时间更新,求得一步状态预测和预测方差的上界;
(4)利用上述求得的一步状态预测和预测方差的上界,进行鲁棒扩展卡尔曼滤波量测更新,求得最优的滤波增益,进而求出k+1时刻的状态估计值和方差的上界,同时由于四元数归一化约束,将k+1时刻的状态估计中四元数部分进行强制的归一化约束;
(5)姿态估计系统的运行时间为M,若k=M,则输出姿态四元数及陀螺漂移的结果,完成姿态估计;若k<M,表示滤波过程未完成,则重复步骤(3)至步骤(4),直至姿态估计系统运行结束。
对于一种小视场星敏感器量测延时滤波方法,所述的步骤(2)具体包括以下步骤:
(2.1)建立飞行器姿态估计系统的状态方程:
将姿态四元数qk和陀螺漂移βk组成维数为n的状态变量根据飞行器的运动学方程,建立基于姿态四元数的飞行器状态方程为:
式中:wk满足均值为零,方差为的高斯白噪声;是乘性噪声项,s选取3,ηi,k表示均值为零,方差为1的噪声,Aik表示有适当阶数的确定矩阵,为:
式中:σv为陀螺测量噪声;
(2.2)建立量测延时下飞行器姿态估计系统的量测方程:
式中:zk为k时刻星敏感器量测值;rk为星敏感器的参考矢量;i为星敏感器观测到恒星的个数;vk为零均值的高斯白噪声,其方差为A(qk)为四元数qk的姿态描述矩阵,若A(qk)能表示为:
由于星敏感器进行姿态估计时,不可避免的存在量测延迟现象,上述建立的星敏感器量测模型没有考虑延迟的存在,使得输出结果并不准确,我们考虑一步延时的情况给出带有延迟的星敏感器测量模型:
yk=(Ι-Γk)zk+Γkzk
yk表示k时刻真实的量测量,Γk表示不同的延迟速率,满足Γk=diag{[μk,1μk,2....μk,m]},其中μk,i∈R(i=1,2...m)服从伯努利分布(0~1分布),满足:
关于量测延时的非线性离散系统:
由于实际的量测输出yk与zk、zk-1两个时刻的量测量均相关,需要利用状态扩维理论,使其得到相同时刻量测量的表达式:
式中,是扩维后的状态变量,是扩维后的状态函数,表示扩维后的乘性噪声项,表示扩维后的高斯白噪声,表示扩维后的量测函数,表示扩维后的量测噪声,γk=[Ι-ΓkΓk]表示扩维后的量测延时项,
其中,由Γk的分布特性可知,是一个零均值的随机矩阵序列;h(xk)满足:
σ1、σ2是标量。
优选的,所述的步骤(2)中陀螺测量噪声标准差为陀螺漂移噪声标准差为陀螺的采样周期为Δt=0.25s;星敏感器测量噪声标准差为σs=0.005deg,输出频率为1Hz;星敏感器的参考矢量设为r1=[100]Τ,r2=[010]Τ,r3=[001]Τ;初始陀螺漂移β0=[0.10.10.1]Tdeg/h;σij=10sec(i=1,2,3;j=x,y,z)。
对于一种小视场星敏感器量测延时滤波方法,所述的步骤(3)具体包括:
假设k时刻的状态估计值为方差上界为Pk,根据鲁棒扩展卡尔曼滤波,求得一步状态预测值为
一步状态预测误差为:
扩维后的状态函数f'(Xk)通过在点处泰勒展开,得到:
式中, 表示经过展开后的高阶项,通过引入未知时变矩阵βk∈Rn×n,满足具体的缩放矩阵Bk∈Rn×n,Lk表示已知的调节矩阵,故高阶项可表示为:
一步预测误差方差矩阵可表示为:
式中:
而ηik与wk是互相独立的高斯白噪声,均值为零;故:Ε[WkgT(Xk,ηk)]、
这几项都是零矩阵;
则一步预测方差可简化表示为:
根据二阶矩的概念可以得到:
假设存在正数λ和矩阵Lk,满足条件能推导出:
求得一步预测误差方差矩阵的上界:
优选的,所述的步骤(3)中初始状态估计值为x0=[0 0 0 1 0.1 0.1 0.1]T;忽略非线性函数线性化的高阶项,Bk,Dk+1设置为0;调节矩阵为了保证系统能达到的精度,参数λ和μ均设置为0.0001,参数ε设置为0.1;延时速率设为
对于一种小视场星敏感器量测延时滤波方法,所述的步骤(4)具体包括:
同EKF算法中量测更新步骤类似,对量测函数泰勒展开,保留高阶项得到:
式中,αk+1是未知的时变矩阵,Dk+1是考虑不确定系统模型误差的比例矩阵;
将式(23)代入一步状态预测值中,所以有:
将上式代到滤波误差协方差矩阵,表示为:
其中, 表示为:
同样根据二阶矩原理,假设存在正数ε2、ε3:
假设存在正数μ和矩阵Lk+1,满足条件能推出:
所以,k+1时刻的更新后状态误差的方差矩阵为:
利用最小方差理论,令求得最优滤波增益为:
最后利用求得的最优滤波增益求出k+1时刻的状态估计值和方差的上界Pk+1;
由于状态变量四元数部分满足四元数归一化约束||qk||=1,因此再将k+1时刻的状态估计值中的四元数部分进行归一化约束。
优选的,所述的步骤(4)中Cik=0。
优选的,所述的步骤(5)中,M=1000。
本发明的有益效果在于:
本发明考虑到非线性姿态估计系统状态模型存在乘性噪声以及量测模型存在未知干扰的情况,建立了两种模型不确定存在下的飞行器姿态估计模型,同时基于扩展卡尔曼滤波的结构,利用鲁棒算法设计找到预测方差及估计方差的上界范围,再利用最小方差理论设计最优的滤波增益。
本发明同时考虑到非线性姿态估计系统状态模型存在乘性噪声以及量测模型存在未知干扰的情况,有利于提高系统的鲁棒性;
本发明采用基于最小方差的鲁棒滤波设计,能够实现在最小方差意义下最优滤波增益设计,有利用提高系统的姿态估计精度。
本发明创新性的采用鲁棒扩展卡尔曼滤波对飞行器姿态确定,其本质是一种最小方差准则估计,不同于一般的频率域滤波器,本发明可以用于处理不确定性条件下进行姿态估计,具有实时性高、对先验信息依赖度低等优势,解决了星敏感器存在延时的问题,即使在信息不确定的情况下也能保证较高的姿态估计精度。
附图说明
图1为本发明中小视场星敏感器量测延时滤波方法的步骤示意图;
图2为本发明中小视场星敏感器量测延时滤波方法的流程图;
图3为实施例1中姿态角均方根误差对比;
图4为实施例1中各姿态角误差对比;
图5为实施例2中姿态角均方根误差对比;
图6为为实施例2中各姿态角误差对比。
具体实施方式
下面结合附图对本发明做进一步描述。
结合图1及图2,本发明公开了一种小视场星敏感器量测延时滤波方法,包括以下步骤:
步骤一:采集飞行器运动过程中陀螺与星敏感器的输出数据;
步骤二:建立量测延时下基于姿态四元数的飞行器非线性状态空间模型;
步骤2.1建立飞行器姿态估计系统的状态方程;
将姿态四元数qk和陀螺漂移βk组成维数为n的状态变量根据飞行器的运动学方程,建立基于姿态四元数的飞行器状态方程为:
式中:wk满足均值为零,方差为的高斯白噪声。是乘性噪声项,s选取3,ηi,k表示均值为零,方差为1的噪声,Aik表示有适当阶数的确定矩阵,为:
式中:σv为陀螺测量噪声;
步骤2.2建立量测延时下飞行器姿态估计系统的量测方程;
式中:zk为k时刻星敏感器量测值;rk为星敏感器的参考矢量;i为星敏感器观测到恒星的个数;vk为零均值的高斯白噪声,其方差为A(qk)为四元数qk的姿态描述矩阵,若A(qk)能表示为:
由于星敏感器进行姿态估计时,不可避免的存在量测延迟现象,上述建立的星敏感器量测模型没有考虑延迟的存在,使得输出结果并不准确,我们考虑一步延时的情况给出带有延迟的星敏感器测量模型:
yk=(Ι-Γk)zk+Γkzk (5)
yk表示k时刻真实的量测量,Γk表示不同的延迟速率,满足Γk=diag{[μk,1μk,2....μk,m]},其中μk,i∈R(i=1,2...m)服从伯努利分布(0~1分布),满足:
关于量测延时的非线性离散系统:
由于实际的量测输出yk与zk、zk-1两个时刻的量测量均相关,需要利用状态扩维理论,使其得到相同时刻量测量的表达式:
式中,是扩维后的状态变量,是扩维后的状态函数,表示扩维后的乘性噪声项,表示扩维后的高斯白噪声,表示扩维后的量测函数,表示扩维后的量测噪声,γk=[Ι-ΓkΓk]表示扩维后的量测延时项,
其中,由Γk的分布特性可知,是一个零均值的随机矩阵序列。h(xk)满足:
σ1、σ2是标量。
步骤三:针对上述量测延时下的飞行器状态空间模型,已知k时刻的状态估计值和方差的上界,以扩展卡尔曼滤波为结构框架,利用鲁棒扩展卡尔曼滤波设计,进行时间更新,求得一步状态预测和预测方差的上界;
假设k时刻的状态估计值为方差上界为Pk,根据鲁棒扩展卡尔曼滤波,求得一步状态预测值为
一步状态预测误差为:
扩维后的状态函数f'(Xk)通过在点处泰勒展开,得到:
式中, 表示经过展开后的高阶项,通过引入未知时变矩阵βk∈Rn×n,满足具体的缩放矩阵Bk∈Rn×n,Lk表示已知的调节矩阵,故高阶项可表示为:
一步预测误差方差矩阵可表示为:
式中:
而ηik与wk是互相独立的高斯白噪声,均值为零。故:Ε[WkgT(Xk,ηk)]、 这几项都是零矩阵。
那么一步预测方差可简化表示为:
根据二阶矩的概念可以得到:
假设存在正数λ和矩阵Lk,满足条件能推导出:
求得一步预测误差方差矩阵的上界:
步骤四:利用上述求得的一步状态预测和预测方差的上界,进行鲁棒扩展卡尔曼滤波量测更新,求得最优的滤波增益,进而求出k+1时刻的状态估计值和方差的上界,同时由于四元数归一化约束,将k+1时刻的状态估计中四元数部分进行强制的归一化约束;
同EKF算法中量测更新步骤类似,对量测函数泰勒展开,保留高阶项得到:
式中,αk+1是未知的时变矩阵,Dk+1是考虑不确定系统模型误差的比例矩阵。将式(23)代入一步状态预测值中,所以有:
将上式代到滤波误差协方差矩阵,表示为:
其中, 表示为:
同样根据二阶矩原理,假设存在正数ε2、ε3:
假设存在正数μ和矩阵Lk+1,满足条件能推出:
所以,k+1时刻的更新后状态误差的方差矩阵为:
利用最小方差理论,令求得最优滤波增益为:
最后将上述求得的最优滤波增益代入到式(24)和式(28)中,求出k+1时刻的状态估计值和方差的上界Pk+1;
由于状态变量四元数部分满足四元数归一化约束||qk||=1,因此再将k+1时刻的状态估计值中的四元数部分进行归一化约束;
步骤五:姿态估计系统的运行时间为M,若k=M,则输出姿态四元数及陀螺漂移的结果,完成姿态估计;若k<M,表示滤波过程未完成,则重复步骤三至步骤四,直至姿态估计系统运行结束。
对于一种小视场星敏感器量测延时滤波方法,步骤二中,陀螺测量噪声标准差为陀螺漂移噪声标准差为陀螺的采样周期为Δt=0.25s;星敏感器测量噪声标准差为σs=0.005deg,输出频率为1Hz;星敏感器的参考矢量设为r1=[1 0 0]Τ,r2=[0 1 0]Τ,r3=[0 0 1]Τ;初始陀螺漂移β0=[0.1 0.1 0.1]Tdeg/h;σij=10sec(i=1,2,3;j=x,y,z);
对于一种小视场星敏感器量测延时滤波方法,步骤三中,初始状态估计值为x0=[0 00 1 0.1 0.1 0.1]T;忽略非线性函数线性化的高阶项,Bk,Dk+1设置为0;调节矩阵为了保证系统能达到的精度,参数λ和μ均设置为0.0001,参数ε设置为0.1。延时速率设为
对于一种小视场星敏感器量测延时滤波方法,步骤四中,Cik=0。
对于一种小视场星敏感器量测延时滤波方法,步骤五中,M=1000。
为了验证本发明的合理性、可行性,利用Matlab程序对设计的小视场星敏感器量测延时滤波方法进行仿真,为实现对比分析,将本发明方法与AEKF、RKF方法进行比较。
综合考虑各种情况,设定了两个实施例:
实施例1
初始状态值为x0=[0 0 0 1 0.1 0.1 0.1]T,对应的初始四元数部分为q0=[0 00 1]T,初始陀螺漂移部分为β0=[0.1 0.1 0.1]Tdeg/h,星敏感器的三个参考矢量设为r1=[1 0 0]Τ,r2=[0 1 0]Τ,r3=[0 0 1]Τ。星敏感器与陀螺的采样频率为1Hz。初始的三个姿态角误差方差矩阵表示为(0.1deg)2Ι3×3,陀螺漂移的误差方差矩阵表示为(0.2deg/h)2Ι3×3。实际的星敏感器精度较高,我们可以把泰勒展开得到的高阶项忽略,Bk,Dk+1设置为0。调节矩阵为了保证系统能达到的精度,参数λ和μ均设置为0.0001,参数ε设置为0.1。通过第二章给出的状态模型及量测模型可知,其中包括加性噪声,并且我们考虑噪声及延迟对系统的影响,可令矩阵Cik=0。量测模型参数σij=10sec(i=1,2,3;j=x,y,z),仿真时间设为两小时。
实施例2
假设星敏感器工作时出现延时现象,设三个方向的矢量延时速率各不相同,并且满足伯努利分布,有:
仿真结果:
结合图2及图3,当假设系统不存在延时情况时,各滤波算法的性能相差不大,基本都能达到0.0001deg左右的精度。整体来看,REKF和RKF滤波算法要略优于AEKF滤波算法,这是因为AEKF算法经过非线性估计后存在一定的误差,导致其滤波精度稍差一点。
结合图4及图5,我们提出的鲁棒扩展卡尔曼滤波算法(REKF)处理带有延时及乘性噪声的非线性系统的效果要明显优于RKF滤波算法及AEKF滤波算法,这是因为我们建立了带有延时的误差模型来表示这种情况,而AEKF滤波算法并不适用带有延时的系统,RKF滤波算法只能满足乘性噪声项所带来的干扰,但没有考虑延时问题。所以,当出现延时时,RKF算法并不能保证系统精度,甚至会带来滤波发散。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (8)
1.一种小视场星敏感器量测延时滤波方法,其特征在于,包括以下步骤:
(1)采集飞行器运动过程中陀螺与星敏感器的输出数据;
(2)建立量测延时下基于姿态四元数的飞行器非线性状态空间模型,包括建立飞行器姿态估计系统的状态方程及量测延时下飞行器姿态估计系统的量测方程;
(3)针对上述量测延时下的飞行器状态空间模型,已知k时刻的状态估计值和方差的上界,以扩展卡尔曼滤波为结构框架,利用鲁棒扩展卡尔曼滤波设计,进行时间更新,求得一步状态预测和预测方差的上界;
(4)利用上述求得的一步状态预测和预测方差的上界,进行鲁棒扩展卡尔曼滤波量测更新,求得最优的滤波增益,进而求出k+1时刻的状态估计值和方差的上界,同时由于四元数归一化约束,将k+1时刻的状态估计中四元数部分进行强制的归一化约束;
(5)姿态估计系统的运行时间为M,若k=M,则输出姿态四元数及陀螺漂移的结果,完成姿态估计;若k<M,表示滤波过程未完成,则重复步骤(3)至步骤(4),直至姿态估计系统运行结束。
2.根据权利要求1所述的一种小视场星敏感器量测延时滤波方法,其特征在于,所述的步骤(2)具体包括以下步骤:
(2.1)建立飞行器姿态估计系统的状态方程:
将姿态四元数qk和陀螺漂移βk组成维数为n的状态变量根据飞行器的运动学方程,建立基于姿态四元数的飞行器状态方程为:
式中:wk满足均值为零,方差为的高斯白噪声;是乘性噪声项,s选取3,ηi,k表示均值为零,方差为1的噪声,Aik表示有适当阶数的确定矩阵,为:
式中:σv为陀螺测量噪声;
(2.2)建立量测延时下飞行器姿态估计系统的量测方程:
式中:zk为k时刻星敏感器量测值;rk为星敏感器的参考矢量;i为星敏感器观测到恒星的个数;vk为零均值的高斯白噪声,其方差为A(qk)为四元数qk的姿态描述矩阵,若A(qk)能表示为:
由于星敏感器进行姿态估计时,不可避免的存在量测延迟现象,上述建立的星敏感器量测模型没有考虑延迟的存在,使得输出结果并不准确,我们考虑一步延时的情况给出带有延迟的星敏感器测量模型:
yk=(Ι-Γk)zk+Γkzk
yk表示k时刻真实的量测量,Γk表示不同的延迟速率,满足Γk=diag{[μk,1 μk,2 ....μk,m]},其中μk,i∈R(i=1,2...m)服从伯努利分布(0~1分布),满足:
关于量测延时的非线性离散系统:
由于实际的量测输出yk与zk、zk-1两个时刻的量测量均相关,需要利用状态扩维理论,使其得到相同时刻量测量的表达式:
式中,是扩维后的状态变量,是扩维后的状态函数,表示扩维后的乘性噪声项,表示扩维后的高斯白噪声,表示扩维后的量测函数,表示扩维后的量测噪声,γk=[Ι-Γk Γk]表示扩维后的量测延时项,
其中,由Γk的分布特性可知,是一个零均值的随机矩阵序列;h(xk)满足:
σ1、σ2是标量。
3.根据权利要求2所述的一种小视场星敏感器量测延时滤波方法,其特征在于:所述的步骤(2)中陀螺测量噪声标准差为陀螺漂移噪声标准差为陀螺的采样周期为Δt=0.25s;星敏感器测量噪声标准差为σs=0.005deg,输出频率为1Hz;星敏感器的参考矢量设为r1=[1 0 0]Τ,r2=[0 1 0]Τ,r3=[0 0 1]Τ;初始陀螺漂移β0=[0.1 0.1 0.1]Tdeg/h;σij=10sec(i=1,2,3;j=x,y,z)。
4.根据权利要求1所述的一种小视场星敏感器量测延时滤波方法,其特征在于,所述的步骤(3)具体包括:
假设k时刻的状态估计值为方差上界为Pk,根据鲁棒扩展卡尔曼滤波,求得一步状态预测值为
一步状态预测误差为:
扩维后的状态函数f'(Xk)通过在点处泰勒展开,得到:
式中, 表示经过展开后的高阶项,通过引入未知时变矩阵βk∈Rn×n,满足具体的缩放矩阵Bk∈Rn×n,Lk表示已知的调节矩阵,故高阶项可表示为:
一步预测误差方差矩阵可表示为:
式中:
而ηik与wk是互相独立的高斯白噪声,均值为零;故:Ε[WkgT(Xk,ηk)]、 这几项都是零矩阵;
则一步预测方差可简化表示为:
根据二阶矩的概念可以得到:
假设存在正数λ和矩阵Lk,满足条件能推导出:
求得一步预测误差方差矩阵的上界:
5.根据权利要求4所述的一种小视场星敏感器量测延时滤波方法,其特征在于:所述的步骤(3)中初始状态估计值为x0=[0 0 0 1 0.1 0.1 0.1]T;忽略非线性函数线性化的高阶项,Bk,Dk+1设置为0;调节矩阵为了保证系统能达到的精度,参数λ和μ均设置为0.0001,参数ε设置为0.1;延时速率设为
6.根据权利要求1所述的一种小视场星敏感器量测延时滤波方法,其特征在于,所述的步骤(4)具体包括:
同EKF算法中量测更新步骤类似,对量测函数泰勒展开,保留高阶项得到:
式中, αk+1是未知的时变矩阵,Dk+1是考虑不确定系统模型误差的比例矩阵;
将上式代入一步状态预测值中,所以有:
将上式代到滤波误差协方差矩阵,表示为:
其中, 表示为:
同样根据二阶矩原理,假设存在正数ε2、ε3:
假设存在正数μ和矩阵Lk+1,满足条件能推出:
所以,k+1时刻的更新后状态误差的方差矩阵为:
利用最小方差理论,令求得最优滤波增益为:
最后利用求得的最优滤波增益求出k+1时刻的状态估计值和方差的上界Pk+1;
由于状态变量四元数部分满足四元数归一化约束||qk||=1,因此再将k+1时刻的状态估计值中的四元数部分进行归一化约束。
7.根据权利要求6所述的一种小视场星敏感器量测延时滤波方法,其特征在于:所述的步骤(4)中Cik=0。
8.根据权利要求1所述的一种小视场星敏感器量测延时滤波方法,其特征在于:所述的步骤(5)中,M=1000。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810519570.XA CN109000638A (zh) | 2018-05-28 | 2018-05-28 | 一种小视场星敏感器量测延时滤波方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810519570.XA CN109000638A (zh) | 2018-05-28 | 2018-05-28 | 一种小视场星敏感器量测延时滤波方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN109000638A true CN109000638A (zh) | 2018-12-14 |
Family
ID=64573208
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810519570.XA Pending CN109000638A (zh) | 2018-05-28 | 2018-05-28 | 一种小视场星敏感器量测延时滤波方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109000638A (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109724599A (zh) * | 2019-03-12 | 2019-05-07 | 哈尔滨工程大学 | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 |
CN112082574A (zh) * | 2020-09-04 | 2020-12-15 | 中国科学院微小卫星创新研究院 | 星敏感器的校正方法及系统 |
CN112560681A (zh) * | 2020-12-15 | 2021-03-26 | 哈尔滨工程大学 | 一种乘性噪声下动力定位船非线性状态估计方法 |
CN117214857A (zh) * | 2023-11-09 | 2023-12-12 | 中国人民解放军海军航空大学 | 一种三维场景下高斯多假设多扩展目标的跟踪方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103900574A (zh) * | 2014-04-04 | 2014-07-02 | 哈尔滨工程大学 | 一种基于迭代容积卡尔曼滤波姿态估计方法 |
CN103940448A (zh) * | 2014-04-11 | 2014-07-23 | 哈尔滨工程大学 | 一种船用罗经光纤陀螺噪声在线估计系统及估计方法 |
CN104020671A (zh) * | 2014-05-30 | 2014-09-03 | 哈尔滨工程大学 | 一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法 |
CN107590317A (zh) * | 2017-08-17 | 2018-01-16 | 河海大学 | 一种计及模型参数不确定性的发电机动态估计方法 |
US20180031387A1 (en) * | 2016-07-29 | 2018-02-01 | Carnegie Mellon University | State estimation for aerial vehicles using multi-sensor fusion |
-
2018
- 2018-05-28 CN CN201810519570.XA patent/CN109000638A/zh active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103900574A (zh) * | 2014-04-04 | 2014-07-02 | 哈尔滨工程大学 | 一种基于迭代容积卡尔曼滤波姿态估计方法 |
CN103940448A (zh) * | 2014-04-11 | 2014-07-23 | 哈尔滨工程大学 | 一种船用罗经光纤陀螺噪声在线估计系统及估计方法 |
CN104020671A (zh) * | 2014-05-30 | 2014-09-03 | 哈尔滨工程大学 | 一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法 |
US20180031387A1 (en) * | 2016-07-29 | 2018-02-01 | Carnegie Mellon University | State estimation for aerial vehicles using multi-sensor fusion |
CN107590317A (zh) * | 2017-08-17 | 2018-01-16 | 河海大学 | 一种计及模型参数不确定性的发电机动态估计方法 |
Non-Patent Citations (1)
Title |
---|
HUAMING QIAN, ETC.: "Robust extended Kalman filtering for nonlinear stochastic systems with random sensor delays, packet dropouts and correlated noises", 《AEROSPACE SCIENCE AND TECHNOLOGY》 * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109724599A (zh) * | 2019-03-12 | 2019-05-07 | 哈尔滨工程大学 | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 |
CN112082574A (zh) * | 2020-09-04 | 2020-12-15 | 中国科学院微小卫星创新研究院 | 星敏感器的校正方法及系统 |
CN112082574B (zh) * | 2020-09-04 | 2023-05-12 | 中国科学院微小卫星创新研究院 | 星敏感器的校正方法及系统 |
CN112560681A (zh) * | 2020-12-15 | 2021-03-26 | 哈尔滨工程大学 | 一种乘性噪声下动力定位船非线性状态估计方法 |
CN117214857A (zh) * | 2023-11-09 | 2023-12-12 | 中国人民解放军海军航空大学 | 一种三维场景下高斯多假设多扩展目标的跟踪方法 |
CN117214857B (zh) * | 2023-11-09 | 2024-02-02 | 中国人民解放军海军航空大学 | 一种三维场景下高斯多假设多扩展目标的跟踪方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110109470B (zh) | 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统 | |
CN109000638A (zh) | 一种小视场星敏感器量测延时滤波方法 | |
CN104567871B (zh) | 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法 | |
CN104121907B (zh) | 一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法 | |
CN105973238B (zh) | 一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法 | |
CN103940433B (zh) | 一种基于改进的自适应平方根ukf算法的卫星姿态确定方法 | |
Dang et al. | Cubature Kalman filter under minimum error entropy with fiducial points for INS/GPS integration | |
CN111798491A (zh) | 一种基于Elman神经网络的机动目标跟踪方法 | |
Teixeira et al. | Flight path reconstruction–A comparison of nonlinear Kalman filter and smoother algorithms | |
CN109597864A (zh) | 椭球边界卡尔曼滤波的即时定位与地图构建方法及系统 | |
CN108267731B (zh) | 无人机目标跟踪系统的构建方法及应用 | |
CN109900309B (zh) | 一种基于混合状态空间模型的传感器数据盲校正方法 | |
CN105447574B (zh) | 一种辅助截断粒子滤波方法、装置及目标跟踪方法及装置 | |
CN103776449B (zh) | 一种提高鲁棒性的动基座初始对准方法 | |
CN108344409B (zh) | 提高卫星姿态确定精度的方法 | |
CN103218482B (zh) | 一种动力学系统中不确定参数的估计方法 | |
CN110532621A (zh) | 一种飞行器气动参数在线辨识方法 | |
CN111649747A (zh) | 一种基于imu的自适应ekf姿态测量改进方法 | |
Chauhan et al. | Review of aerodynamic parameter estimation techniques | |
US12078489B2 (en) | Weight and reference quaternoin correction unscented quaternoin estimation method | |
CN104020671A (zh) | 一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法 | |
Kim et al. | Non-symmetric unscented transformation with application to in-flight alignment | |
CN104331087B (zh) | 一种鲁棒的水下传感器网络目标跟踪方法 | |
CN113449384A (zh) | 一种基于中心误差熵准则扩展卡尔曼滤波的姿态确定方法 | |
CN107886058B (zh) | 噪声相关的两阶段容积Kalman滤波估计方法及系统 |
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 | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20181214 |
|
RJ01 | Rejection of invention patent application after publication |