收稿日期:2009-12-06;修回日期:2010-03-10作者简介:程会艳(1984-),女,硕士研究生,主要研究方向:自主导航算法。
2010年8月宇航计测技术A u g .,2010第30卷 第4期J o u r n a l o f A s t r o n a u t i c M e t r o l o g y a n d M e a s u r e m e n tV o l .30,N o .4文章编号:1000-7202(2010)04-0014-05 中图分类号:V 557 文献标识码:A卫星自主导航系统存在常值偏差的滤波方法研究程会艳1,2 郝云彩1,2 乔国栋1,2(1.北京控制工程研究所,北京100190;2.空间智能控制技术国家级重点实验室,北京100190) 摘 要 由于敏感器常值偏差对导航精度影响较大,因此有必要对其进行标定。
考虑到传统常偏扩维算法计算量大,且可能出现数值病态的问题,提出了两步U K F 算法,该算法对状态和偏差实施分离估计,从而达到解耦目的。
不仅能够准确标定常值偏差,而且还可以提高导航精度。
近地卫星自主导航系统仿真结果验证了该算法的有效性。
关键词 卫星导航 常值偏差 卡尔曼滤波 分离估计F i l t e r A l g o r i t h m i n S a t e l l i t e A u t o n o m o u s N a v i g a t i o nS y s t e mw i t hC o n s t a n t B i a sC H E N GH u i -y a n 1,2 H A OY u n -c a i 1,2 Q I A OG u o -d o n g1,2(1.B e i j i n gI n s t i t u t e o f C o n t r o l E n g i n e e r i n g ,B e i j i n g 100190;2.N a t i o n a l L a b o r a t o r y o f S p a c e I n t e l l i g e n t C o n t r o l ,B e i j i n g 100190) A b s t r a c t A c c o r d i n g t o t h e e f f e c t o f t h e c o n s t a n t b i a s o n t h e n a v i g a t i o n p r e c i s i o n i s d i s t i n c t ,i t i sn e c e s s a r y t o e s t i m a t e .S i n c e t h ea m o u n t o f c o m p u t a t i o ni nt r a d i t i o n a l e x t e n d e dd i m e n s i o na l g o r i t h m i s g r e a t ,t h e t w o -s t a g e U n s c e n t e d K a l m a n F i l t e r (T U K F )i s p r o p o s e d ,w h i c h c o u l d e s t i m a t e t h e s t a t e v a r i -a n t s a n d b i a s r e s p e c t i v e l y .B y t h i s w a y ,t h e c o n s t a n t b i a s c a n b e e s t i m a t e d a c c u r a t e l y ,a n d t h e n a v i g a t i o n p r e c i s i o n c a n b e i m p r o v e d o b v i o u s l y .T h e s i m u l a t i o n r e s u l t s f o r l o w -e a r t h a u t o n o m o u s n a v i g a t i o n s y s t e m s h o wt h a t t h e p r o p o s e d a l g o r i t h mi s e f f e c t i v e . K e y w o r d s S a t e l l i t e n a v i g a t i o n C o n s t a n t b i a s K a l m a n f i l t e r S e p a r a t i o n e s t i m a t i o n1 引 言卫星自主导航是指卫星不依赖地面支持,而利用星上自备的测量设备实时确定自己的位置和速度。
自主导航技术是实现卫星自主运行的一个重要方面,是当今卫星控制技术的发展趋势,它对减轻地面测控负担,降低卫星运行成本,提高卫星生存能力等方面都有重要意义[1]。
卡尔曼滤波[1]是一种最小方差意义上的现代滤波方法,一般情况下用于线性系统的状态估计。
对于非线性系统,则需考虑使用扩展卡尔曼滤波[2](E K F )。
E K F 算法首先对非线性状态方程和测量方程线性化,然后按照线性系统滤波方法进行滤波,但其对非线性系统进行简单一阶线性化,容易带来较大高阶截断误差,影响导航精度。
无迹卡尔曼滤波[3](U K F )方法可直接用于非线性系统,不必计算雅克比矩阵,可得到更好的状态估计。
考虑到实际情况中大多数系统都存在常值或随机时变偏差,从而导致滤波性能下降,甚至引起滤波发散。
为此,对系统偏差进行估计和校正的滤波算法得到了快速发展,文献[4]将偏差增扩为状态,提出了状态联合滤波算法(A S K F),但该算法的计算量随着偏差向量维数的增高而增加,且在实现过程中可能会出现数值病态问题。
F r i e d l a n d[5]提出了用于线性系统具有定常偏差的两步卡尔曼滤波算法(T K F),该算法提出后,引起了人们极大重视。
H s i e h等人将两步滤波思想应用于标准卡尔曼滤波器,提出了两步卡尔曼滤波算法[6] (T K F)及最优多步卡尔曼滤波算法[7](O M S K F)。
此外,一些学者将T K F算法推广到非线性系统,S i n h a 和M a h a l a n a b i s对非线性系统具有定常输入偏差的滤波算法[8]进行研究,S h e r e v e等人针对非线性系统只有测量偏差的问题,提出了两阶次优滤波器[9]。
M e n d e l提出了一种新的两步扩展卡尔曼滤波[10] (T E K F)。
但这些算法都需要对非线性系统进行线性化,容易带来较大高阶截断误差,影响导航精度。
考虑到U K F算法可直接用于非线性系统,本文在此基础上提出了两步U K F算法,用于存在常值偏差的非线性系统状态与偏差的估计问题。
本文第二部分建立导航系统数学模型,第三部分针对导航系统测量存在常值偏差的问题,在文献[10]基础上提出两步U K F算法(t w o-s t a g e u n s c e n t e d k a l m a n f i l t e r,T U K F)。
第四部分利用该算法进行数学仿真,仿真结果验证了该算法的有效性。
2 建立导航系统的数学模型2.1 导航系统的状态方程在建立系统的状态方程时,仅考虑地球中心引力和J2项摄动,将其它摄动因素等效为高斯白噪声,在地心赤道惯性坐标系中建立轨道动力学方程x﹒(t)=f(x,t)+w(t)(1)式中:μ———地球引力常数;J2———二阶带谐项系数;R e———地球平均赤道半径;x=[r x,r y,r z,v x,v y, v z]T———状态变量;r=r2x+r2y+r2z———表示地心距;w(t)———表示与测量噪声v(t)无关的系统噪声,假定v(t)和w(t)均为高斯白噪声。
2.2 导航系统的观测方程选取地心方向单位矢量及视半径作为观测量,同时假定地心方向矢量和视半径都存在常值偏差,得到系统的观测方程为z=h(x,t)+D(t)b+v(t)(2) h(x,t)=r xrr yrr zr a r c s i n R e+h c o2rT式中:D(t)———四阶单位阵;b———常值偏差向量; r x,r y,r z———分别为地心方向矢量在惯性坐标系下分量;h c o2———地球大气层中波长为14μm～16μm 的二氧化碳吸收带的平均高度。
3 带有常值偏差滤波算法设计敏感器测量数据中一般包括随机误差和系统误差,随机误差通常可以通过滤波算法来消除,系统误差会造成观测模型的不准确,一般难以通过滤波方法来消除,导致系统导航精度比较低。
针对系统误差为常值的情况,本文考虑将常值偏差作为状态变量,利用滤波算法对其进行估计和校准。
3.1 U K F算法U K F算法利用一系列近似高斯分布的采样点,通过U n s c e n t e d变换来进行状态与误差协方差阵的递推和更新,因此不需要计算状态方程和测量方程的雅克比矩阵,克服了E K F的缺点,同时保留了K a l m a n滤波的递推形式,其实现也相对简单。
U K F算法的基础是U n s c e n t e d变换[11](U T),它用一组确定性的离散采样点即s i g m a点来近似状态变量的分布,逼近系统状态变量的均值和方差,具体的U T变换如图1所示。
U T变换根据先验知识选取一组s i g m a点集,确保它们的均值和方差为给定的均值和方差,将每个s i g m a点通过非线性变换得到一组经过变换的点集。
对变换后的点集进行统计,可得到非线性变换后的均值和方差估计。
3.2 两步U K F算法T U K F算法是在U K F算法的基础上改进得到·15· 第4期 卫星自主导航系统存在常值偏差的滤波方法研究图1 U T 变换原理图的,其分别对状态和偏差采用U K F 算法进行估计,然后利用估计的偏差对状态变量进行更新。
假定离散非线性系统为x k =f (x k -1,k -1)+w k -1z k =h (x k ,k )+D k b +vk(3)式中:x k ———n 维系统状态矢量;z k ———m 维观测矢量;b ———m 维常值偏差矢量;w k -1———系统噪声序列;v k ———量测噪声序列,并假设w k -1,v k 为零均值高斯白噪声;其协方差矩阵分别为:Q 和R 。
具体的滤波算法如下所示1)初始化 x 0=E [x 0],P 0=E [(x -x 0)(x -x 0)T] b 0=b 0,P b 0=E [(b -b 0)(b -b 0)T ] 2)假定偏差为零时的状态估计计算s i g m a 点 (χk -1)0=x k -1 (χk -1)i =x k -1+n +λ(P k -1)i ,i =1,…,n (χk -1)i =x k -1-n+λ(P k -1)i -n ,i =n +1, (2) 相应权值为ω0=λn+λ,ωi =λ2(n+λ),i =1,…,2n 状态预测 (χk k -1)i =f ((χk -1)i ),i =0,…,2n x k k -1=∑2ni =0ωi (χk k -1)i P f f =∑2ni =0ωi [(χk k -1)i -x k k -1][(χk k -1)i -x k k -1]T P k k -1=P f f +Q 输出预测 (Z k k -1)i =h ((χk k -1)i ),i =0,…,2n z k k -1=∑2ni =0ωi (Z k k -1)i P h h =∑2ni =0ωi [(Z k k -1)i -z k k -1][(Z k k -1)i -z k k -1]T P z k z k=P h h +R 状态与统计特性参数更新 P x k z k=∑2ni =0ωi [(χk k -1)i -x k k -1][(Z k k -1)i -z k k -1]T K k =P x k z k P -1z k z k x k =x k k -1+K k (z k -z k k -1) P k =P k k -1-K k P z k z k K Tk 3)偏差估计 b k k -1=b k -1,P b k k -1=P b k -1 P b x k z k=P b k k -1S T P b z k z k =S P b k k -1S T+P z k z k K bk =P bx k z k P bz k z k-1 P bk =P bk k -1-K b k P bz k z k(K b k )T b k =b k k -1+K b k (z k -z k k -1-S b k k -1) 4)更新状态估计 x k =x ～k +V k b k 关系矩阵 U k =Υk k -1V k -1,S k =H k U k +D k V k =U k -K k S k Υk k -1= f (x ) x x =x ～k -1,H k =h (x ) x x =x ～k -14 仿真结果与分析4.1 仿真条件为了模拟卫星在轨的真实运动情况,利用S T K 生成卫星轨道数据,考虑如下摄动因素:(1)地球引力;(2)地球非球形摄动考虑前10阶带谐项;(3)日、月摄动;(4)太阳光压和大气阻力。
