基于非线性滤波和平滑相迭代的 GPS 定位估计算法

收稿日期：2008 年 4 月 22 日 基金项目：国家自然科学基金（40674002） 作者简介：曹意（1983—） ，男，广西人，硕士生，主要从事高精度单机 GPS 方面的研究，Email： twist@。茅旭初（联系人） ，男，副教授，电话（Tel.） ：021-34204424；Email： maoxc@
smoothing algorithm. UKF is employed to estimate user’s position at each epoch, the estimation is used to smooth the estimation of previous epoch, then the updated estimation can be taken as the initial value for kalman filter in turn, the filtering algorithm is applied again. This process proceeds with several iterations ending up with final estimation. Two smoothing algorithms are addressed, Fixed-Interval smoother and Fixed-Lag smoother, the former can be applied to nonlinear system, and the latter effectively reduces the computational burden when smoothing step increases. GPS experiment result reveals that the iterative algorithm amounts to a method contributing to accuracy improvement, it can be used not only in the post-processing, but also in real-time GPS positioning estimation.
xt [ X t
Xt
Yt
Yt
Zt
Zt
c tt
c tt
Rt 1t 2t
nt ]
（1）
根据接收机位置坐标的通用运动学模型和系统的状态关系可得系统的状态模型为：
T2 X t 1 X t T X t X t 2
X t 1 X t T X t
Yt 1 Yt T Yt
（7）
式中： n 为 xt 的维数； 2 ( L ) L 为一个比例因子； L 为状态矢量的维数； 通常 情况下取一个很小的正数； 通常取 0； ( L ) P t 为 (L )P t 平方根的第 i 列, Wi 为第 i
i
i
点的权重。 从上式可看出， t 是对 xt 呈高斯分布的模拟，因此均值和协方差仍为 xt 和 Pt ，通过非 线性函数 h( t ) 传递，便可得到 yt 的均值和方差。
i
方差与状态随机变量的均值 xt 和协方差 Pxx 一致,将这些点进行非线性变换后获得变换点的 均值 yt 和协方差 Pyy 。
t0 xt i i t xt ( L )Pt in i t xt ( L )Pt
W0 / (L ) Wi 1 / [ L 2 ( Wi n 1 / [ L 2 ( ) ]i )] 1 ,n2 ,
c t Rt
（5）
式中， Rt 为由时钟偏移引起的距离差。当接收到 n 颗（ n 3 ）卫星信号时，系统的测量 值为：
zt [ 1t , 2t ,
nt , D1t , D2t
Dnt ]T
（6）
2 UKF 算法
UKF 算法是基于 UT (Unscented Transform) 变换的最小方差估计， 由 Julier 等人在 1995 [4] 年首先提出 。其主要思想是逼近一种分布要比逼近任意的非线性函数更容易，因此对于非 线性的随机变量传递 yt h( xt , vt ) ，可以按式（7）选择一组采样点 t ,使其样本均值和协
关键词：全球定位系统；最优平滑；迭代算法；平淡卡尔曼滤波 中图分类号：TN976.1 文献标识码：A
A Nonlinear Iterative Filtering-Smoothing Algorithm for GPS Positioning
Cao Yi, Mao Xu-chu (Dept. of Instrument Science & Engineering, Shanghai Jiao Tong Univ., Shanghai 200240, China) Abstract: A new GPS positioning algorithm is proposed, which iteratively applies UKF and
（2）
式 t 为接收机的加速度和其他系统干扰，被视作 系统噪声； w1t 和 w2t 为时钟偏移模型的驱动噪声； w R 为 R 的噪声。由于每颗卫星的误差 是独立的，可用一阶高斯—马尔可夫过程对该误差进行非白噪声建模，表示为：
i (t 1) it wcit
（4）
, n)
由于接收机的位置和速度受多普勒频移影响， 所以将多普勒方程 Di [2]加到测量模型中：
Dit
( X t X it )( X t X it ) (Yt Yit )(Yt Yit ) ( Z t Z it )( Z t Z it ) ( X t X it ) 2 (Yt Yit ) 2 ( Z t Z it ) 2
滤波进行位置解算， 实验结果表明该模型虽然是一种有效的方法， 定位估计结果明显优于传 [3] 统的最小二乘估计 ，但由于雅克比矩阵仅是测量方程的泰勒一阶展开，所以定位精度低于 100 米（有 SA 干扰） 。随着对定位精度要求的提高和滤波理论的逐步完善，特别是 UKF 的 出现，该模型得到进一步改进，文献[2]建立了非线性的 GPS 定位模型，不需线性化测量方 程， UKF 通过确定性采样得到的采样点来表示系统的统计特性, 高斯随机变量通过非线性方 程直接传递,避免了系统模型线性化引入的误差和求解雅克比矩阵的繁琐计算过程，所以定 位精度得到进一步提高。 然而，无论是 EKF 还是 UKF，当前估计值均来自于前一时刻“历史记录”的递推和当 前观测量对递推值的修正。 作为当前时刻状态估计的起始值， 前一时刻状态估计的精确与否 直接影响到当前时刻的估计精度， 而且估计值对单一时间点的测量误差扰动非常敏感， 故要 进一步提高定位精度， 应通过提高每次滤波递推的起始值精度。 本文提出将后向卡尔曼平滑 算法融合到前向卡尔曼滤波过程中， 二者交替迭代， 通过后向平滑算法为前向滤波提供较高 精度的起始值。实验结果表明，滤波平滑相迭代的方法不仅提高了滤波初始值的精度，而且 后向平滑将“未来”的观测值信息融入当前估计值中，使得滤波曲线变得更加平缓，该方法 既在验后数据处理中体现出优越性，又能保证定位的实时性。
Key words: global positioning system (GPS); optimal smoother; iterative algorithm; unscented
Kalman filter (UKF) 在 GPS 定位中，应用扩展卡尔曼滤波[1]（Extended Kalman Filter, EKF）和平淡卡尔曼 滤波[2](Unscented Kalman Filter, UKF)对接收机的位置和速度进行估计被证明是较有效的手 段，文献[1]提出了通用的 GPS 定位卡尔曼滤波模型，由于 GPS 中的伪距方程是高度非线性 的， 因此该模型通过对伪距方程求解雅克比矩阵来获得线性的测量方程， 再应用离散卡尔曼
基于非线性滤波和平滑相迭代的 GPS 定位估计算法
曹意，茅旭初1 （上海交通大学 仪器科学与工程系，上海 200240） 摘要：提出了一种卡尔曼滤波与平滑相迭代的 GPS 定位估计算法，每一时刻点先用平淡卡
尔曼滤波进行位置估计， 然后利用滤波结果对前一时刻点的位置估计进行平滑修正， 接着再 用修正值进行滤波得到当前时刻点更新的估计值，通过若干次迭代，得到最终估计值。后向 平滑算法分别采用固定区间平滑和固定滞后平滑， 前者适用于非线性系统模型， 后者在平滑 步长增大时，其计算量较小。实验结果表明，迭代算法能够有效提高定位精度，不仅可以应 用在后验数据处理中，而且也可以应用于 GPS 实时定位估计。
it rt rit c t it vit
( X t X it ) 2 (Yt Yit ) 2 ( Z t Z it ) 2 c t it vit
式中： X t 、Yt 、Z t 为接收机在 t 时刻的位置坐标； X it 、Yit 、Z it 为 t 时刻第 i(i 1, 2, 颗卫星的位置坐标； rt 为接收机的位置矢量； vit 为通道 i 的测量白噪声。
Y t 1 Yt T Yt
T2 Yt 2
T2 Zt 1 Zt T Zt Zt 2
Z t 1 Z t T Z t
c tt 1 c tt Tc tt cw1t c t t 1 c tt cw2t
Rt 1 Rt w R
i
3 基于滤波与平滑的迭代算法的 GPS 定位估计 3.1 最优线性平滑
对于噪声呈高斯分布的线性系统，最优线性平滑是在已知 N 个观测量 y1 , y2
yN 的基
础上，对状态 xt 的最优线性估计，其中 1 t N ，根据平滑方式不同，可分为三种类型[5]： （1） 固定区间平滑：观测区间是固定的，即时刻 1 和时刻 N 是常数，所求状态向量的