[发明专利]一种基于高度约束的扩展卡尔曼滤波定位方法

说明书

技术领域

本发明涉及卫星定位领域，尤其涉及一种基于高度约束的扩展卡尔曼滤波定位方法。

背景技术

卡尔曼滤波算法可以对线性高斯白噪声系统的状态进行最优估计，在GNSS定位导航中被广泛使用。卡尔曼滤波算法首先利用状态方程预测接收机当前的位置、速度和钟差等状态，然后根据状态估计值以及卫星星历所提供的卫星位置和速度，预测出各卫星的伪距和多普勒频移值，最后通过处理测量残余得到校正后的最优估计值。卡尔曼滤波算法在每得到一个新的测量值后，就可以对系统状态估计值更新一次，成为提高GNSS导航定位实时性的一种有效手段。

传统的卡尔曼滤波定位方法多采用传感器、气压计等辅助设备使用具有航向约束、速度约束的卡尔曼滤波算法来提高GNSS在高度方向上的定位精度。这些方法均采用外设或需要已知航向、实时速度等信息，增加了卫星定位的成本。

发明内容

发明目的：为了解决卫星定位在高度方向上的剧变问题，本发明提供一种基于高度约束的扩展卡尔曼滤波定位方法，在不借助外界辅助信息和外接传感器设备的基础上，能够提高GNSS导航定位的精度。

技术方案：为了实现上述目的，本发明中基于高度约束的扩展卡尔曼滤波定位方法，包括如下步骤：

(1)利用预测公式根据上一历元的最终状态估计值以及最终状态估计值的均方误差P_k-1估计待定位目标的在当前历元的先验状态估计值和该先验状态估计值的均方误差

(2)构建高度约束条件，利用最小均方误差原则获取在高度约束条件下，当前历元的最优状态估计值以及最优状态估计值的均方误差

(3)利用卡尔曼滤波的增益矩阵K_k对所述当前历元的最优状态估计值以及最优状态估计值的均方误差进行校正，得到当前历元的最终状态估计值和该最终状态估计值的均方误差阵P_k。

有益效果：本发明中基于高度约束的扩展卡尔曼滤波定位方法，从应用角度出发，充分考虑汽车行驶的各种场景，针对各场景均存在临界角度的特点提出爬坡模型无需外设传感器设备，便能够提高卫星定位精度、解决定位高度剧变现象。

附图说明

图1为本发明中建立的用户运动约束模型示意图；

图2为本发明中基于高度约束扩展卡尔曼滤波定位方法的流程图；

图3为本发明中基于高度约束扩展卡尔曼滤波定位方法HCEKF与传统的卡尔曼滤波EKF、高度气压计和卡尔曼滤波EKF融合算法与真实运动轨迹在海拔高度微变场景下的天向误差图；

图4为本发明中基于高度约束扩展卡尔曼滤波定位方法HCEKF与传统的卡尔曼滤波EKF、高度气压计和卡尔曼滤波EKF融合算法与真实运动轨迹在海拔高度陡变场景下的天向误差图；

具体实施方式

下面结合附图进一步解释本发明。

本发明中基于高度约束的扩展卡尔曼滤波定位方法，建立高度约束条件对传统的扩展卡尔曼滤波定位方法进行优化，本实施中先具体介绍所使用的爬坡模型。以汽车作为待定位目标，在实际场景中，如图1所示，汽车在行驶时与东北向平面的夹角均小于某一角度，称该角度为临界角θ，即汽车行驶的平面和东北向平面的实际角度均小于该临界角，本实施例中设置该临界角为5°。

根据三角函数关系，可以得到高度约束条件：

式(1)中，E_k、N_k分别表示待定位目标在第k历元的东向、北向坐标，U_k表示待定位目标在第k历元的天向坐标，待定位目标的东北天坐标需满足上述高度约束条件。

在扩展卡尔曼滤波定位算法过程中，所得到的待定位目标位置为地心地固直角坐标系坐标，记为(X_k[0]，X_k[1]，X_k[2])，对应的东北天坐标记为(E_k,N_k,U_k)，其中E_k，N_k和U_k分别代表状态的东向、北向和天向坐标。

地心地固直角坐标(X_k[0]，X_k[1]，X_k[2])转化为东北天坐标(E_k,N_k,U_k)，首先需要转化到大地坐标(φ,λ,h)，其变换公式为：