CN102928858B - 基于改进扩展卡尔曼滤波的gnss单点动态定位方法 - Google Patents

Abstract

本发明涉及一种基于改进扩展卡尔曼滤波的GNSS单点动态定位方法，属于卫星导航技术领域。本方法将GNSS接收机的位置速度、钟差与钟漂作为位置参数设为状态向量X_k，通过状态转移方程，由前一历元的状态向量推算当前历元状态向量的预测值；并通过观测方程进一步获取状态向量预测值的修正量；将预测值和修正量加权，获取状态向量的估计值。在基于扩展卡曼滤波定位过程中，本方法通过延迟对状态向量误差协方差矩阵的更新，使得在初始取状态向量的情况下，滤波估计值快速收敛在真值附近，并达到很高的定位测速精度；不需要保存每一步的计算数据，占用计算机内存资源少；适用于GNSS单点动态定位。

Description

基于改进扩展卡尔曼滤波的GNSS单点动态定位方法

技术领域

本发明涉及一种基于改进扩展卡尔曼滤波的GNSS单点动态定位方法，属于卫星导航技术领域。

背景技术

GNSS单点动态定位由于只需要一台单频接收机，获取4颗或4颗以上可见卫星与接收机间的测码伪距和多普勒频移观测量，便可解算载体的位置、速度、钟差和钟漂等信息而广泛应用于各种车辆、船只的导航和监控、海上定位、野外勘测等领域。扩展卡尔曼滤波(EKF)是在GNSS单点动态定位中除最小二乘(LS)外最常用的解算方法。

卡尔曼滤波(KF)是卡尔曼于1960年提出的从与被提取信号有关的观测量中，通过算法估计出所需信号的一种滤波方法。这种方法将信号过程视为白噪声作用下的一个线性系统，利用高斯白噪声的统计特性，以系统的观测量为输入，以所需要的估计值（称为系统的状态向量）为输出，将输入和输出由时间更新和观测更新联系在一起，根据系统的状态转移方程和观测方程获取状态向量的最优估计值。

KF的原理是：将系统中需求解的所有参数设为一个状态向量；通过状态转移方程建立两个相邻历元的状态向量之间的关系，由前一历元的状态向量推算当前历元状态向量的预测值；通过观测方程建立当前历元状态向量与观测量之间的关系，从而获取一个状态向量预测值的修正量；将状态向量的预测值和修正量通过滤波增益加权，获得状态向量的最优滤波估计。

KF适用于线性系统，而大多数情况下，系统是非线性的。要使KF适用于非线性系统，需要将状态转移方程和观测方程进行线性化处理，若线性化处理的方法是泰勒级数展开法，则所对应的KF被称为扩展卡尔曼滤波(EKF)。

以线性的状态转移方程和非线性的观测方程所构成的系统为例：

状态转移方程：X_k=Φ_k|k-1X_k-1+Γ_k|k-1ω_k-1            [1]

观测方程：Z_k=f_k(X_k)+v_k                 [2]

由于f_k(·)是非线性的，需要对方程[2]进行线性化处理，在

处进行泰勒展开，并取其一阶近似，可得到方程[3]。

ΔZ_k=H_kΔX_k+v_k          [3]

其中，k表示观测历元数；X_k和X_k-1为第k个和第k-1个观测历元的状态向量；Φ_k|k-1是状态转移矩阵；Γ_k|k-1为噪声驱动矩阵；Z_k为第k个历元的观测量；f_k描述了第k个历元，Z_k和X_k之间的函数关系；ω_k-1和v_k分别为过程噪声和观测噪声，二者皆为高斯白噪声；

为X_k的预测值。

基于方程[1]和[3]的EKF步骤如下：

a)推算状态向量X_k的预测值

${\hat{X}}_{k|k-1}={\mathrm{\Φ}}_{k|k-1}{\hat{X}}_{k-1}$

其中，

是X_k-1的最优滤波估计；

b)计算

的协方差矩阵P_k|k-1：

$P_{k|k-1}={\mathrm{\Φ}}_{k|k-1}{P}_{k-1}{\mathrm{\Φ}}_{k|k-1}^T+Q_{k-1}$

其中，Q_k-1为ω_k-1的协方差矩阵；

c)计算滤波增益矩阵K_k:

$K_k=P_{k|k-1}{H}_k^T{[H_k{P}_{k|k-1}{H}_k^T+O_k]}^{-1}$

其中，O_k为v_k的协方差矩阵；

d)计算状态向量X_k的滤波估计值

${\hat{X}}_k={\hat{X}}_{k|k+1}+K_k\·\mathrm{\Δ}{Z}_k$

其中，即为第k个历元状态向量X_k的滤波解算结果；

e)计算X_k的误差协方差矩阵P_k：

P_k=[I-K_kH_k]P_k|k-1

f)令k-1＝k，转入步骤a)……

与基于LS的GNSS单点动态定位方法(LS方法)相比，基于EKF的GNSS单点动态定位方法可达到较高的定位测速精度，但是，这种方法存在一个问题：在不了解初始状态的统计特性的情况下，如果初始状态向量

及其协方差矩阵P₀取值不合适，滤波收敛极慢。

和P₀的常用取值方法有两种：1.取

为零向量，取P₀为对角阵，其对角线元素取大数量级的正数；2.取

为与真实的X₀相近的概略值，取P₀为对角阵，其对角线元素取大数量级的正数；2.取

为与真实的X₀相近的概略值。第一种方法操作简便，但会使滤波收敛速度变慢，延长首次定位时间；第二种方法可使滤波快速收敛，但是与真实的X₀接近到何种程度可使滤波快速收敛很难界定，且接收机的大概位置、速度信息不是在任何时间地点都可轻易获取。

发明内容

本发明目的是为解决GNSS单点动态定位时，EKF滤波不收敛或收敛很慢的问题，提出一种基于改进EKF的GNSS单点动态定位方法。

本发方法采用一种基于改进EKF的GNSS单点动态定位方法，采用状态向量误差协方差矩阵延迟更新的方法（DU-EKF），通过对状态向量误差协方差矩阵更新的延迟，使得在初始取状态向量

的情况下，滤波估计值快速收敛在真值附近，并达到很高的定位测速精度。

本发明的具体技术方案如下：

预设GNSS接收机处于ECEF坐标系下，其的状态向量为：

$X_k={[x_k^u,v_k^{\mathrm{ux}},a_k^{\mathrm{ux}},y_k^u,v_k^{\mathrm{uy}},a_k^{\mathrm{uy}},z_k^u,v_k^{\mathrm{uz}},a_k^{\mathrm{uz}},{\mathrm{c\Δt}}_k^u,\mathrm{c\Δ}{f}_k^u]}^T$

其中，k为观测历元数，

分别为ECEF坐标下GNSS接收机载体的三维位置、速度和加速度，

和

分别为接收机的钟差和钟漂。该状态向量包含了导航定位所需求解的全部信息。

则X_k的滤波估计 ${\hat{X}}_k={[{\hat{x}}_k^u,{\hat{v}}_k^{\mathrm{ux}},{\hat{a}}_k^{\mathrm{ux}},{\hat{y}}_k^u,{\hat{v}}_k^{\mathrm{uy}},{\hat{a}}_k^{\mathrm{uy}},{\hat{z}}_k^u,{\hat{v}}_k^{\mathrm{uz}},{\hat{a}}_k^{\mathrm{uz}},{\mathrm{c\Δ}\hat{t}}_k^u,\mathrm{c\Δ}{\hat{f}}_k^u]}^T.$

ECEF坐标系即地心地固坐标系，原点为地球质心，Z轴与地轴平行指向北极点，X轴指向本初子午线与赤道的交点，Y轴垂直于XOY平面(即东经90度与赤道的交点)。

步骤1：在观测历元数k＝1时，初始状态向量

初始状态向量误差协方差矩阵P₀=diag[1/ε_p 1/ε_v 1/ε_a 1/ε_p 1/ε_v 1/ε_a 1/ε_p 1/ε_v 1/ε_a 1/ε_t 1/ε_f]。

ε_p为载体距地心距离平方的倒数，ε_v为载体最大允许速度平方的倒数，ε_a为载体最大允许加速度的倒数，ε_t为接收机钟差与光速乘积平方的倒数，ε_f为接收机钟漂与光速乘积平方的倒数。在实际运用中，ε_p，ε_v，ε_a，ε_t和ε_f均为正数，取值在数量级上与接收机真实情况相近。

步骤2：计算状态向量X_k的预测值

${\hat{X}}_{k|k-1}={\mathrm{\Φ}}_{k|k-1}{\hat{X}}_{k-1}{[{\hat{x}}_{k|k-1}^u,{\hat{v}}_{k|k-1}^{\mathrm{ux}},{\hat{a}}_{k|k-1}^{\mathrm{ux}},{\hat{y}}_{k|k-1}^u,{\hat{v}}_{k|k-1}^{\mathrm{uy}},{\hat{a}}_{k|k-1}^{\mathrm{uy}},{\hat{z}}_{k|k-1}^u,{\hat{v}}_{k|k-1}^{\mathrm{uz}},{\hat{a}}_{k|k-1}^{\mathrm{uz}},{\mathrm{c\Δ}\hat{t}}_{k|k-1}^u,\mathrm{c\Δ}{\hat{f}}_{k|k-1}^u]}^T$

其中Φ_k|k-1为状态转移矩阵，来自接收机载体的动力学模型。

${\mathrm{\Φ}}_{k|k-1}=\left[\begin{array}{cccc}{\mathrm{\Φ}}_{k|k-1}^x& & & \\ & {\mathrm{\Φ}}_{k|k-1}^y& & \\ & & {\mathrm{\Φ}}_{k|k-1}^z& \\ & & & {\mathrm{\Φ}}_{k|k-1}^c\end{array}\right]---\left[4\right]$

分别是ECEF坐标系下，X向，Y向和Z向运动状态量的状态转移矩阵，依据动力学模型确定；

是有关钟差和钟漂的状态转移矩阵，依据时钟模型确定。

步骤3：计算预测值

的协方差矩阵P_k|k-1：

$P_{k|k-1}={\mathrm{\Φ}}_{k|k-1}{P}_{k-1}{\mathrm{\Φ}}_{k|k-1}^T+Q_{k-1}$

过程噪声协方差矩阵：

$Q_{k-1}=\left[\begin{array}{cccc}{2\mathrm{\α}}_x{\mathrm{\σ}}_x^2{Q}_{k-1}^x& & & \\ & {2\mathrm{\α}}_y{\mathrm{\σ}}_y^2{Q}_{k-1}^y& & \\ & & {2\mathrm{\α}}_z{\mathrm{\σ}}_z^2{Q}_{k-1}^z& \\ & & & Q_{k-1}^c\end{array}\right]---\left[5\right]$

和

分别是ECEF坐标系下，X,Y,Z三向运动状态量的过程噪声矩阵；α_y和α_z分别为X向，Y向和Z向的机动加速度频率；

分别是X,Y，Z三向的机动加速度方差。

步骤4：根据步骤3得到的P_k|k-1，计算滤波增益矩阵K_k：

$K_k=P_{k|k-1}{H}_k^T{[H_k{P}_{k|k-1}{H}_k^T+Q_k]}^{-1}$

(a)观测矩阵H_k由 $f_x\left(X_k\right)={\left[\begin{array}{cccccc}{R}_k^1& \·\·\·& R_k^n& D_k^1& \·\·\·& D_k^n\end{array}\right]}^T$ 线性化而来，

和

分别为第s颗可见星的伪距方程[6]和多普勒方程[7]，s=1,…,n，n为接收机观测到的参与定位计算的可见星总数。

$R_k^s=\sqrt{{(X_k^s-x_k^u)}^2+{(Y_k^s-x_k^u)}^2+{(Z_k^s-z_k^u)}^2}+c\·{\mathrm{\Δt}}_k^u---\left[6\right]$

$D_k^s=[(X_k^s-x_k^u)\·(V_k^{\mathrm{sx}}-v_k^{\mathrm{ux}})+(Y_k^s-y_k^u)\·(V_k^{\mathrm{sy}}-v_k^{\mathrm{uy}})+(Z_k^s-z_k^u)\·(V_k^{\mathrm{sz}}-v_k^{\mathrm{uz}})]/{\mathrm{\ρ}}_k^s+c{\·\mathrm{\Δf}}_k^u---\left[7\right]$

其中，

和

分别为ECEF坐标系下第s颗可见星在第k个历元的三维位置和速度，

是第s颗可见星与GNSS接收机之间的真实距离，

${\mathrm{\ρ}}_k^s=\sqrt{{(X_k^s-x_k^u)}^2+{(Y_k^s-y_k^u)}^2+{(Z_k^s-z_k^u)}^2}.$

H_k中前n行由伪距方程线性化而来，后n行由多普勒方程线性化而来。

$h_{\mathrm{sx}|k}^1=\frac{{\∂R}_k^s}{\∂x_k^u}=\frac{{\∂D}_k^s}{{\∂v}_k^{\mathrm{ux}}}=\frac{({\hat{x}}_{k|k-1}^u-X_k^s)}{{\mathrm{\ρ}}_{k|k-1}^s}$

$h_{\mathrm{sx}|k}^2=\frac{\∂D_k^s}{\∂x_k^u}=\frac{{({\hat{v}}_{k|k-1}^{\mathrm{ux}}-V_k^{\mathrm{sx}})\left({\mathrm{\ρ}}_{k|k-1}^s\right)}^2-({\hat{x}}_{k|k-1}^u-X_k^s)J_{k|k-1}^s}{{\left({\mathrm{\ρ}}_{k|k-1}^s\right)}^2}$

${\mathrm{\ρ}}_{k|k-1}^s=\sqrt{{(X_k^s-{\hat{x}}_{k|k-1}^u)}^2+{(Y_k^s-{\hat{y}}_{k|k-1}^u)}^2+{(Z_k^s-{\hat{z}}_{k|k-1}^u)}^2}$

$J_{k|k-1}^s=(X_k^s-{\hat{x}}_{k|k-1}^u)\·(V_k^{\mathrm{sx}}-{\hat{v}}_{k|k-1}^{\mathrm{ux}})(Y_k^s-{\hat{y}}_{k|k-1}^u)\·(V_k^{\mathrm{sy}}-{\hat{v}}_{k|k-1}^{\mathrm{uy}})+(Z_k^s-{\hat{z}}_{k|k-1}^u)\·(V_k^{\mathrm{sz}}-{\hat{v}}_{k|k-1}^{\mathrm{uz}})$

用y和z分别代替

和

中的x，得到

和

和

(b)过程噪声方差矩阵 $O_k=\left[\begin{array}{cc}{\mathrm{\β}}_1\·I_{n\×n}& 0\\ 0& {\mathrm{\β}}_2\·I_{n\×n}\end{array}\right]---\left[9\right]$

其中，I_n×n为n阶方阵，β₁为伪距观测噪声方差，β₂为多普勒频移观测噪声方差。

步骤5：根据步骤2得到的计算状态向量X_k的滤波估计值

${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k\·\mathrm{\Δ}{Z}_k$

$\mathrm{\Δ}{Z}_k={[(r_k^1-{\hat{r}}_{k|k-1}^1),\·\·\·,(r_k^n-{\hat{r}}_{k|k-1}^n),(d_k^1-{\hat{d}}_{k|k-1}^1),\·\·\·,(d_k^n-{\hat{d}}_{k|k-1}^n)]}^T---\left[10\right]$

和

分别为第s颗可见星的伪距观测值和多普勒观测值。

${\hat{r}}_{k|k-1}^s=\sqrt{{(X_k^s-{\hat{x}}_{k|k-1}^u)}^2+{(Y_k^s-{\hat{x}}_{k|k-1}^u)}^2+{(Z_k^s-{\hat{z}}_{k|k-1}^u)}^2}+c\·\mathrm{\Δ}{\hat{t}}_{k|k-1}^u,$

${\hat{d}}_{k|k-1}^s=[(X_k^s-{\hat{x}}_{k|k-1}^u)\·(V_k^{\mathrm{sx}}-{\hat{v}}_{k|k-1}^{\mathrm{ux}})+(Y_k^s-{\hat{y}}_{k|k-1}^u)\·(V_k^{\mathrm{sy}}-v_{k|k-1}^{\mathrm{uy}})+(Z_k^s-z_{k|k-1}^u)\·(V_k^{\mathrm{sz}}-v_{k|k-1}^{\mathrm{uz}})]/{\mathrm{\ρ}}_{k|k-1}^s+c\·\mathrm{\Δ}{\hat{f}}_{k|k-1}^u$

步骤6：判断k的取值，若k≤m，转到步骤7，若k＞m，转到步骤8。

m取值范围为2<m<6。

若m≤2，滤波收敛速度慢；如果2<m<6，状态向量的滤波估计值在前5个历元就收敛到真值附近，解算误差与LS方法的解算误差相当，在第10个历元取得很好的解算精度，随着历元数的积累，解算精度略有提高；如果m≥6，状态向量在第10个历元才能收敛到真值附近，收敛速度慢。

步骤7：令P_k＝P_k-1。

步骤8：根据步骤4得到的K_k，计算P_k：P_k=[I-K_kH_k]P_k|k-1。

步骤9：输出 ${\hat{X}}_k={[{\hat{x}}_k^u,{\hat{v}}_k^{\mathrm{ux}},{\hat{a}}_k^{\mathrm{ux}},{\hat{y}}_k^u,{\hat{v}}_k^{\mathrm{uy}},{\hat{a}}_k^{\mathrm{uy}},{\hat{z}}_k^u,{\hat{v}}_k^{\mathrm{uz}},{\hat{a}}_k^{\mathrm{uz}},{\mathrm{c\&Delta;}\hat{t}}_k^u,\mathrm{c\&Delta;}{\hat{f}}_k^u]}^T.$

即为第k个历元的定位和测速结果。

步骤10：令k＝k+1，转入步骤2，继续计算并输出下一个历元的状态向量的滤波估计值，直到不需要进行接收机载体的定位测速时，停止计算。

有益效果

本发明的DU-EKF方法不需要保存每一步的计算数据，占用计算机内存资源少；直接令

不需要考虑

的取值，操作简便，且滤波收敛快，解算精度与LS方法相比有数量级上的提升，与EKF方法相当；这种方法从第5个历元起，所得的解算结果就是有效的。

附图说明

图1为本发明的基于改进EKF的GNSS单点动态定位方法流程图；

图2为具体实施方式中ECEF坐标系下，LS方法、CEKF方法、DU-EKF方法解算所得的接收机Z向位置误差对比图，图中将三种方法分别记作LSM、CEKFM和DU-EKFM；

图3为具体实施方式中ECEF坐标系下，LS方法、CEKF方法、DU-EKF方法解算所得的接收机Z向速度误差对比图，图中将三种方法分别记作LSM、CEKFM和DU-EKFM；

图4a和图4b分别为具体实施方式中ECEF坐标系下，DU-EKF方法和ZEKF方法解算所得的接收机Z向定位和测速误差，图中将两种方法分别记作DU-EKFM和ZEKFM。

具体实施方式

下面结合附图和具体实施例对本发明作进一步的详细说明。

本发明提供一种基于改进EKF的GNSS单点动态定位方法，所述方法流程如图1所示，实施例中以GPS系统为卫星导航系统。

在基于改进EKF的GNSS单点动态定位方法中，接收机载体的状态转移方程来自动力学模型，常用的动力学模型有CV模型、CA模型和Singer模型；对应于载体的动力学模型，接收机的钟差和钟漂之间的关系由时钟模型描述；观测方程来自观测模型，常用的观测模型有伪距观测模型和多普勒观测模型。实施例中，选取Singer模型和石英钟模型来描述状态转移方程，选取伪距和多普勒两种观测模型来描述观测方程，选取地心地固坐标系（ECEF）为参考坐标系。以下为实施例的具体执行步骤：

预设接收机处于ECEF坐标系下，其状态向量 $X_k={[x_k^u,v_k^{\mathrm{ux}},a_k^{\mathrm{ux}},y_k^u,v_k^{\mathrm{uy}},a_k^{\mathrm{uy}},z_k^u,v_k^{\mathrm{uz}},a_k^{\mathrm{uz}},{\mathrm{c\&Delta;t}}_k^u,\mathrm{c\&Delta;}{f}_k^u]}^T,$ 包含了导航定位所需求解的全部信息，其中，

分别为ECEF坐标下载体的三维位置、速度和加速度，

和

分别为接收机的钟差和钟漂。

则X_k的滤波估计 ${\hat{X}}_k={[{\hat{x}}_k^u,{\hat{v}}_k^{\mathrm{ux}},{\hat{a}}_k^{\mathrm{ux}},{\hat{y}}_k^u,{\hat{v}}_k^{\mathrm{uy}},{\hat{a}}_k^{\mathrm{uy}},{\hat{z}}_k^u,{\hat{v}}_k^{\mathrm{uz}},{\hat{a}}_k^{\mathrm{uz}},{\mathrm{c\&Delta;}\hat{t}}_k^u,\mathrm{c\&Delta;}{\hat{f}}_k^u]}^T.$

步骤1：令k＝1，设置初始状态向量

（维数为11），初始状态向量误差协方差矩阵P₀=diag[1/ε_p 1/ε_v 1/ε_a 1/ε_p 1/ε_v 1/ε_a 1/ε_p 1/ε_v 1/ε_a 1/ε_t 1/ε_f]。

步骤2：计算状态向量预测值X_k的预测值

(a)状态转移矩阵Φ_k|k-1参见公式[4]。Singer模型下，载体的状态转移矩阵如下描述，以ECEF坐标系下X向运动为例：

${\mathrm{\&Phi;}}_{k|k-1}^x=\left[\begin{array}{ccc}1& T& \frac{1}{{\mathrm{\&alpha;}}_x^2}(-1+{\mathrm{\&alpha;}}_x+e^{-{\mathrm{\&alpha;}}_{x}T})\\ 0& 1& \frac{1}{{\mathrm{\&alpha;}}_x}(1-e^{-{\mathrm{\&alpha;}}_{x}T})\\ 0& 0& e^{-{\mathrm{\&alpha;}}_{x}T}\end{array}\right]$

和

参见

只需将α_x轮换为α_y和α_z。α_x，α_y和α_z分别为X向，Y向和Z向的机动加速度频率，T为观测步长，即相邻两个观测历元间的时间差。

实施例中选取的石英钟模型由二阶马尔科夫过程来描述，

${\mathrm{\&Phi;}}_{k|k-1}^c=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]$

(b)将

和Φ_k|k-1代入

求

步骤3：计算

的协方差矩阵P_k|k-1，

(a)过程噪声协方差矩阵Q_k-1参见公式[5],仍以X向运动为例：

$Q_{k-1}^x=\left[\begin{array}{ccc}{q}_{11}& q_{12}& q_{13}\\ q_{21}& q_{22}& q_{23}\\ q_{31}& q_{32}& q_{33}\end{array}\right]$

$q_{11}=(1-e^{-2{\mathrm{\&alpha;}}_{x}T}+2{\mathrm{\&alpha;}}_{x}T+2{\mathrm{\&alpha;}}_x^3{T}^3/3-2{\mathrm{\&alpha;}}_x^2{T}^2-4{\mathrm{\&alpha;}}_{x}T{e}^{-{\mathrm{\&alpha;}}_{x}T})/{2\mathrm{\&alpha;}}_x^5$

$q_{22}=({4e}^{-{\mathrm{\&alpha;}}_{x}T}-3-e^{-2{\mathrm{\&alpha;}}_{x}T}+2{\mathrm{\&alpha;}}_{x}T)/2{\mathrm{\&alpha;}}_x^3$

$q_{33}=(1-2e^{-2{\mathrm{\&alpha;}}_{x}T})/2{\mathrm{\&alpha;}}_x$

$q_{12}=q_{21}=(e^{-2{\mathrm{\&alpha;}}_{x}T}+1-2e^{-{\mathrm{\&alpha;}}_{x}T}+2{\mathrm{\&alpha;}}_x{\mathrm{Te}}^{-{\mathrm{\&alpha;}}_{x}T}-2{\mathrm{\&alpha;}}_{x}T+{\mathrm{\&alpha;}}_x^2{T}^2)/2{\mathrm{\&alpha;}}_x^4$

$q_{13}=q_{31}=(1-e^{-2{\mathrm{\&alpha;}}_{x}T}-2{\mathrm{\&alpha;}}_{x}T{e}^{-{\mathrm{\&alpha;}}_{x}T})/2{\mathrm{\&alpha;}}_x^3$

$q_{23}=q_{32}=(1+e^{-2{\mathrm{\&alpha;}}_{x}T}-2e^{-{\mathrm{\&alpha;}}_{x}T})/2{\mathrm{\&alpha;}}_x^2$

和参见

将α_x轮换为α_y和α_z即可，

分别是X，Y，Z三向的机动加速度方差。

有关时钟的过程噪声解释如下：

$Q_{k-1}^c=\left[\begin{array}{cc}{q}_c^{11}& q_c^{12}\\ q_c^{21}& q_c^{22}\end{array}\right]$

$q_c^{11}=\frac{h_0}{2}T+2h_{-1}{T}^2+\frac{2}{3}{\mathrm{\&pi;}}^2{h}_{-2}{T}^3,$ $q_c^{12}=q_c^{21}={2h}_{-1}T+{\mathrm{\&pi;}}^2{h}_{-2}{T}^3,$ $q_c^{22}=\frac{h_0}{2T}+2h_{-1}+\frac{8}{3}{\mathrm{\&pi;}}^2{h}_{-2}T,$ 对于石英钟而言，h₀=9.4×10^-20,h_-1=1.8×10^-19,h_-2=3.8×10^-21；

(b)将Φ_k|k-1，P_k-1和Q_k-1代入

计算P_k|k-1。

步骤4：计算滤波增益矩阵K_k， $K_k=P_{k|k-1}{H}_k^T{[H_k{P}_{k|k-1}{H}_k^T+Q_k]}^{-1}.$

(a)观测矩阵H_k参见公式[8]；

(b)过程噪声协方差矩阵O_k参见公式[9]；

(c)将P_k|k-1，H_k和O_k代入 $K_k=P_{k|k-1}{H}_k^T{[H_k{P}_{k|k-1}{H}_k^T+Q_k]}^{-1}(k=1),$ 求得K_k。

步骤5：计算状态向量X_k的滤波估计值

(a)观测量△Z_k参见公式[10]；

(b)将

K_k和△Z_k代入

求得

步骤6：判断k的取值，若k≤m，转到步骤7，若k＞m，转到步骤8。

步骤7：直接令P_k＝P_k-1。

步骤8：对P_k进行计算，P_k=[I-K_kH_k]P_k|k-1；

步骤9：输出

步骤10：令k＝k+1，转入步骤2，继续计算得到下一个历元的

下面以具体数值进一步说明本发明。

本实施例在数学仿真环境中进行，设定仿真开始时间为UTC时间2011-6-202:00:00，仿真步长为1s（T＝1s）。接收机初始位置为纬度5°，经度5°，高度0m，在ECEF坐标系中的位置是[6329853.79,553790.45,552184.40]m，接收机的速度为ECEF坐标系中[5,5,5]m/s。观测量（伪距和多普勒频移）是根据GPS的RINEX星历和载体的真实运动轨迹计算所得的真距和真实多普勒频移分别加上8m和0.2Hz的高斯白噪声构成的，符合实际情形中排除电离层、对流层和多径误差后伪距和多普勒频移观测量的真实情况。

DU-EKF方法具体实施步骤如下，如图1所示，取m＝3：

预设接收机处于ECEF坐标系下，其状态向量 $X_k={[x_k^u,v_k^{\mathrm{ux}},a_k^{\mathrm{ux}},y_k^u,v_k^{\mathrm{uy}},a_k^{\mathrm{uy}},z_k^u,v_k^{\mathrm{uz}},a_k^{\mathrm{uz}},{\mathrm{c\&Delta;t}}_k^u,\mathrm{c\&Delta;}{f}_k^u]}^T,$ 包含了导航定位所需求解的全部信息，其中，

分别为ECEF坐标下载体的三维位置、速度和加速度，

和

分别为接收机的钟差和钟漂。

则X_k的滤波估计 ${\hat{X}}_k={[{\hat{x}}_k^u,{\hat{v}}_k^{\mathrm{ux}},{\hat{a}}_k^{\mathrm{ux}},{\hat{y}}_k^u,{\hat{v}}_k^{\mathrm{uy}},{\hat{a}}_k^{\mathrm{uy}},{\hat{z}}_k^u,{\hat{v}}_k^{\mathrm{uz}},{\hat{a}}_k^{\mathrm{uz}},{\mathrm{c\&Delta;}\hat{t}}_k^u,\mathrm{c\&Delta;}{\hat{f}}_k^u]}^T.$

步骤1：令k＝1，设（维数为11），

P₀=diag[1/ε_p 1/ε_v 1/ε_a 1/ε_p 1/ε_v 1/ε_a 1/ε_p 1/ε_v 1/ε_a 1/ε_t 1/ε_f]其中ε_p=10^-14，ε_a=0.01，ε_v=10^-3，ε_t=ε_f=10^-5。

步骤2：计算X_k-1的预测值

(a)求取状态转移矩阵Φ_k|k-1，参见公式[4]，Φ_k|k-1中α_x=α_y=α_z=10⁶；

(b)将

和Φ_k|k-1代入

求

步骤3：计算

的误差协方差矩阵P_k|k-1。

(a)求取过程噪声协方差矩阵Q_k-1，参见公式[5]，Q_k-1中α_x=α_y=α_z=10⁶，

${\mathrm{\&sigma;}}_x^2={\mathrm{\&sigma;}}_y^2={\mathrm{\&sigma;}}_z^2=100;$

(b)将Φ_k|k-1，P_k-1和Q_k-1代入

计算P_k|k-1。

步骤4：计算滤波增益矩阵K_k。

(a)求H_k

将第k个历元中参与定位的可见星的三维位置和三维速度以及代入公式[8],求得H_k，表1为k＝1时12颗可见星在ECEF坐标系下的三维位置和速度；

表1.第1个历元GPS可见星的三维位置和速度

(b)根据公式[9]求得O_k，O_k中β₁=64,β₂=0.04。

(c)将P_k|k-1，H_k和O_k代入 $K_k=P_{k|k-1}{H}_k^T{[H_k{P}_{k|k-1}{H}_k^T+Q_k]}^{-1}(k=1),$ 求得K_k。

步骤5：计算状态向量X_k的滤波估计值

(a)将可见星的伪距和多普勒观测值代入公式[10],求得△Z_k，表2为k＝1时12颗可见星的伪距和多普勒频移观测值；

表2.第1个历元GPS可见星的伪距观测值和多普勒观测值

(b)将

K_k和△Z_k代入

求得

步骤6：判断k的取值,若k≤3，转入步骤7；若k>3，转入步骤8。

步骤7：令P_k＝P_k-1。

步骤8：令P_k=[I-K_kH_k]P_k|k-1。

步骤9：输出

步骤10：令k＝k+1，转入步骤2，继续计算输出

本实施例中，将现有技术中基于EKF的GNSS单点动态定位方法分为两种，第一种称为ZEKF方法，其特点为

取零向量，P₀取为对角阵，其对角线元素取大数量级的正数；第二种称为CEKF方法，其特点为

取与真实的X₀相近的概略值，P₀取为对角阵，其对角线元素取大数量级的正数。

按本实施例中的步骤1-10，计算305个历元，统计从第5个历元到第305个历元的定位和测速结果，以相同仿真条件下LS方法和CEKF方法的解算结果作对比。定位误差如表3所示，测速误差如表4所示。

表3.不同方法的各向定位误差     单位：m

表4.不同方法的各向测速误差     单位：m/s

图2和图3分别是LS方法、CEKF方法和DU-EKF方法下，Z向的定位和测速精度。由于从第1个历元到第4个历元，DU-EKF方法和CEKF方法的定位和测速误差有5个数量级的提升，若展示在图中，后续历元的解算结果是一条0值附近的直线，不利于对三种方法解算结果的比较，因此图中横轴观测历元的取值从第5个历元开始。

分析表3和表4，结合图2和图3可得，算例中LS方法的定位误差在10m以内，测速误差在0.05m/s以内；CEKF方法的定位误差在2m以内，测速误差在0.02m/s以内；DU-EKF方法的定位误差在1.5m以内，测速误差在0.02m/s以内。与LS方法相比，DU-EKF方法的解算精度有数量级上的提升；与CEKF方法相比，DU-EKF方法的解算精度并没有明显的提高，但是DU-EKF方法不需要获知载体的概略位置和速度。

如图4所示，在初始状态向量同为

的情况下，DU-EKF方法的收敛速度要比ZEKF方法快很多，体现了DU-EKF方法的优越性。

本发明的DU-EKF方法对EKF方法进行了改进，在接收机的GNSS单点动态定位中，本方法收敛速度快，不需要在滤波开始时获知接收机的概略位置和速度，且解算精度与CEKF方法相当。将本方法运用到实际导航仪中，用户不需要在其位置发生大范围变动时对导航仪的概略位置进行重新设定，且获取首次定位测速信息的时间短，定位测速的精度高，能够为用户提供方便、快捷和高质量的定位测速服务。本发明所提供的方法适用于GNSS单点动态定位。

Claims (3)

1.基于改进扩展卡尔曼滤波的GNSS单点动态定位方法，其特征在于：

步骤1：在观测历元数k=1时，初始状态向量

初始状态向量误差协方差矩阵P₀＝diag[1/ε_p 1/ε_v 1/ε_a 1/ε_p 1/ε_v 1/ε_a 1/ε_p 1/ε_v 1/ε_a 1/ε_t 1/ε_f]；

ECEF坐标系下，GNSS接收机的状态向量定义为：

$X_k=[x_k^u,v_k^{\mathrm{ux}},a_k^{\mathrm{ux}},y_k^u,v_k^{\mathrm{uy}},a_k^{\mathrm{uy}},z_k^u,v_k^{\mathrm{uz}},a_k^{\mathrm{uz}},\mathrm{c\&Delta;}{t}_k^u,\mathrm{c\&Delta;}{f}_k^u]$

其中，k为观测历元数，

分别为ECEF坐标下GNSS接收机载体的三维位置、速度和加速度，

和

分别为接收机的钟差和钟漂；

X_k的滤波估计

为：

${\hat{X}}_k=[{\hat{x}}_k^u,{\hat{v}}_k^{\mathrm{ux}},{\hat{a}}_k^{\mathrm{ux}},{\hat{y}}_k^u,{\hat{v}}_k^{\mathrm{uy}},{\hat{a}}_k^{\mathrm{uy}},{\hat{z}}_k^u,{\hat{v}}_k^{\mathrm{uz}},{\hat{a}}_k^{\mathrm{uz}},\mathrm{c\&Delta;}{\hat{t}}_k^u,\mathrm{c\&Delta;}{\hat{f}}_k^u{]}^T$

ε_p为载体与地心距离平方的倒数，ε_v为载体最大允许速度平方的倒数，ε_a为载体最大允许加速度的倒数，ε_t为接收机钟差与光速乘积平方的倒数，ε_f为接收机钟漂与光速乘积平方的倒数；

步骤2：计算状态向量X_k的预测值

${\hat{X}}_{k|k-1}={\mathrm{\&Phi;}}_{k|k-1}{\hat{X}}_{k-1}=[{\hat{x}}_{k|k-1}^u,{\hat{v}}_{k|k-1,}^{\mathrm{ux}}{\hat{a}}_{k|k-1}^{\mathrm{ux}},{\hat{y}}_{k|k-1}^u,{\hat{v}}_{k|k-1,}^{\mathrm{uy}}{\hat{a}}_{k|k-1}^{\mathrm{uy}},{\hat{z}}_{k|k-1}^u,{\hat{v}}_{k|k-1}^{\mathrm{uz}},{\hat{a}}_{k|k-1}^{\mathrm{uz}},\mathrm{c\&Delta;}{\hat{t}}_{k|k-1}^u,\mathrm{c\&Delta;}{\hat{f}}_{k|k-1}^u{]}^T$

其中Φ_k|k-1为状态转移矩阵，来自接收机载体的动力学模型，

${\mathrm{\&Phi;}}_{k|k-1}=\left[\begin{array}{cccc}{\mathrm{\&Phi;}}_{k|k-1}^x& & & \\ & {\mathrm{\&Phi;}}_{k|k-1}^y& & \\ & & {\mathrm{\&Phi;}}_{k|k-1}^z& \\ & & & {\mathrm{\&Phi;}}_{k|k-1}^c\end{array}\right]$

分别是ECEF坐标系下，X向，Y向和Z向运动状态量的状态转移矩阵，依据动力学模型确定；

是有关钟差和钟漂的状态转移矩阵，依据时钟模型确定；

步骤3：计算预测值

的协方差矩阵

$P_{k|k-1}={\mathrm{\&Phi;}}_{k|k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k|k-1}^T+Q_{k-1}$

过程噪声协方差矩阵：

$Q_{k-1}=\left[\begin{array}{cccc}2{\mathrm{\&alpha;}}_x{\mathrm{\&sigma;}}_x^2{Q}_{k-1}^x& & & \\ & 2{\mathrm{\&alpha;}}_y{\mathrm{\&sigma;\&delta;}}_y^2{Q}_{k-1}^y& & \\ & & 2{\mathrm{\&alpha;}}_z{\mathrm{\&sigma;}}_z^2{Q}_{k-1}^z& \\ & & & Q_{k-1}^c\end{array}\right]$

和

分别是ECEF坐标系下，X,Y,Z三向运动状态量的过程噪声矩阵；α_y和α_z分别为X向，Y向和Z向的机动加速度频率；

分别是X,Y,Z三向的机动加速度方差；

步骤4：根据步骤3得到的P_k|k-1，计算滤波增益矩阵K_k：

$K_k=P_{k|k-1}{H}_k^T[H_k{P}_{k|k-1}{H}_k^T+Q_k{]}^{-1}$

(a)观测矩阵H_k由

线性化而来，

和

分别为第s颗可见星的伪距方程[6]和多普勒方程[7]，s＝1,...,n，n为接收机观测到的参与定位计算的可见星总数；

$R_k^s=\sqrt{{(X_k^s-x_k^u)}^2+{(Y_k^s-x_k^u)}^2+{(Z_k^s-z_k^u)}^2}+c\&CenterDot;\mathrm{\&Delta;}{t}_k^u---\left[6\right]$

$D_k^s=[(X_k^s-x_k^u)\&CenterDot;(V_k^{\mathrm{sx}}-v_k^{\mathrm{ux}})+(Y_k^s-y_k^u)\&CenterDot;(V_k^{\mathrm{sy}}-v_k^{\mathrm{uy}})+(Z_k^s-z_k^u)\&CenterDot;(V_k^{\mathrm{sz}}-v_k^{\mathrm{uz}})]/{\mathrm{\&rho;}}_k^s+c\&CenterDot;\mathrm{\&Delta;}{f}_k^u---\left[7\right]$

其中，

和

分别为ECEF坐标系下第s颗可见星在第k个历元的三维位置和速度，

是第s颗可见星与GNSS接收机之间的真实距离， ${\mathrm{\&rho;}}_k^s=\sqrt{{(X_k^s-x_k^u)}^2+{(Y_k^s-y_k^u)}^2+{(Z_k^s-z_k^u)}^2};$

$H_k=\left[\begin{array}{cccccccc}{h}_{1x|k}^1& 0& h_{1y|k}^1& 0& h_{1z|k}^1& 0& 1& 0\\ .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .\\ h_{\mathrm{nx}|k}^1& 0& h_{\mathrm{ny}|k}^1& 0& h_{\mathrm{nz}|k}^1& 0& 1& 0\\ h_{1x|k}^2& h_{1x|k}^1& h_{1y|k}^2& h_{1y|k}^1& h_{1y|k}^2& h_{1z|k}^1& 0& 0\\ .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .\\ h_{\mathrm{nx}|k}^2& h_{\mathrm{nx}|k}^1& h_{\mathrm{ny}|k}^2& h_{\mathrm{ny}|k}^1& h_{\mathrm{nz}|k}^2& h_{\mathrm{nz}|k}^1& 0& 1\end{array}\right]---\left[8\right]$

H_k中前n行由伪距方程线性化而来，后n行由多普勒方程线性化而来；

$h_{\mathrm{sx}|k}^1=\frac{{\&PartialD;R}_k^s}{{\&PartialD;x}_k^u}=\frac{{\&PartialD;D}_k^s}{{\&PartialD;v}_k^{\mathrm{ux}}}=\frac{({\hat{x}}_{k|k-1}^u-X_k^s)}{{\mathrm{\&rho;}}_{k|k-1}^s}$

$h_{\mathrm{sx}|k}^2=\frac{{\&PartialD;D}_k^s}{{\&PartialD;x}_k^u}=\frac{({\hat{v}}_{k|k-1}^{\mathrm{ux}}-V_k^{\mathrm{sx}}){\left({\mathrm{\&rho;}}_{k|k-1}^s\right)}^2-({\hat{x}}_{k|k-1}^u-X_k^s)J_{k|k-1}^s}{{\left({\mathrm{\&rho;}}_{k|k-1}^s\right)}^2}$

${\mathrm{\&rho;}}_{k|k-1}^s=\sqrt{{(X_k^s-{\hat{x}}_{k|k-1}^u)}^2+{(Y_k^s-{\hat{y}}_{k|k-1}^u)}^2+{(Z_k^s-{\hat{z}}_{k|k-1}^u)}^2}$

$J_{k|k-1}^s=(X_k^s-{\hat{x}}_{k|k-1}^u)\&CenterDot;(V_k^{\mathrm{sx}}-{\hat{v}}_{k|k-1}^{\mathrm{ux}})+(Y_k^s-{\hat{y}}_{k|k-1}^u)\&CenterDot;(V_k^{\mathrm{sy}}-{\hat{v}}_{k|k-1}^{\mathrm{uy}})+(Z_k^s-{\hat{z}}_{k|k-1}^u)\&CenterDot;(V_k^{\mathrm{sz}}-{\hat{v}}_{k|k-1}^{\mathrm{uz}})$

用y和z分别代替和

中的x，得到和和

(b)过程噪声方差矩阵 $O_k=\left[\begin{array}{cc}{\mathrm{\&beta;}}_1\&CenterDot;I_{n\&times;n}& 0\\ 0& {\mathrm{\&beta;}}_2\&CenterDot;I_{n\&times;n}\end{array}\right]---\left[9\right]$ 其中，I_n×n为n阶方阵，β₁为伪距观测噪声方差，β₂为多普勒频移观测噪声方差；

步骤5：根据步骤2得到的

计算状态向量X_k的滤波估计值

${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k\&CenterDot;\mathrm{\&Delta;}{Z}_k$

$\mathrm{\&Delta;}{Z}_k=[(r_k^1-{\hat{r}}_{k|k-1}^1),...,(r_k^n-{\hat{r}}_{k|k-1}^n),(d_k^1-{\hat{d}}_{k|k-1}^1),...,(d_k^n-{\hat{d}}_{k|k-1}^n)]---\left[10\right]$

和

分别为第s颗可见星的伪距观测值和多普勒观测值；

${\hat{r}}_{k|k-1}^s=\sqrt{{(X_k^s-{\hat{x}}_{k|k-1}^u)}^2+{(Y_k^s-{\hat{x}}_{k|k-1}^u)}^2+{(Z_k^s-{\hat{z}}_{k|k-1}^u)}^2}+c\&CenterDot;\mathrm{\&Delta;}{\hat{t}}_{k|k-1}^u,$

${\hat{d}}_{k|k-1}^s=[(X_k^s-{\hat{x}}_{k|k-1}^u)\&CenterDot;(V_k^{\mathrm{sx}}-{\hat{v}}_{k|k-1}^{\mathrm{ux}})+(Y_k^s-{\hat{y}}_{k|k-1}^u)\&CenterDot;(V_k^{\mathrm{sy}}-v_{k|k-1}^{\mathrm{uy}})+(Z_k^s-z_{k|k-1}^u)\&CenterDot;(V_k^{\mathrm{sz}}-v_{k|k-1}^{\mathrm{uz}})]/{\mathrm{\&rho;}}_{k|k-1}^s+c\&CenterDot;\mathrm{\&Delta;}{\hat{f}}_{k|k-1}^u$

步骤6：判断k的取值，若k≤m，转到步骤7，若k＞m，转到步骤8；

步骤7：令P_k=P_k-1，执行步骤9；

步骤8：根据步骤4得到的K_k，计算P_k：P_k＝[I-K_kH_k]P_k|k-1，执行步骤9；

步骤9：输出

${\hat{X}}_k=[{\hat{x}}_k^u,{\hat{v}}_k^{\mathrm{ux}},{\hat{a}}_k^{\mathrm{ux}},{\hat{y}}_k^u,{\hat{v}}_k^{\mathrm{uy}},{\hat{a}}_k^{\mathrm{uy}},{\hat{z}}_k^u,{\hat{v}}_k^{\mathrm{uz}},{\hat{a}}_k^{\mathrm{uz}},c{\mathrm{\&Delta;}\hat{t}}_k^u,\mathrm{c\&Delta;}{\hat{f}}_k^u{]}^T;$

即为第k个历元的定位和测速结果；

步骤10：令k=k+1，转入步骤2，继续计算并输出下一个历元的状态向量的滤波估计值，直到不需要进行接收机载体的定位测速时，停止计算。

2.根据权利要求1所述的基于改进扩展卡尔曼滤波的GNSS单点动态定位方法，其特征在于：ε_p，ε_v，ε_a，ε_t和ε_f均为正数。

3.根据权利要求1所述的基于改进扩展卡尔曼滤波的GNSS单点动态定位方法，其特征在于：2＜m＜6。

Images