CN110109162A - 一种gnss接收机自适应的卡尔曼滤波定位解算方法 - Google Patents

Abstract

本发明公开的一种GNSS接收机自适应的卡尔曼滤波定位解算方法，根据GNSS接收机接收到基带信号处理并输出GNSS测量值，采用自适应卡尔曼滤波算法进行定位解算。该自适应卡尔曼滤波算法在常规卡尔曼滤波算法的基础上，根据新息量或残差量对系统噪声协方差和测量噪声协方差进行实时估计，从而达到使接收机输出更加平滑、准确的定位结果。

Description

一种GNSS接收机自适应的卡尔曼滤波定位解算方法

技术领域

本发明属于卫星导航技术领域，具体涉及一种GNSS接收机自适应的卡尔曼滤波定位解算方法，能够提升车载导航系统的定位精度。

背景技术

GNSS定位解算算法是减少定位精度误差的关键技术之一。长期以来最小二乘算法以其简单原理等优点被广泛应用，其原理是在各个测量值的误差及其噪声之间寻求一最优点，使得所有测量量的残余平方和最小，但是由于不同时刻的不同测量误差与噪声在最小二乘计算后转化为相应的不同定位误差与噪声，因而最小二乘的定位结果粗糙又杂乱。为了克服最小二乘在性能上的不足，又将非线性滤波算法引入导航领域。

非线性卡尔曼滤波算法通常有扩展卡尔曼滤波和线性化卡尔曼滤波两种形式。扩展卡尔曼滤波的基本思路是假定卡尔曼滤波对当前系统状态的估计值非常接近于其真实值，于是将非线性函数f和h在当前状态估计值处进行泰勒展开并实现线性化。而线性化卡尔曼滤波与扩展卡尔曼滤波主要区别在于：前者将非线性函数f和h在滤波器对当前系统状态x_k的最优估计值线性化，而后者因为预先知道非线性系统的实际运行状态x_k大致按照所要求、希望的轨迹x_k变化，所以这些非线性函数在x_k点的值可以表达为在x_k处的泰勒展开式，从而完成线性化。所以线性化卡尔曼滤波需要预先知道系统状态的运行轨迹，而这一条件对于定位导航系统来说是不可预测的，所以扩展卡尔曼滤波被广泛应用。为了减少系统实时的计算量，扩展卡尔曼采用一阶泰勒级数展开式线性化的方法产生模型误差，导致滤波性能降低，甚至导致滤波器发散。扩展卡尔曼滤波时常因难以调节有关的滤波参数而不能给出最优估计值。

发明内容

针对GNSS导航系统中车载运动模型和噪声统计特性未知造成的定位误差大的问题，本申请提出了一种接收机自适应的卡尔曼滤波定位解算方法，通过设定的模型自动地预测和调整参数，从而达到使接收机输出更加平滑、准确的定位结果。

本发明是通过以下技术方案来实现：

一种GNSS接收机自适应的卡尔曼滤波定位解算方法，包括以下步骤；

S1、建立基于GNSS定位系统的接收机状态方程，设定状态方程的状态变量X；

X＝{x，y，z，vx，vy，vz，δf_u，δt_u]

其中，x，y，z表示接收机对应的坐标系统的位置信息；vx，vy，vz分别表示接收机对应坐标的速度信息；δf_u表示接收机的钟差漂移频率；δt_u表示接收机的钟差；

S2、建立描述载体接收机运动状态的系统模型，系统模型表达式如下；

x_k＝Ax_k-1+q_k

其中，A表示状态变量的状态转移矩阵，q_k表示过程噪声向量的均值；

S3、建立基于GNSS定位系统的接收机的测量方程y⁽ⁿ⁾；

ρ⁽ⁿ⁾表示某一观测时刻接收机到第n颗卫星的伪距，公式如下；

其中，n＝1,2....N是参与定位的卫星编号；r⁽ⁿ⁾表示接收机到卫星n的几何距离；δt_u表示接收机钟差；δt⁽ⁿ⁾表示卫星钟差；Iono⁽ⁿ⁾表示对应卫星电离层延迟；T⁽ⁿ⁾表示对应卫星对流层延迟；表示伪距测量噪声量；

表示伪距变化率，λ为GNSS定位系统载波频率对应的波长；为接收机的多普勒频移测量值；

I⁽ⁿ⁾表示第n颗卫星在接收机处的单位观测矢量；

V为表示接收机速度；δf_u为接收机时钟频漂；V⁽ⁿ⁾为第n颗卫星的速度；δf⁽ⁿ⁾为第n颗卫星的时钟频漂；为伪距变化率测量噪声量；

S4、对步骤S3的测量方程y⁽ⁿ⁾进行求解，得到起始0时刻的初始滤波值，初始滤波值包括最优估计值X₀，以及起始0时刻的后验估计误差协防差矩阵P₀；

S5、根据步骤S4得到的0时刻的初始滤波值对步骤2的系统模型进行自适应滤波，得到第k个历元时的先验估计值和先验估计误差协方差矩阵

其中，表示第k个历元时刻过程噪声向量w_k的均值的估计值，表示第k个历元时刻过程噪声协方差矩阵的估计值，并对和进行实时更新，A表示状态变量的状态转移矩阵；

S6、对步骤5得到的先验估计值和先验估计误差协方差矩阵进行计算，得到第k历元时的测量关系矩阵C_k、增益矩阵K_k的最优值以及新息序列e_k；

其中，表示第k个历元时刻测量噪声协方差矩阵的估计值，并对其进行实时更新；C_k ^T为C_k的转置矩阵；

其中，y_k为第k个历元的系统观测量；为测量噪声向量v_k的均值，并对进行实时更新；

S7、根据步骤6得到的测量关系矩阵C_k、增益矩阵K_k的最优值以及新息序列e_k进行计算，得到第k个历元时的最优估计值X_k和后验估计误差协方差矩阵作为第k+1个历元时的滤波器初始输入值；

优选的，步骤S4中采用加权最小二乘法对测量方程计算，得到起始0时刻接收机状态变量X的最优估计值X₀，以及起始0时刻的后验估计误差协防差矩阵P₀；

其中，var_pos为测量伪距的均方误差，var_vel为多普勒测速的均方误差。

优选的，步骤S5中过程噪声向量w_k的均值q_k，其初始值为0，更新方法如下公式；

其中，X_j的含义第j个历元时刻对应的最优估计值，N表示滑动窗宽的长度，其物理含义表示所选取的滤波次数；

过程噪声协方差矩阵的初始值如下：

其中，(S_vx，S_vy，S_vz)接收机速度的功率谱密度；S_f钟漂的功率谱密度，S_t钟的功率谱密度；

根据对N-1次的残差平方和与N次的残差平方取alpha滤波对过程噪声协方差矩阵进行更新，更新公式如下；

其中，ΔX_j为相邻两个历元的状态量残差序列，A^T表示状态转移矩阵的转置矩阵。

优选的，步骤S6中测量噪声协方差矩阵的初始值为理论测量噪声向量v_k的均值，测量噪声协方差矩阵更新的方法如下公式；

优选的，步骤S6中测量噪声协方差矩阵的起始值设定为：

测量噪声协方差矩阵更新公式如下；

其中，e_j为第j个历元的新息序列。

与现有技术相比，本发明具有以下有益的技术效果：

本发明提出一种GNSS接收机自适应的卡尔曼滤波定位解算方法，首先利用相邻时刻的估计值实时的更新残差序列，作为过程噪声的关键校正量实时调整。其次，新息序列作为自适应因子，能够更加精确地计算出估计值与测量值间的误差，更加精确的修正测量噪声和过程噪声矩阵，降低系统模型因噪声造成的定位误差问题；与现有的定位解算方法比较，针对GNSS车载导航系统的运动模型和噪声统计特性更好地提高定位精度。

附图说明

图1为本发明GNSS定位的卡尔曼滤波算法的流程图；

图2为仿真平台的结构图；

图3为静态场景下东向位置误差对比图；

图4为静态场景下北向位置误差对比图；

图5为静态场景下天顶向误差对比图；

图6为静态场景下东向速度误差对比图；

图7为静态场景下北向速度误差对比图；

图8为静态场景下天顶向速度误差对比图；

图9为动态场景下东向位置误差对比图；

图10为动态场景下北向位置误差对比图；

图11为动态场景下天顶向位置误差对比图；

图12为动态场景下东向速度误差对比图；

图13为动态场景下北向速度误差对比图；

图14为动态场景下天顶向速度误差对比图。

具体实施方式

下面结合附图对本发明做进一步的详细说明，所述是对本发明的解释而不是限定。

一种GNSS接收机自适应的卡尔曼滤波定位解算方法，根据GNSS接收机接收到基带信号处理并输出GNSS测量值，采用自适应卡尔曼滤波算法进行定位解算。该自适应卡尔曼滤波算法在常规卡尔曼滤波算法的基础上，根据新息量或残差量对系统噪声协方差和测量噪声协方差进行实时估计，从而达到使接收机输出更加平滑、准确的定位结果。

一种GNSS接收机自适应的卡尔曼滤波定位解算方法，具体包括以下步骤；

建立系统

S1、建立基于GNSS定位系统的接收机状态量方程，设X为状态方程的状态变量；

X＝[x，y，z，vx，vy，vz，δf_u，δt_u]

其中，x，y，z表示接收机对应的坐标系统的位置信息；vx，vy，vz分别表示接收机对应坐标的速度信息；δf_u表示接收机的钟差漂移频率；δt_u表示接收机的钟差。

S2、建立描述载体接收机运动状态的系统模型，系统模型表达式如下；

x_k＝Ax_k-1+q_k

其中，A表示状态变量的状态转移矩阵，q_k表示过程噪声向量的均值。

S3、建立基于GNSS定位系统的接收机的测量方程y⁽ⁿ⁾；

测量方程包括伪距观测方程式和多普勒定速方程，伪距观测方程ρ⁽ⁿ⁾如下，其对应接收机的位置坐标的状态量为[x，y，z，δt_u]；

其中，n＝1,2....N是参与定位的卫星编号；

ρ⁽ⁿ⁾表示在某一观测时刻接收机到第n颗卫星的伪距；

δt_u表示接收机钟差；δt⁽ⁿ⁾表示卫星钟差；Iono⁽ⁿ⁾表示对应卫星电离层延迟；T⁽ⁿ⁾表示对应卫星对流层延迟；伪距测量噪声量。

r⁽ⁿ⁾表示接收机到卫星n的几何距离，公式如下：

(x⁽ⁿ⁾,y⁽ⁿ⁾,z⁽ⁿ⁾)为第n颗星对应定位系统下的三维坐标；(x,y,z)为接收机对应坐标系的坐标；

表示伪距变化率，λ为GNSS定位系统载波频率对应的波长；为接收机的多普勒频移测量值；多普勒定速方程如下,对应接收机的速度坐标信息状态量为[vx，vy，vx，δf_u]；

其中，I⁽ⁿ⁾表示第n颗卫星在接收机处的单位观测矢量；

V表示未知的接收机速度；

δf_u表示未知的接收机时钟频漂；

V⁽ⁿ⁾表示第n颗卫星的速度；

δf⁽ⁿ⁾表示第n颗卫星的时钟频漂；

表示伪距变化率测量噪声量；

伪距和多普勒测量噪声量

测量噪声协方差矩阵R＝E(εε^T)。

S4、卡尔曼滤波器0时刻初值的选取

根据加权最小二乘法对测量方程y⁽ⁿ⁾进行解算，得到起始0时刻的初始滤波值，其包括接收机状态变量X的最优估计值X₀，以及起始0时刻的后验估计误差协防差矩阵P₀；

其中，var_pos为测量伪距的均方误差，var_vel为多普勒测速的均方误差。

S5、根据步骤S4得到的0时刻的初始滤波值对步骤2的系统模型进行滤波，得到第k个历元时的先验估计值和先验估计误差协方差矩阵

其中，表示第k个历元时刻过程噪声向量w_k的均值的估计值，其初始值取0，并对程噪声向量w_k的均值实时更新；

表示第k个历元时刻过程噪声协方差矩阵的估计值，其初始值取值如下：

其中，(S_vx，S_vy，S_vz)接收机速度的功率谱密度；S_f钟漂的功率谱密度，S_t钟的功率谱密度；

根据对前N-1次的残差平方和与N次的残差平方取alpha滤波对过程噪声协方差矩阵的估计值进行更新，N表示滑动窗宽的长度，物理含义表示所选取的滤波次数，可根据历元时刻的长短进行适当调整。

过程噪声向量w_k的均值估计值和过程噪声协方差矩阵估计值的更新公式如下：

其中，A^T的为状态转移矩阵A的转置矩阵；为第j个历元时刻对应的最优估计值；ΔX_j为相邻两个历元的状态量残差序列，能够实时修正矩阵，已达到自适应的效果。

S6、根据步骤S5得到的先验估计值进行计算，得到第k历元时的测量关系矩阵C_k，详见公式(6)：

根据步骤S5得到的先验估计误差协方差矩阵和测量关系矩阵C_k，计算出第k个历元时的增益矩阵K_k的最优值，详见公式(7)；

其中，测量噪声协方差矩阵的起始值设定为并对测量噪声协方差矩阵更新，更新公式如下；

其中，e_j的含义第j个历元的新息序列。

根据步骤S5得到的先验估计值计算新息序列e_k，详见公式(9)；

其中，y_k的表示第k个历元的系统观测量；的初始值为理论测量噪声向量v_k的均值，并对实时更新，更新公式如下：

新息序列e_k为本发明自适应滤波的核心，能够实时的对噪声协方差矩阵Q_k和测量噪声协方差矩阵R_k进行实时估计。

S7、根据步骤S6得到的增益矩阵K_k和新息序列e_k，依据公式(11)和(12)得到第k个历元时的滤波器输出值，作为第k+1个历元时的最优估计值。

本发明提出的自适应滤波与现有技术相比较，针对GNSS车载导航系统的运动模型和噪声统计特性更好地提高定位精度。首先利用相邻时刻的估计值实时的更新残差序列，作为过程噪声的关键校正量实时调整。其次，新息序列e_k作为自适应因子，能够更加精确地计算出估计值与测量值间的误差，更加精确的修正测量噪声和过程噪声矩阵，降低系统模型因噪声造成的定位误差问题。图1为本发明自适应卡尔曼滤波的流程示意图。

本发明的效果可以通过以下仿真结果进一步说明：

测试场景

仿真数据的平台搭建如图2所示，测试场景数据来自安捷伦信号发生器，采样间隔50ms，总的观测时间为45min。本次测试主要是针对最小二乘算法、扩展卡尔曼滤波和本发明的自适应卡尔曼滤波算法针对定位精度的对比分析。两组测试场景：静态场景和动态场景。

图3-14中第一行表示WLS(加权最小二乘算法)的定位结果与信号发生器理论差值；

第二行表示EKF(扩展卡尔曼滤波算法)的定位结果与信号发生器理论差值；

第三行表示AKF(本发明自适应滤波算法)的定位结果与信号发生器理论坐标差值；

第四行表示WLS、EKF和AKF的定位结果与信号发生器理论坐标差值对比图。

图3-14中每一行的横轴表示为UTC时间轴，竖轴表示接收机站心坐标下与信号发生器参考坐标的差值。

静态场景:

图3至图8表示接收机站心坐标下与信号发生器参考坐标的差值，图3、4、5分别表示位置坐标的东向、北向及天顶方向的差值，图6、7、8分别表示接收机速度的东向、北向及天顶方向的差值。图中可明显看出本发明算法在定位误差上更加平滑，浮动变小，使定位位置和速度在精度上有明显提升，具体详见表1中的数值统计结果。

在此说明表中水平方向是东向和北向的平方和开根号得出的结论。 其中ΔN_i、ΔE_i、ΔH_i分别表示定位数据的N(北向)、E(东向)方向和水平方向的定位误差。

表1静态场景位置和速度误差对比

动态场景：

图9至图14表示接收机站心坐标下与信号发生器参考坐标的差值，图9、10、11分别表示位置坐标的东向、北向及天顶方向的差值，图12、13、14分别表示接收机速度的东向、北向及天顶方向的差值。图中可明显看出本发明算法在定位精度上有明显提升，具体详见表2中的数值统计。

表2动态场景位置和速度误差对比

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (5)

1.一种GNSS接收机自适应的卡尔曼滤波定位解算方法，其特征在于，包括以下步骤；

S1、建立基于GNSS定位系统的接收机状态方程，设定状态方程的状态变量X；

X＝[x，y，z，vx，uy，uz，δf_u，δt_u]

其中，x，y，z表示接收机对应的坐标系统的位置信息；vx，vy，vz分别表示接收机对应坐标的速度信息；δf_u表示接收机的钟差漂移频率；δt_u表示接收机的钟差；

S2、建立描述载体接收机运动状态的系统模型，系统模型表达式如下；

x_k＝Ax_k-1+q_k

其中，A表示状态变量的状态转移矩阵，q_k表示过程噪声向量的均值；

S3、建立基于GNSS定位系统的接收机的测量方程y⁽ⁿ⁾；

ρ⁽ⁿ⁾表示某一观测时刻接收机到第n颗卫星的伪距，公式如下；

其中，n＝1，2....N是参与定位的卫星编号；r⁽ⁿ⁾表示接收机到卫星n的几何距离；δt_u表示接收机钟差；δt⁽ⁿ⁾表示卫星钟差；Iono⁽ⁿ⁾表示对应卫星电离层延迟；T⁽ⁿ⁾表示对应卫星对流层延迟；友示伪距测量噪声量；

表示伪距变化率，λ为GNSS定位系统载波频率对应的波长；为接收机的多普勒频移测量值；

I⁽ⁿ⁾表示第n颗卫星在接收机处的单位观测矢量；

V为表示接收机速度；δf_u为接收机时钟频漂；V⁽ⁿ⁾为第n颗卫星的速度；δf⁽ⁿ⁾为第n颗卫星的时钟频漂；为伪距变化率测量噪声量；

S4、对步骤S3的测量方程y⁽ⁿ⁾进行求解，得到起始0时刻的初始滤波值，初始滤波值包括最优估计值X₀，以及起始0时刻的后验估计误差协防差矩阵P₀；

S5、根据步骤S4得到的0时刻的初始滤波值对步骤2的系统模型进行自适应滤波，得到第k个历元时的先验估计值和先验估计误差协方差矩阵

其中，友示第k个历元时刻过程噪声向量w_k的均值的估计值，友示第k个历元时刻过程噪声协方差矩阵的估计值，并对和进行实时更新，A表示状态变量的状态转移矩阵；

S6、对步骤5得到的先验估计值和先验估计误差协方差矩阵进行计算，得到第k历元时的测量关系矩阵C_k、增益矩阵K_k的最优值以及新息序列e_k；

其中，表示第k个历元时刻测量噪声协方差矩阵的估计值，并对其进行实时更新；C_k ^T为C_k的转置矩阵；

其中，y_k为第k个历元的系统观测量；为测量噪声向量v_k的均值，并对进行实时更新；

S7、根据步骤6得到的测量关系矩阵C_k、增益矩阵K_k的最优值以及新息序列e_k进行计算，得到第k个历元时的最优估计值X_k和后验估计误差协方差矩阵作为第k+1个历元时的滤波器初始输入值；

2.根据权利要求1所述GNSS接收机自适应的卡尔曼滤波定位解算方法，其特征在于，步骤S4中采用加权最小二乘法对测量方程计算，得到起始0时刻接收机状态变量X的最优估计值X₀，以及起始0时刻的后验估计误差协防差矩阵P₀；

X₀＝[x₀，y₀，z₀，vx₀，υy₀，v₀z，δf_u0，δt_u0]

其中，var_pos为测量伪距的均方误差，var_vel为多普勒测速的均方误差。

3.根据权利要求2所述GNSS接收机自适应的卡尔曼滤波定位解算方法，其特征在于，步骤S5中过程噪声向量w_k的均值q_k，其初始值为0，更新方法如下公式；

其中，X_j的含义第j个历元时刻对应的最优估计值，N表示滑动窗宽的长度，其物理含义表示所选取的滤波次数；

过程噪声协方差矩阵的初始值如下：

其中，(S_vx，S_vy，S_vz)接收机速度的功率谱密度；S_f钟漂的功率谱密度，S_t钟的功率谱密度；

根据对N-1次的残差平方和与N次的残差平方取alpha滤波对过程噪声协方差矩阵进行更新，更新公式如下；

其中，ΔX_j为相邻两个历元的状态量残差序列，A^T表示状态转移矩阵的转置矩阵。

4.根据权利要求3所述GNSS接收机自适应的卡尔曼滤波定位解算方法，其特征在于，步骤S6中测量噪声协方差矩阵的初始值为理论测量噪声向量v_k的均值，测量噪声协方差矩阵更新的方法如下公式；

5.根据权利要求4所述GNSS接收机自适应的卡尔曼滤波定位解算方法，其特征在于，步骤S6中测量噪声协方差矩阵的起始值设定为：

测量噪声协方差矩阵更新公式如下；

其中，e_j为第j个历元的新息序列。