CN105300384A - 一种用于卫星姿态确定的交互式滤波方法 - Google Patents
一种用于卫星姿态确定的交互式滤波方法 Download PDFInfo
- Publication number
- CN105300384A CN105300384A CN201510159381.2A CN201510159381A CN105300384A CN 105300384 A CN105300384 A CN 105300384A CN 201510159381 A CN201510159381 A CN 201510159381A CN 105300384 A CN105300384 A CN 105300384A
- Authority
- CN
- China
- Prior art keywords
- moment
- represent
- matrix
- state
- estimation
- 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
Landscapes
- Medicines Containing Antibodies Or Antigens For Use As Internal Diagnostic Agents (AREA)
- Gyroscopes (AREA)
Abstract
本发明公开了一种用于卫星姿态确定的交互式滤波方法,弥补了optimal-REQUEST算法无法估计陀螺常值漂移的缺陷,提高了算法的适用性。包括以下几个步骤:步骤一:采集传感器量测数据,包括陀螺仪数据和星敏感器数据;步骤二:建立卫星姿态估计***的状态空间模型,包括构建姿态K矩阵;步骤三:针对上述状态空间模型,已知k时刻的状态估计和陀螺量测,利用CKF算法及k时刻的最优四元数估计k+1时刻的陀螺常值漂移,从而补偿陀螺量测;步骤四:针对上述补偿的陀螺量测,利用optimal-REQUEST算法进行时间更新和量测更新,得到k+1时刻的最优K矩阵,从而得到最优四元数等步骤。本发明有利于提高估计精度。
Description
技术领域:
本发明涉及一种用于卫星姿态确定的交互式滤波方法,属于利用最优K矩阵方法和非线性滤波技术进行卫星姿态估计的技术领域。
背景技术:
卫星姿态估计技术是航天技术的关键技术之一,由陀螺与星敏感器组成的卫星姿态估计***由于测姿精度高、可靠性好以及自主性强等优点得到了广泛的应用。K矩阵姿态估计方法是基于Wahba问题的四元数确定方法,具有计算效率高的优点。在QUEST算法的基础上,ItzhackY.Bar-Itzhack提出了利用陀螺量测更新K矩阵的REQUEST算法,进一步提高了QUEST算法的估计精度,但是REQUEST算法具有增益因子不可调节的缺点,无法根据实际的量测误差,自动调节增益因子,使得算法的适用性存在一定问题。在此基础上,D.Choukroun等于2004年提出了optimal-REQUEST算法,在经典Kalman滤波框架的基础下,利用最优增益因子提高了姿态估计精度。但是optimal-REQUEST算法存在不能估计陀螺常值漂移的缺点,而且最优增益因子无法补偿陀螺常值漂移带来的影响,使得算法存在发散现象。
为克服optimal-REQUEST算法无法估计陀螺常值漂移的缺陷,采用CKF算法与optimal-REQUEST算法相结合的方式实现陀螺常值漂移的估计,提高算法的适用性。由于陀螺常值漂移估计为非线性状态空间模型,常用的方法有EKF、UKF和CKF算法,但是EKF算法存在理论上的局限性:模型线性化引入了截断误差,导致滤波精度下降,同时要计算雅克比矩阵,计算复杂。UKF与CKF算法同是基于最优高斯滤波框架,相比于UKF,CKF采用三阶球面相径容积规则来近似非线性函数的均值和方差,可以保证在理论上以三阶多项式逼近任何非线性高斯状态的后验均值和方差,具有实现简单,收敛性好等优点。将optimal-REQUEST算法与CKF算法相结合的滤波方式,可以发挥各自的优势,optimal-REQUEST算法具有线性特征,估计最优四元数,可以避免四元数规范化带来的误差,而采用CKF算法估计陀螺常值漂移,也可以解决量测方程非线性化带来的估计精度差的缺点,使得算法的估计精度得到提高,提高了算法的适用性。
发明内容:
本发明的目的是为了提高姿态估计精度和增强***的适用性,提出了一种用于卫星姿态确定的optimal-REQUEST与CKF交互式滤波方法。发明采用optimal-REQUEST算法估计卫星姿态四元数,采用CKF算法估计陀螺常值漂移,并将optimal-REQUEST算法估计的最优四元数,作为CKF算法的输入,同时CKF算法估计的陀螺常值漂移作为optimal-REQUEST算法的更新补偿,实现两种算法的交互,达到***的最优性能。
本发明的技术方案具体如下:
本发明的用于一种用于卫星姿态确定的交互式滤波方法,包括以下几个步骤:
步骤1:读取传感器量测数据,所述传感器测量数据包括陀螺仪数据和星敏感器数据;
步骤2:建立卫星姿态估计***的状态空间模型;
所述的卫星姿态估计***状态空间模型包括:
(1)陀螺常值漂移估计状态空间模型
βk+1=βk+ηk
式中:βk表示k时刻陀螺常值漂移;βk+1为k+1时刻陀螺常值漂移;表示k时刻最优估计四元数;Δt表示采样时间;ηk为零均值高斯白噪声,方差为Φ(·)为四元数更新矩阵;ωk表示k时刻载体转速,[ωk×]表示向量叉乘矩阵;rk+1为k+1时刻向量观测器参考向量;bk+1为k+1时刻向量观测器量测向量;为陀螺仪量测值,量测噪声方差为δbk+1为零均值高斯白噪声,方差为A(·)表示姿态旋转矩阵。以上是单向量观测量测模型,对于一般星敏感器,在同一时间往往能够观测多颗有效星,其量测模型可以扩充为:
式中,zk+1,i表示第i个有效矢量观测;δbk+1,i为相应的随机噪声;N(k+1)表示k+1时刻的有效向量个数;xk表示组合之后的状态参量,此处表示陀螺常值漂移βk;vk表示组合之后的零均值高斯白噪声;h(·)表示非线性量测函数;
(2)基于optimal-REQUEST方法的姿态确定状态空间模型
Yk+1=Xk+1+Vk+1
式中:Xk表示状态K矩阵;Yk+1为k+1时刻量测矩阵;Vk+1表示k+1时刻量测噪声;bk+1,i表示k+1时刻第i个有效观测向量;rk+1,i表示k+1时刻第i个有效参考向量;tr(·)表示取矩阵的迹;ak+1,i表示k+1时刻第i个加权系数;Wk为***过程噪声矩阵;ε为陀螺量测随机噪声向量;
步骤3:针对上述状态空间模型,在已知时刻陀螺常值漂移的状态估计和估计方差的情况下,利用标准容积卡尔曼滤波进行陀螺常值漂移估计的时间更新和量测更新,得到陀螺常值漂移的一步状态预测方差、量测预测方差以及互协方差;
步骤3.1时间更新;
已知k时刻的状态估计为以及估计协方差Pk|k,求取容积点为:
式中,n为状态变量的维数;ξi为第i个容积点;ei表示n维单位列向量在第i个位置为1;Pk|k表示k时刻估计状态协方差矩阵;表示k时刻状态估计;αi,k|k表示传递容积点;
由于陀螺常值漂移状态方程是线性方程,故容积点经过状态传递之后仍为原值;一步状态预测估计与估计方差:
式中,表示一步预测状态;Pk+1|k表示一步预测状态协方差矩阵;Qk表示k时刻过程噪声方差阵;
步骤3.2量测更新;
计算更新容积点:
容积点经过量测非线性函数传递为:
γi,k+1|k=h(λi,k+1|k)
式中,h(·)的定义如前向量观测器非线性量测模型;λi,k+1|k为计算更新容积点;
量测一步预测、预测方差和互协方差为:
式中,表示k+1时刻量测一步预测;Pzz,k+1|k表示k+1时刻量测一步预测协方差矩阵;Pxz,k+1|k表示k+1时刻量测与状态一步预测互协方差矩阵;Rk+1表示k+1时刻量测噪声方差阵;
陀螺常值漂移估计状态空间滤波增益、k+1时刻的状态估计及估计方差如下式所示:
式中,表示k+1时刻容积卡尔曼滤波增益矩阵;表示k+1时刻陀螺常值漂移最优估计;表示k+1时刻向量观测器量测向量;Pk+1|k+1表示k+1时刻估计状态误差协方差矩阵;
步骤4:利用步骤3估计的陀螺常值漂移补偿陀螺量测,然后利用补偿后的陀螺量测构造四元数更新矩阵,在optimal-REQUEST算法更新过程中,修正由于陀螺常值漂移引起的Φk不准确问题,在利用optimal-REQUEST算法实现最优K矩阵的求取:
步骤4.1optimal-REQUEST时间更新;
已知k时刻的状态K矩阵估计为以及估计协方差得到一步状态预测估计和估计方差:
式中,Φk表示k时刻四元数更新矩阵;表示k时刻状态K矩阵一步预测;表示k时刻一步预测状态协方差矩阵;表示k时刻过程噪声方差阵;
步骤4.2optimal-REQUEST量测更新
计算最优增益及权值更新:
式中,表示k+1时刻最优增益;mk+1表示k+1时刻状态K矩阵归一化权值;δmk+1表示k+1时刻量测K矩阵归一化权值;
得到k+1时刻的状态估计和估计方差:
式中,Yk+1表示k+1时刻量测K矩阵;表示k+1时刻状态K矩阵最优估计;表示k+1时刻量测噪声方差阵;表示k+1时刻估计状态协方差矩阵;
过程噪声方差阵和量测噪声方差阵计算如下:
式中,与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计;表示陀螺量测噪声方差值;Δt表示***采样时间;
其中,表示向量观测随机噪声方差值;N(k+1)表示k+1时刻有效向量数;bk+1,i表示k+1时刻第i个观测向量;rk+1,i表示k+1时刻第i个参考向量;
步骤5:根据步骤4计算得到的最优状态量,计算最优四元数,利用乘性误差四元数,得出误差角的大小:
步骤5.1最优四元数的提取;
根据步骤3和步骤4交互式滤波得到最优K矩阵,根据Wahba问题可知:
通过上式,再根据四元数规范化约束,采用拉格朗日乘子,可以得到K矩阵最大特征值对应的特征向量就是最优四元数,计算方法如下式:
式中,与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计。
步骤5.2估计载体坐标系与真实载体坐标系旋转误差角计算;
根据乘性误差四元数,在评估估计姿态角与真实姿态角之间的误差时,可以采用估计载体坐标系与真实载体坐标系之间的旋转角来表示,计算如下:
式中:δqk|k表示k时刻旋转误差四元数;表示k时刻最优估计四元数的逆;表示四元数乘法;表示k时刻旋转误差四元数标量;δφk|k表示k时刻估计载体坐标系与真实载体坐标系之间的旋转误差角;qk表示k时刻真实旋转四元数;
步骤6:姿态估计非线性离散***的运行时间为M,若k=M,则输出姿态四元数及陀螺常值漂移结果,完成姿态估计,若k<M,表示滤波过程未完成,则重复上述步骤3至步骤5,直至姿态估计***运行结束。
进一步地,所述步骤2中,陀螺量测噪声标准差为陀螺常值漂移随机噪声标准差为输出频率为1Hz;星敏感器测量标准差为σs=5”,输出频率为1Hz;陀螺初始漂移β=[111]Tdeg/hr,静态情况真实角速率为w=[000]Trad/s,动态情况真实角速率为w=[-0.00230.0023-0.0023]Trad/s。
进一步地,所述步骤3和步骤4中,初始陀螺常值漂移设置为 初始方差设置为P0|0=diag([(2°/hr)2(2°/hr)2(2°/hr)2]);初始K矩阵设置为初始过程噪声方差为初始权值为m0=1。
进一步地,所述步骤6中,M=5400。
本发明的优点在于:
(1)本发明采用容积卡尔曼滤波对陀螺常值漂移进行估计,不需要对量测非线性方程线性化,避免了雅可比矩阵的计算,有利于提高估计精度。相比于无迹卡尔曼滤波,容积卡尔曼滤波也具有容积点少,计算简便的优点;
(2)本发明采用交互式滤波方法,使得两个滤波过程相互弥补,同时采用optimal-REQUEST与CKF对载体最优四元数和陀螺常值漂移进行估计,并利用最新的估计值带入算法进行下一次的估计,有利于算法快速收敛,提高各参数的估计精度;
(3)本发明采用多重量测构造量测方程,考虑一次观测多颗参考星的方式,更加符合真实情况,同时采用optimal-REQUEST算法估计最优四元数,避免了四元数范数约束带来的估计误差,有利于提高估计精度。
附图说明:
图1是算法流程图;
图2是不含陀螺常值漂移时,QUEST算法、增益为0.1的REQUEST算法、增益为0.01的REQUEST算法、增益为0.001的REQUEST算法及optimal-REQUEST算法姿态估计误差结果图;
图3是静态滤波过程中,不含陀螺漂移时,optimal-REQUEST算法自动调节增益曲线图;
图4是含有陀螺常值漂移时,QUEST算法、增益为0.1的REQUEST算法、增益为0.01的REQUEST算法、增益为0.001的REQUEST算法及optimal-REQUEST算法姿态估计误差结果图;
图5是动态滤波过程中,含陀螺漂移时,optimal-REQUEST算法自动调节增益曲线图;
图6是含有陀螺常值漂移时,optimal-REQUEST与CKF算法、optimal-REQUEST算法及陀螺常值漂移为0时optimal-REQUEST算法姿态估计误差结果图;
图7是CKF算法估计的陀螺常值漂移估计结果图。
具体实施方式:
下面结合附图和实施举例对本发明作进一步的详细说明:
本发明提出的一种用于卫星姿态确定的optimal-REQUEST与CKF交互式滤波方法是通过Matlab仿真软件进行仿真实验,与现有滤波算法的估计性能进行比较,如QUEST方法、变因子REQUEST方法及optimal-REQUEST方法。仿真硬件环境均为Intel(R)Core(TM)T9600CPU2.80GHz,4GRAM,Windows7操作***。如图2和图4所示,在不含有陀螺常值漂移和含有陀螺常值漂移时,几种滤波算法估计误差曲线图,从图中可知,QUEST算法由于不利用陀螺更新,故估计误差只与星敏感器量测误差有关,不受陀螺常值漂移影响,但是在不含陀螺漂移时,估计精度要低于采用陀螺更新的REQUEST与optimal-REQUEST算法。而在不同的增益因子的REQUEST算法与optimal-REQUEST算法对比中,optimal-REQUEST算法的收敛速度与收敛精度都是相对最优的,但是增加陀螺常值漂移之后,由于增益因子的调节作用,使得滤波估计开始发散,降低了估计精度;如图3和图5所示,从静态和动态两种方式对比了增益因子的变化,在静态时增益因子逐渐收敛,表示为估计精度高于量测精度,估计结果在最终结果中占有的权值较高;而动态时,由于载体姿态处于变化过程,增益因子也在不断调节,在匹配估计与量测之间达到算法的优化,得到最优的估计;从图6比较得到,采用CKF对陀螺常值估计,并补偿optimal-REQUEST算法更新方程之后,估计误差能够达到无陀螺常值漂移时的精度,使得算法的适用性得到提高;图7为CKF估计的陀螺常值漂移。
本发明是一种用于卫星姿态确定的optimal-REQUEST与CKF交互式滤波方法,流程如图1所示,包括以下几个步骤:
步骤1:读取传感器量测数据,所述传感器测量数据包括陀螺仪数据和星敏感器数据;
步骤2:建立卫星姿态估计***的状态空间模型;
所述的卫星姿态估计***状态空间模型包括:
(1)陀螺常值漂移估计状态空间模型
βk+1=βk+ηk
式中:βk表示k时刻陀螺常值漂移;βk+1为k+1时刻陀螺常值漂移;表示k时刻最优估计四元数;Δt表示采样时间;ηk为零均值高斯白噪声,方差为Φ(·)为四元数更新矩阵;ωk表示k时刻载体转速,[ωk×]表示向量叉乘矩阵;rk+1为k+1时刻向量观测器参考向量;bk+1为k+1时刻向量观测器量测向量;为陀螺仪量测值,量测噪声方差为δbk+1为零均值高斯白噪声,方差为A(·)表示姿态旋转矩阵。以上是单向量观测量测模型,对于一般星敏感器,在同一时间往往能够观测多颗有效星,其量测模型可以扩充为:
式中,zk+1,i表示第i个有效矢量观测;δbk+1,i为相应的随机噪声;N(k+1)表示k+1时刻的有效向量个数;xk表示组合之后的状态参量,此处表示陀螺常值漂移βk;vk表示组合之后的零均值高斯白噪声;h(·)表示非线性量测函数;
(2)基于optimal-REQUEST方法的姿态确定状态空间模型
Yk+1=Xk+1+Vk+1
式中:Xk表示状态K矩阵;Yk+1为k+1时刻量测矩阵;Vk+1表示k+1时刻量测噪声;bk+1,i表示k+1时刻第i个有效观测向量;rk+1,i表示k+1时刻第i个有效参考向量;tr(·)表示取矩阵的迹;ak+1,i表示k+1时刻第i个加权系数;Wk为***过程噪声矩阵;ε为陀螺量测随机噪声向量;
步骤3:针对上述状态空间模型,在已知时刻陀螺常值漂移的状态估计和估计方差的情况下,利用标准容积卡尔曼滤波进行陀螺常值漂移估计的时间更新和量测更新,得到陀螺常值漂移的一步状态预测方差、量测预测方差以及互协方差;
步骤3.1时间更新;
已知k时刻的状态估计为以及估计协方差Pk|k,求取容积点为:
式中,n为状态变量的维数;ξi为第i个容积点;ei表示n维单位列向量在第i个位置为1;Pk|k表示k时刻估计状态协方差矩阵;表示k时刻状态估计;αi,k|k表示传递容积点;
由于陀螺常值漂移状态方程是线性方程,故容积点经过状态传递之后仍为原值;一步状态预测估计与估计方差:
式中,表示一步预测状态;Pk+1|k表示一步预测状态协方差矩阵;Qk表示k时刻过程噪声方差阵;
步骤3.2量测更新;
计算更新容积点:
容积点经过量测非线性函数传递为:
γi,k+1|k=h(λi,k+1|k)
式中,h(·)的定义如前向量观测器非线性量测模型;λi,k+1|k为计算更新容积点;
量测一步预测、预测方差和互协方差为:
式中,表示k+1时刻量测一步预测;Pzz,k+1|k表示k+1时刻量测一步预测协方差矩阵;Pxz,k+1|k表示k+1时刻量测与状态一步预测互协方差矩阵;Rk+1表示k+1时刻量测噪声方差阵;
陀螺常值漂移估计状态空间滤波增益、k+1时刻的状态估计及估计方差如下式所示:
式中,表示k+1时刻容积卡尔曼滤波增益矩阵;表示k+1时刻陀螺常值漂移最优估计;表示k+1时刻向量观测器量测向量;Pk+1|k+1表示k+1时刻估计状态误差协方差矩阵;
步骤4:利用步骤3估计的陀螺常值漂移补偿陀螺量测,然后利用补偿后的陀螺量测构造四元数更新矩阵,在optimal-REQUEST算法更新过程中,修正由于陀螺常值漂移引起的Φk不准确问题,在利用optimal-REQUEST算法实现最优K矩阵的求取:
步骤4.1optimal-REQUEST时间更新;
已知k时刻的状态K矩阵估计为以及估计协方差得到一步状态预测估计和估计方差:
式中,Φk表示k时刻四元数更新矩阵;表示k时刻状态K矩阵一步预测;表示k时刻一步预测状态协方差矩阵;表示k时刻过程噪声方差阵;
步骤4.2optimal-REQUEST量测更新
计算最优增益及权值更新:
式中,表示k+1时刻最优增益;mk+1表示k+1时刻状态K矩阵归一化权值;δmk+1表示k+1时刻量测K矩阵归一化权值;
得到k+1时刻的状态估计和估计方差:
式中,Yk+1表示k+1时刻量测K矩阵;表示k+1时刻状态K矩阵最优估计;表示k+1时刻量测噪声方差阵;表示k+1时刻估计状态协方差矩阵;
过程噪声方差阵和量测噪声方差阵计算如下:
式中,与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计;表示陀螺量测噪声方差值;Δt表示***采样时间;
其中,表示向量观测随机噪声方差值;N(k+1)表示k+1时刻有效向量数;bk+1,i表示k+1时刻第i个观测向量;rk+1,i表示k+1时刻第i个参考向量;
步骤5:根据步骤4计算得到的最优状态量,计算最优四元数,利用乘性误差四元数,得出误差角的大小:
步骤5.1最优四元数的提取;
根据步骤3和步骤4交互式滤波得到最优K矩阵,根据Wahba问题可知:
通过上式,再根据四元数规范化约束,采用拉格朗日乘子,可以得到K矩阵最大特征值对应的特征向量就是最优四元数,计算方法如下式:
式中,与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计。
步骤5.2估计载体坐标系与真实载体坐标系旋转误差角计算;
根据乘性误差四元数,在评估估计姿态角与真实姿态角之间的误差时,可以采用估计载体坐标系与真实载体坐标系之间的旋转角来表示,计算如下:
式中:δqk|k表示k时刻旋转误差四元数;表示k时刻最优估计四元数的逆;表示四元数乘法;表示k时刻旋转误差四元数标量;δφk|k表示k时刻估计载体坐标系与真实载体坐标系之间的旋转误差角;qk表示k时刻真实旋转四元数。
对本发明的有益效果说明如下:
MATLAB仿真实验,在以下的仿真条件下,对该方法进行仿真实验:
陀螺量测噪声标准差为陀螺常值漂移随机噪声标准差为输出频率为1Hz;星敏感器测量标准差为σs=5”,输出频率为1Hz;陀螺初始漂移β=[111]Tdeg/hr;静态情况真实角速率为w=[000]Trad/s,动态情况真实角速率为w=[-0.00230.0023-0.0023]Trad/s;初始陀螺常值漂移设置为初始方差设置为P0|0=diag([(2°/hr)2(2°/hr)2(2°/hr)2]);初始K矩阵设置为初始过程噪声方差为初始权值为m0=1,步骤六中,M=5400。
Claims (4)
1.一种用于卫星姿态确定的交互式滤波方法,其特征在于,包括以下几个步骤:
步骤1:读取传感器量测数据,所述传感器测量数据包括陀螺仪数据和星敏感器数据;
步骤2:建立卫星姿态估计***的状态空间模型;
所述的卫星姿态估计***状态空间模型包括:
(1)陀螺常值漂移估计状态空间模型
βk+1=βk+ηk
式中:βk表示k时刻陀螺常值漂移;βk+1为k+1时刻陀螺常值漂移;表示k时刻最优估计四元数;Δt表示采样时间;ηk为零均值高斯白噪声,方差为Φ(·)为四元数更新矩阵;ωk表示k时刻载体转速,[ωk×]表示向量叉乘矩阵;rk+1为k+1时刻向量观测器参考向量;bk+1为k+1时刻向量观测器量测向量;为陀螺仪量测值,量测噪声方差为δbk+1为零均值高斯白噪声,方差为A(·)表示姿态旋转矩阵。以上是单向量观测量测模型,对于一般星敏感器,在同一时间往往能够观测多颗有效星,其量测模型可以扩充为:
式中,zk+1,i表示第i个有效矢量观测;δbk+1,i为相应的随机噪声;N(k+1)表示k+1时刻的有效向量个数;xk表示组合之后的状态参量,此处表示陀螺常值漂移βk;vk表示组合之后的零均值高斯白噪声;h(·)表示非线性量测函数;
(2)基于optimal-REQUEST方法的姿态确定状态空间模型
Yk+1=Xk+1+Vk+1
式中:Xk表示状态K矩阵;Yk+1为k+1时刻量测矩阵;Vk+1表示k+1时刻量测噪声;bk+1,i表示k+1时刻第i个有效观测向量;rk+1,i表示k+1时刻第i个有效参考向量;tr(·)表示取矩阵的迹;ak+1,i表示k+1时刻第i个加权系数;Wk为***过程噪声矩阵;ε为陀螺量测随机噪声向量;
步骤3:针对上述状态空间模型,在已知时刻陀螺常值漂移的状态估计和估计方差的情况下,利用标准容积卡尔曼滤波进行陀螺常值漂移估计的时间更新和量测更新,得到陀螺常值漂移的一步状态预测方差、量测预测方差以及互协方差;
步骤3.1时间更新;
已知k时刻的状态估计为以及估计协方差Pk|k,求取容积点为:
式中,n为状态变量的维数;ξi为第i个容积点;ei表示n维单位列向量在第i个位置为1;Pk|k表示k时刻估计状态协方差矩阵;表示k时刻状态估计;αi,k|k表示传递容积点;
由于陀螺常值漂移状态方程是线性方程,故容积点经过状态传递之后仍为原值;一步状态预测估计与估计方差:
式中,表示一步预测状态;Pk+1|k表示一步预测状态协方差矩阵;Qk表示k时刻过程噪声方差阵;
步骤3.2量测更新;
计算更新容积点:
容积点经过量测非线性函数传递为:
γi,k+1|k=h(λi,k+1|k)
式中,h(·)的定义如前向量观测器非线性量测模型;λi,k+1|k为计算更新容积点;
量测一步预测、预测方差和互协方差为:
式中,表示k+1时刻量测一步预测;Pzz,k+1|k表示k+1时刻量测一步预测协方差矩阵;Pxz,k+1|k表示k+1时刻量测与状态一步预测互协方差矩阵;Rk+1表示k+1时刻量测噪声方差阵;
陀螺常值漂移估计状态空间滤波增益、k+1时刻的状态估计及估计方差如下式所示:
式中,表示k+1时刻容积卡尔曼滤波增益矩阵;表示k+1时刻陀螺常值漂移最优估计;表示k+1时刻向量观测器量测向量;Pk+1|k+1表示k+1时刻估计状态误差协方差矩阵;
步骤4:利用步骤3估计的陀螺常值漂移补偿陀螺量测,然后利用补偿后的陀螺量测构造四元数更新矩阵,在optimal-REQUEST算法更新过程中,修正由于陀螺常值漂移引起的Φk不准确问题,在利用optimal-REQUEST算法实现最优K矩阵的求取:
步骤4.1optimal-REQUEST时间更新;
已知k时刻的状态K矩阵估计为以及估计协方差得到一步状态预测估计和估计方差:
式中,Φk表示k时刻四元数更新矩阵;表示k时刻状态K矩阵一步预测;表示k时刻一步预测状态协方差矩阵;表示k时刻过程噪声方差阵;
步骤4.2optimal-REQUEST量测更新
计算最优增益及权值更新:
式中,表示k+1时刻最优增益;mk+1表示k+1时刻状态K矩阵归一化权值;δmk+1表示k+1时刻量测K矩阵归一化权值;
得到k+1时刻的状态估计和估计方差:
式中,Yk+1表示k+1时刻量测K矩阵;表示k+1时刻状态K矩阵最优估计;表示k+1时刻量测噪声方差阵;表示k+1时刻估计状态协方差矩阵;
过程噪声方差阵和量测噪声方差阵计算如下:
式中,与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计;表示陀螺量测噪声方差值;Δt表示***采样时间;
其中,表示向量观测随机噪声方差值;N(k+1)表示k+1时刻有效向量数;bk+1,i表示k+1时刻第i个观测向量;rk+1,i表示k+1时刻第i个参考向量;
步骤5:根据步骤4计算得到的最优状态量,计算最优四元数,利用乘性误差四元数,得出误差角的大小:
步骤5.1最优四元数的提取;
根据步骤3和步骤4交互式滤波得到最优K矩阵,根据Wahba问题可知:
通过上式,再根据四元数规范化约束,采用拉格朗日乘子,可以得到K矩阵最大特征值对应的特征向量就是最优四元数,计算方法如下式:
式中,与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计。
步骤5.2估计载体坐标系与真实载体坐标系旋转误差角计算;
根据乘性误差四元数,在评估估计姿态角与真实姿态角之间的误差时,可以采用估计载体坐标系与真实载体坐标系之间的旋转角来表示,计算如下:
式中:δqk|k表示k时刻旋转误差四元数;表示k时刻最优估计四元数的逆;表示四元数乘法;δlk|k表示k时刻旋转误差四元数标量;δφk|k表示k时刻估计载体坐标系与真实载体坐标系之间的旋转误差角;qk表示k时刻真实旋转四元数;
步骤6:姿态估计非线性离散***的运行时间为M,若k=M,则输出姿态四元数及陀螺常值漂移结果,完成姿态估计,若k<M,表示滤波过程未完成,则重复上述步骤3至步骤5,直至姿态估计***运行结束。
2.根据权利要求1所述的一种用于卫星姿态确定的交互式滤波方法,其特征在于,步骤2中,陀螺量测噪声标准差为陀螺常值漂移随机噪声标准差为输出频率为1Hz;星敏感器测量标准差为σs=5”,输出频率为1Hz;陀螺初始漂移β=[111]Tdeg/hr,静态情况真实角速率为w=[000]Trad/s,动态情况真实角速率为w=[-0.00230.0023-0.0023]Trad/s。
3.根据权利要求1所述的一种用于卫星姿态确定的交互式滤波方法,其特征在于,步骤3和步骤4中,初始陀螺常值漂移设置为 初始方差设置为P0|0=diag([(2°/hr)2(2°/hr)2(2°/hr)2]);初始K矩阵设置为初始过程噪声方差为初始权值为m0=1。
4.根据权利要求1所述的一种用于卫星姿态确定的交互式滤波方法,其特征在于,步骤6中,M=5400。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510159381.2A CN105300384B (zh) | 2015-04-03 | 2015-04-03 | 一种用于卫星姿态确定的交互式滤波方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510159381.2A CN105300384B (zh) | 2015-04-03 | 2015-04-03 | 一种用于卫星姿态确定的交互式滤波方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105300384A true CN105300384A (zh) | 2016-02-03 |
CN105300384B CN105300384B (zh) | 2017-12-22 |
Family
ID=55197921
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510159381.2A Active CN105300384B (zh) | 2015-04-03 | 2015-04-03 | 一种用于卫星姿态确定的交互式滤波方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105300384B (zh) |
Cited By (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106767837A (zh) * | 2017-02-23 | 2017-05-31 | 哈尔滨工业大学 | 基于容积四元数估计的航天器姿态估计方法 |
CN107014376A (zh) * | 2017-03-01 | 2017-08-04 | 华南农业大学 | 一种适用于农业机械精准作业的姿态倾角估计方法 |
CN107228674A (zh) * | 2017-06-06 | 2017-10-03 | 上海航天控制技术研究所 | 一种针对星敏感器和陀螺联合滤波的改进方法 |
CN107607977A (zh) * | 2017-08-22 | 2018-01-19 | 哈尔滨工程大学 | 一种基于最小偏度单形采样的自适应ukf组合导航方法 |
CN107702710A (zh) * | 2017-08-17 | 2018-02-16 | 上海航天控制技术研究所 | 一种多陀螺表头常值漂移实时估计方法 |
CN108801270A (zh) * | 2018-06-08 | 2018-11-13 | 北京控制工程研究所 | 一种航天器多级复合控制的超高精度姿态确定方法 |
CN108827291A (zh) * | 2018-06-25 | 2018-11-16 | 北京羲朗科技有限公司 | 运动载体下的mems陀螺仪输出的零偏补偿方法及装置 |
CN109470266A (zh) * | 2018-11-02 | 2019-03-15 | 佛山科学技术学院 | 一种处理乘性噪声的星敏感器陀螺组合定姿方法 |
CN109470267A (zh) * | 2018-11-02 | 2019-03-15 | 佛山科学技术学院 | 一种卫星姿态滤波方法 |
CN109489689A (zh) * | 2018-11-20 | 2019-03-19 | 上海航天控制技术研究所 | 一种基于α-β滤波的星矢量测量误差在轨估计方法 |
CN110109470A (zh) * | 2019-04-09 | 2019-08-09 | 西安电子科技大学 | 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制*** |
CN114383614A (zh) * | 2022-01-20 | 2022-04-22 | 东南大学 | 一种弹道环境下的多矢量空中对准方法 |
CN114579934A (zh) * | 2022-05-07 | 2022-06-03 | 山东石油化工学院 | 单向量航姿信息提取方法 |
WO2024012602A1 (zh) * | 2022-11-15 | 2024-01-18 | 浙大城市学院 | 一种权值及参考四元数修正的无迹四元数姿态估计方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1987354A (zh) * | 2006-12-14 | 2007-06-27 | 北京航空航天大学 | 一种微纳航天器用微小型、低功耗惯性恒星罗盘 |
CN101078936A (zh) * | 2007-06-08 | 2007-11-28 | 北京航空航天大学 | 一种基于遗传最优request和gupf的高精度组合定姿方法 |
CN102752092A (zh) * | 2012-07-23 | 2012-10-24 | 东南大学 | 基于虚拟混合自动请求重传的卫星链路自适应传输方法 |
-
2015
- 2015-04-03 CN CN201510159381.2A patent/CN105300384B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1987354A (zh) * | 2006-12-14 | 2007-06-27 | 北京航空航天大学 | 一种微纳航天器用微小型、低功耗惯性恒星罗盘 |
CN101078936A (zh) * | 2007-06-08 | 2007-11-28 | 北京航空航天大学 | 一种基于遗传最优request和gupf的高精度组合定姿方法 |
CN102752092A (zh) * | 2012-07-23 | 2012-10-24 | 东南大学 | 基于虚拟混合自动请求重传的卫星链路自适应传输方法 |
Non-Patent Citations (3)
Title |
---|
于延波等: "基于矢量观测的航天器分段信息融合定姿方法", 《北京航空航天大学学报》 * |
宋丽君: "基于MEMS器件的航向姿态测量***的研究", 《中国优秀博硕士学位论文全文数据库(硕士)工程科技II辑》 * |
钱山: "在轨服务航天器相对测量及姿态控制研究", 《中国博士学位论文全文数据库工程科技II辑》 * |
Cited By (24)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106767837A (zh) * | 2017-02-23 | 2017-05-31 | 哈尔滨工业大学 | 基于容积四元数估计的航天器姿态估计方法 |
CN106767837B (zh) * | 2017-02-23 | 2019-10-22 | 哈尔滨工业大学 | 基于容积四元数估计的航天器姿态估计方法 |
CN107014376B (zh) * | 2017-03-01 | 2019-09-10 | 华南农业大学 | 一种适用于农业机械精准作业的姿态倾角估计方法 |
CN107014376A (zh) * | 2017-03-01 | 2017-08-04 | 华南农业大学 | 一种适用于农业机械精准作业的姿态倾角估计方法 |
CN107228674B (zh) * | 2017-06-06 | 2021-01-29 | 上海航天控制技术研究所 | 一种针对星敏感器和陀螺联合滤波的改进方法 |
CN107228674A (zh) * | 2017-06-06 | 2017-10-03 | 上海航天控制技术研究所 | 一种针对星敏感器和陀螺联合滤波的改进方法 |
CN107702710A (zh) * | 2017-08-17 | 2018-02-16 | 上海航天控制技术研究所 | 一种多陀螺表头常值漂移实时估计方法 |
CN107702710B (zh) * | 2017-08-17 | 2020-10-02 | 上海航天控制技术研究所 | 一种多陀螺表头常值漂移实时估计方法 |
CN107607977B (zh) * | 2017-08-22 | 2020-12-08 | 哈尔滨工程大学 | 一种基于最小偏度单形采样的自适应ukf组合导航方法 |
CN107607977A (zh) * | 2017-08-22 | 2018-01-19 | 哈尔滨工程大学 | 一种基于最小偏度单形采样的自适应ukf组合导航方法 |
CN108801270B (zh) * | 2018-06-08 | 2020-06-09 | 北京控制工程研究所 | 一种航天器多级复合控制的超高精度姿态确定方法 |
CN108801270A (zh) * | 2018-06-08 | 2018-11-13 | 北京控制工程研究所 | 一种航天器多级复合控制的超高精度姿态确定方法 |
CN108827291B (zh) * | 2018-06-25 | 2020-06-23 | 北京羲朗科技有限公司 | 运动载体下的mems陀螺仪输出的零偏补偿方法及装置 |
CN108827291A (zh) * | 2018-06-25 | 2018-11-16 | 北京羲朗科技有限公司 | 运动载体下的mems陀螺仪输出的零偏补偿方法及装置 |
CN109470266A (zh) * | 2018-11-02 | 2019-03-15 | 佛山科学技术学院 | 一种处理乘性噪声的星敏感器陀螺组合定姿方法 |
CN109470267A (zh) * | 2018-11-02 | 2019-03-15 | 佛山科学技术学院 | 一种卫星姿态滤波方法 |
CN109489689B (zh) * | 2018-11-20 | 2020-11-03 | 上海航天控制技术研究所 | 一种基于α-β滤波的星矢量测量误差在轨估计方法 |
CN109489689A (zh) * | 2018-11-20 | 2019-03-19 | 上海航天控制技术研究所 | 一种基于α-β滤波的星矢量测量误差在轨估计方法 |
CN110109470A (zh) * | 2019-04-09 | 2019-08-09 | 西安电子科技大学 | 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制*** |
CN110109470B (zh) * | 2019-04-09 | 2021-10-29 | 西安电子科技大学 | 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制*** |
CN114383614A (zh) * | 2022-01-20 | 2022-04-22 | 东南大学 | 一种弹道环境下的多矢量空中对准方法 |
CN114383614B (zh) * | 2022-01-20 | 2023-12-05 | 东南大学 | 一种弹道环境下的多矢量空中对准方法 |
CN114579934A (zh) * | 2022-05-07 | 2022-06-03 | 山东石油化工学院 | 单向量航姿信息提取方法 |
WO2024012602A1 (zh) * | 2022-11-15 | 2024-01-18 | 浙大城市学院 | 一种权值及参考四元数修正的无迹四元数姿态估计方法 |
Also Published As
Publication number | Publication date |
---|---|
CN105300384B (zh) | 2017-12-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105300384A (zh) | 一种用于卫星姿态确定的交互式滤波方法 | |
CN106291645B (zh) | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 | |
CN104567871B (zh) | 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法 | |
CN105486312B (zh) | 一种星敏感器与高频角位移传感器组合定姿方法及*** | |
RU2701194C2 (ru) | Способ оценки навигационного состояния в условиях ограниченной возможности наблюдения | |
CN110398245B (zh) | 基于脚戴式惯性测量单元的室内行人导航姿态估计方法 | |
CN100559125C (zh) | 一种基于Euler-q算法和DD2滤波的航天器姿态确定方法 | |
CN109000642A (zh) | 一种改进的强跟踪容积卡尔曼滤波组合导航方法 | |
CN106772524B (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
CN105136145A (zh) | 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法 | |
CN103630137A (zh) | 一种用于导航***的姿态及航向角的校正方法 | |
CN110567455B (zh) | 一种求积更新容积卡尔曼滤波的紧组合导航方法 | |
CN101982732A (zh) | 一种基于esoqpf和ukf主从滤波的微小卫星姿态确定方法 | |
CN112325886B (zh) | 一种基于重力梯度仪和陀螺仪组合的航天器自主定姿*** | |
Li et al. | A calibration method of DVL in integrated navigation system based on particle swarm optimization | |
CN105066996B (zh) | 自适应矩阵卡尔曼滤波姿态估计方法 | |
CN107389069B (zh) | 基于双向卡尔曼滤波的地面姿态处理方法 | |
CN112146655A (zh) | 一种BeiDou/SINS紧组合导航***弹性模型设计方法 | |
CN104019817A (zh) | 一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法 | |
CN108917772A (zh) | 基于序列图像的非合作目标相对导航运动估计方法 | |
CN103776449A (zh) | 一种提高鲁棒性的动基座初始对准方法 | |
CN112798021A (zh) | 基于激光多普勒测速仪的惯导***行进间初始对准方法 | |
CN108508463B (zh) | 基于Fourier-Hermite正交多项式扩展椭球集员滤波方法 | |
US20240175685A1 (en) | Weight and reference quaternoin correction unscented quaternoin estimation method | |
CN113587926A (zh) | 一种航天器空间自主交会对接相对导航方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |