[0074] 步骤7:令Pk=Pk-1。
[0075] 步骤8:根据步骤4得到的Kk,计算Pk:Pk=[I-KkHk]Pk|k-1。
[0076] 步骤9:输出
[0077] 即为第k个历元的定位和测速结果。
[0078] 步骤10:令k=k+1,转入步骤2,继续计算并输出下一个历元的状态向量的滤波估计值,直到不需要进行接收机载体的定位测速时,停止计算。
[0079] 有益效果
[0080] 本发明的DU-EKF方法不需要保存每一步的计算数据,占用计算机内存资源少;直接令 不需要考虑 的取值,操作简便,且滤波收敛快,解算精度与LS方法相比有数量级上的提升,与EKF方法相当;这种方法从第5个历元起,所得的解算结果就是有效的。
附图说明
[0081] 图1为本发明的基于改进EKF的GNSS单点动态定位方法流程图;
[0082] 图2为具体实施方式中ECEF坐标系下,LS方法、CEKF方法、DU-EKF方法解算所得的接收机Z向位置误差对比图,图中将三种方法分别记作LSM、CEKFM和DU-EKFM;
[0083] 图3为具体实施方式中ECEF坐标系下,LS方法、CEKF方法、DU-EKF方法解算所得的接收机Z向速度误差对比图,图中将三种方法分别记作LSM、CEKFM和DU-EKFM;
[0084] 图4a和图4b分别为具体实施方式中ECEF坐标系下,DU-EKF方法和ZEKF方法解算所得的接收机Z向定位和测速误差,图中将两种方法分别记作DU-EKFM和ZEKFM。
具体实施方式
[0085] 下面结合附图和具体实施例对本发明作进一步的详细说明。
[0086] 本发明提供一种基于改进EKF的GNSS单点动态定位方法,所述方法流程如图1所示,实施例中以GPS系统为卫星导航系统。
[0087] 在基于改进EKF的GNSS单点动态定位方法中,接收机载体的状态转移方程来自动力学模型,常用的动力学模型有CV模型、CA模型和Singer模型;对应于载体的动力学模型,接收机的钟差和钟漂之间的关系由时钟模型描述;观测方程来自观测模型,常用的观测模型有伪距观测模型和多普勒观测模型。实施例中,选取Singer模型和石英钟模型来描述状态转移方程,选取伪距和多普勒两种观测模型来描述观测方程,选取地心地固坐标系(ECEF)为参考坐标系。以下为实施例的具体执行步骤:
[0088] 预 设 接 收 机 处 于 ECEF 坐 标 系 下, 其 状 态 向 量包含了导航定位所需求解的全部信
息,其中, 分别为ECEF坐标下载体的三维位置、速度和加速
度, 和 分别为接收机的钟差和钟漂。
[0089] 则Xk的滤波估计
[0090] 步骤1:令k=1,设置初始状态向量 (维数为11),初始状态向量误差协方差矩阵P0=diag[1/εp 1/εv 1/εa 1/εp 1/εv 1/εa 1/εp 1/εv 1/εa 1/εt 1/εf]。
[0091] 步骤2:计算状态向量预测值Xk的预测值
[0092] (a)状态转移矩阵Φk|k-1参见公式[4]。Singer模型下,载体的状态转移矩阵如下描述,以ECEF坐标系下X向运动为例:
[0093]
[0094] 和 参见 只需将αx轮换为αy和αz。αx,αy和αz分别为X向,Y向和Z向的机动加速度频率,T为观测步长,即相邻两个观测历元间的时间差。
[0095] 实施例中选取的石英钟模型由二阶马尔科夫过程来描述,
[0096]
[0097] (b)将 和Φk|k-1代入 求
[0098] 步骤3:计算 的协方差矩阵Pk|k-1,
[0099] (a)过程噪声协方差矩阵Qk-1参见公式[5],仍以X向运动为例:
[0100]
[0101]
[0102]
[0103]
[0104]
[0105]
[0106]
[0107] 和 参见 将αx轮换为αy和αz即可, 分别是X,Y,Z三向的机动加速度方差。
[0108] 有关时钟的过程噪声解释如下:
[0109]
[0110]对于石英钟而言,h0=9.4×10-20,h-1=1.8×10-19,h-2=3.8×10-21;
[0111] (b)将Φk|k-1,Pk-1和Qk-1代入 计算Pk|k-1。
[0112] 步骤4:计算滤波增益矩阵Kk,
[0113] (a)观测矩阵Hk参见公式[8];
[0114] (b)过程噪声协方差矩阵Ok参见公式[9];
[0115] (c)将Pk|k-1,Hk和Ok代入 求得Kk。
[0116] 步骤5:计算状态向量Xk的滤波估计值
[0117] (a)观测量△Zk参见公式[10];
[0118] (b)将 Kk和△Zk代入 求得
[0119] 步骤6:判断k的取值,若k≤m,转到步骤7,若k>m,转到步骤8。
[0120] 步骤7:直接令Pk=Pk-1。
[0121] 步骤8:对Pk进行计算,Pk=[I-KkHk]Pk|k-1;
[0122] 步骤9:输出
[0123] 步骤10:令k=k+1,转入步骤2,继续计算得到下一个历元的
[0124] 下面以具体数值进一步说明本发明。
[0125] 本实施例在数学仿真环境中进行,设定仿真开始时间为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的高斯白噪声构成的,符合实际情形中排除电离层、对流层和多径误差后伪距和多普勒频移观测量的真实情况。
[0126] DU-EKF方法具体实施步骤如下,如图1所示,取m=3:
[0127] 预 设 接 收 机 处 于 ECEF 坐 标 系 下, 其 状 态 向 量包含了导航定位所需求解的全部信息,其
中, 分别为ECEF坐标下载体的三维位置、速度和加速度,
和 分别为接收机的钟差和钟漂。
[0128] 则Xk的滤波估计
[0129] 步骤1:令k=1,设 (维数为11),
[0130] P0=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。
[0131] 步骤2:计算Xk-1的预测值
[0132] (a)求取状态转移矩阵Φk|k-1,参见公式[4],Φk|k-1中αx=αy=αz=106;
[0133] (b)将 和Φk|k-1代入 求
[0134] 步骤3:计算 的误差协方差矩阵Pk|k-1。
[0135] (a)求取过程噪声协方差矩阵Qk-1,参见公式[5],Qk-1中αx=αy=αz=106,[0136]
[0137] (b)将Φk|k-1,Pk-1和Qk-1代入 计算Pk|k-1。
[0138] 步骤4:计算滤波增益矩阵Kk。
[0139] (a)求Hk
[0140] 将第k个历元中参与定位的可见星的三维位置和三维速度以及 代入公式[8],求得Hk,表1为k=1时12颗可见星在ECEF坐标系下的三维位置和速度;
[0141] 表1.第1个历元GPS可见星的三维位置和速度
[0142]
[0143]
[0144] (b)根据公式[9]求得Ok,Ok中β1=64,β2=0.04。
[0145] (c)将Pk|k-1,Hk和Ok代入 求得Kk。
[0146] 步骤5:计算状态向量Xk的滤波估计值
[0147] (a)将可见星的伪距和多普勒观测值代入公式[10],求得△Zk,表2为k=1时12颗可见星的伪距和多普勒频移观测值;
[0148] 表2.第1个历元GPS可见星的伪距观测值 和多普勒观测值
[0149]
[0150] (b)将 Kk和△Zk代入 求得
[0151] 步骤6:判断k的取值,若k≤3,转入步骤7;若k>3,转入步骤8。
[0152] 步骤7:令Pk=Pk-1。
[0153] 步骤8:令Pk=[I-KkHk]Pk|k-1。
[0154] 步骤9:输出
[0155] 步骤10:令k=k+1,转入步骤2,继续计算输出
[0156] 本实施例中,将现有技术中基于EKF的GNSS单点动态定位方法分为两种,第一种称为ZEKF方法,其特点为 取零向量,P0取为对角阵,其对角线元素取大数量级的正数;第二种称为CEKF方法,其特点为 取与真实的X0相近的概略值,P0取为对角阵,其对角线元素取大数量级的正数。
[0157] 按本实施例中的步骤1-10,计算305个历元,统计从第5个历元到第305个历元的定位和测速结果,以相同仿真条件下LS方法和CEKF方法的解算结果作对比。定位误差如表3所示,测速误差如表4所示。
[0158] 表3.不同方法的各向定位误差 单位:m
[0159]
[0160] 表4.不同方法的各向测速误差 单位:m/s
[0161]
[0162] 图2和图3分别是LS方法、CEKF方法和DU-EKF方法下,Z向的定位和测速精度。由于从第1个历元到第4个历元,DU-EKF方法和CEKF方法的定位和测速误差有5个数量级的提升,若展示在图中,后续历元的解算结果是一条0值附近的直线,不利于对三种方法解算结果的比较,因此图中横轴观测历元的取值从第5个历元开始。
[0163] 分析表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方法不需要获知载体的概略位置和速度。
[0164] 如图4所示,在初始状态向量同为 的情况下,DU-EKF方法的收敛速度要比ZEKF方法快很多,体现了DU-EKF方法的优越性。
[0165] 本发明的DU-EKF方法对EKF方法进行了改进,在接收机的GNSS单点动态定位中,本方法收敛速度快,不需要在滤波开始时获知接收机的概略位置和速度,且解算精度与CEKF方法相当。将本方法运用到实际导航仪中,用户不需要在其位置发生大范围变动时对导航仪的概略位置进行重新设定,且获取首次定位测速信息的时间短,定位测速的精度高,能够为用户提供方便、快捷和高质量的定位测速服务。本发明所提供的方法适用于GNSS单点动态定位。