WO2017215026A1 - 一种基于高度约束的扩展卡尔曼滤波定位方法 - Google Patents

一种基于高度约束的扩展卡尔曼滤波定位方法 Download PDF

Info

Publication number
WO2017215026A1
WO2017215026A1 PCT/CN2016/087287 CN2016087287W WO2017215026A1 WO 2017215026 A1 WO2017215026 A1 WO 2017215026A1 CN 2016087287 W CN2016087287 W CN 2016087287W WO 2017215026 A1 WO2017215026 A1 WO 2017215026A1
Authority
WO
WIPO (PCT)
Prior art keywords
epoch
coordinates
kalman filter
mean square
square error
Prior art date
Application number
PCT/CN2016/087287
Other languages
English (en)
French (fr)
Inventor
庄园
马金凤
戚隆宁
刘新宁
杨军
Original Assignee
东南大学
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 东南大学 filed Critical 东南大学
Priority to US16/309,939 priority Critical patent/US10422883B2/en
Publication of WO2017215026A1 publication Critical patent/WO2017215026A1/zh

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/50—Determining position whereby the position solution is constrained to lie upon a particular curve or surface, e.g. for locomotives on railway tracks
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/40—Correcting position, velocity or attitude
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/13—Receivers
    • G01S19/35—Constructional details or hardware or software details of the signal processing chain
    • G01S19/37—Hardware or software details of the signal processing chain
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position

Definitions

  • the invention relates to the field of satellite positioning, and in particular to a method for positioning an extended Kalman filter based on a height constraint.
  • the Kalman filter algorithm can optimally estimate the state of a linear Gaussian white noise system and is widely used in GNSS positioning navigation.
  • the Kalman filter algorithm first uses the state equation to predict the current position, velocity and clock difference of the receiver, and then predicts the pseudorange and Doppler of each satellite based on the state estimate and the satellite position and velocity provided by the satellite ephemeris.
  • the frequency shift value is finally obtained by processing the measurement residual to obtain the corrected optimal estimate.
  • the Kalman filter algorithm can update the system state estimation value once, which is an effective means to improve the real-time performance of GNSS navigation and positioning.
  • the traditional Kalman filter positioning method mostly uses sensors such as sensors and barometers to use Kalman filter algorithm with heading constraint and speed constraint to improve the positioning accuracy of GNSS in the height direction. These methods all use peripherals or require information such as known heading and real-time speed, which increases the cost of satellite positioning.
  • the present invention provides an extended Kalman filter positioning method based on height constraint, which can improve GNSS navigation and positioning without using external auxiliary information and external sensor equipment. Precision.
  • the method for positioning a Kalman filter based on a high degree of constraint in the present invention includes the following steps:
  • a method based on a highly constrained extended Kalman filter is applied from the perspective of application. Taking full account of various scenes of car driving, the characteristics of critical angles exist in each scene.
  • the climbing model can improve the positioning accuracy of satellites and solve the phenomenon of rapid change of positioning height without the need for peripheral sensor equipment.
  • FIG. 1 is a schematic diagram of a user motion constraint model established in the present invention
  • FIG. 2 is a flow chart of a method for positioning a Kalman filter based on a highly constrained extension in the present invention
  • FIG. 3 is a diagram showing the natural error of the height-constrained extended Kalman filter positioning method HCEKF and the conventional Kalman filter EKF, the high barometer and the Kalman filter EKF fusion algorithm and the real motion trajectory in the altitude change scene. ;
  • FIG. 4 is a diagram showing a natural error of a height-constrained extended Kalman filter positioning method HCEKF and a conventional Kalman filter EKF, a high barometer and a Kalman filter EKF fusion algorithm and a real motion trajectory in an altitude abrupt scene;
  • a highly constrained condition is established to optimize the traditional extended Kalman filter localization method.
  • the climbing model used is firstly introduced. Taking the car as the target to be positioned, in the actual scene, as shown in Figure 1, the angle between the car and the northeast plane is smaller than a certain angle, which is called the critical angle ⁇ , that is, the plane and northeast of the car. The actual angle to the plane is smaller than the critical angle. In this embodiment, the critical angle is set to 5°.
  • E k and N k respectively represent the eastward and northward coordinates of the target to be located in the kth epoch
  • U k represents the heavenly coordinate of the target to be located in the kth epoch, and the northeastern day of the target to be located.
  • the coordinates must meet the above height constraints.
  • the obtained target position to be located is the coordinate of the geocentric fixed angle coordinate system, which is recorded as (X k [0], X k [1], X k [2]), corresponding
  • the northeast sky is marked as (E k , N k , U k ), where E k , N k and U k represent the east, north and sky coordinates of the state, respectively.
  • the geodetic coordinate system defines an ellipsoid that best matches the geometry of the Earth, called the reference ellipsoid.
  • N is the radius of curvature of the circle of the reference ellipsoid
  • e is the eccentricity of the ellipsoid.
  • the iterative method is generally used to successively approximate and solve the values of ⁇ and h.
  • the calculation process of the iteration is as follows: firstly, the initial value of ⁇ is assumed to be 0, and N, h and ⁇ are sequentially calculated by the formulas (2), (3) and (4), and then the newly obtained ⁇ is brought back into the above three. Then, update the values of N, h and ⁇ again.
  • the calculation can usually be completed after 3 to 4 iterations.
  • a is the long radius of the reference ellipsoid
  • ( ⁇ , ⁇ , ⁇ h) is the difference between the geodetic coordinates of the k-th epoch and the k-1 epoch to be located
  • ( ⁇ e, ⁇ n, ⁇ u) is to be located.
  • the height-constrained extended Kalman filter positioning method in the present invention is specifically as follows: in the satellite positioning navigation, the receiver located on the positioning target receives the satellite signal for positioning, The received satellite signals are processed by data to obtain the satellite ephemeris, pseudorange and Doppler shift measurements required for Kalman positioning.
  • the Kalman localization algorithm generally uses the state equation to estimate the state based on the previous epoch. Predicting the state estimate of the next epoch, the present invention adds a high degree of constraint in this process, and uses the high constraint condition to obtain the pseudorange and Doppler shift measurements obtained by the receiver and the Kalman localization algorithm.
  • the method for positioning the Kalman filter based on the height constraint in the present invention It mainly includes: prediction process, optimal state value estimation process and correction process based on high constraint conditions.
  • Prediction process Use the prediction formula to predict the current position of the receiver and other states.
  • the input variable is the final state estimate of the previous epoch
  • the mean square error P k-1 of the final state estimate, the covariance Q k-1 of the process noise, and the output variable is the state estimate of the predicted current epoch Mean square error matrix of the state estimate
  • the optimal state value estimation process based on the height constraint condition as in the above formulas (2) to (7), the position estimation vector obtained by the prediction process and the position coordinate of the final state estimation value of the previous epoch are converted into the target to be located.
  • the extended Kalman filter follows the principle of minimum mean square error, and the optimal state estimate can be selected from equation (1).
  • formula (11) Representing the state estimate predicted by Kalman filtering, Represents the mean square error of this state estimate.
  • the range interval of the natural coordinate of all the state estimation vectors satisfying the formula (10) is divided into enough equidistant subintervals, and the endpoint values of the interception interval constitute a series of discrete heavenly coordinates. Select among these discrete points The minimum value is taken as the mean square error of the optimal state estimation value, and the corresponding state estimate x is recorded as the optimal state estimation value.
  • the satellite received signal is processed by the receiver to obtain the satellite ephemeris, pseudorange and Doppler shift measurements.
  • the satellite position and velocity obtained by the satellite ephemeris, the Kalman filter predicts the receiver pair.
  • the pseudorange and Doppler shift values of the satellite, and the difference between these predicted values and the measured values of the pseudorange and the Doppler shift form the measurement residual, which is optimal under the constraints obtained by the correction of the actual measured values.
  • State estimate The gain matrix K k of the Kalman filter is derived by using the minimum principle of the diagonal element of the mean squared error matrix P k ⁇ of the state estimate, and the actual state is used to estimate the optimal state. Verification, so that its mean square error value becomes smaller, reliability increases, and further correction results in the final state estimate And the mean squared error matrix P k of the state estimate.
  • C is the matrix of the relationship between the observations and the state of the system
  • R is the covariance matrix of the measurement noise vector
  • y k is the actual observation of the pseudorange and Doppler shift.
  • the actual path test is performed in the scene of the altitude change and the altitude abrupt change respectively, and the experiment adopts the Big Dipper.
  • the baseband and processing modules of the C200 receiver are used to obtain the satellite ephemeris, pseudorange and Doppler shift measurements required for Kalman filter positioning; the actual route is calibrated using NovAtel's SPAN-CPT integrated navigation system.
  • the traditional extended Kalman filter positioning method is described and compared, which is recorded as EKF; the method of fusion of high barometer and Kalman filtering method is recorded as EKF+height barometer; and the height constraint based on the present invention is proposed.
  • Extended Kalman filter positioning method recorded as HCEKF.
  • the height-constrained extended Kalman filter positioning method HCEKF proposed by the present invention is compared with the conventional extended Kalman filter positioning method in the scenario where the altitude of the altitude is slightly changed or abruptly changed.
  • the EKF and the other height barometer fusion methods have relatively stable natural error, and the fluctuations in the whole test period are not very large. Therefore, the HCEKF method based on the highly constrained extended Kalman filter in the present invention can effectively solve the satellite positioning.
  • the problem of drastic changes in the height direction improves the positioning accuracy.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Signal Processing (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Navigation (AREA)

Abstract

一种基于高度约束的扩展卡尔曼滤波定位方法，适用于GNSS导航定位系统，先利用扩展卡尔曼滤波算法根据上一历元的状态估计值获取当前历元的状态估计值，再通过建立高度约束条件来约束当前历元的定位高度得到当前历元符合高度约束条件的最优状态估计值和对应的均方误差，最后再利用均方误差获取伪距和多普勒频移的测量残余对状态估计值进行进一步地校正得到当前历元最终的状态估计值，更准确的获得待定位目标在当前历元的位置信息，提高GNSS导航定位的精度。

Description

一种基于高度约束的扩展卡尔曼滤波定位方法 技术领域
本发明涉及卫星定位领域，尤其涉及一种基于高度约束的扩展卡尔曼滤波定位方法。
背景技术
卡尔曼滤波算法可以对线性高斯白噪声系统的状态进行最优估计，在GNSS定位导航中被广泛使用。卡尔曼滤波算法首先利用状态方程预测接收机当前的位置、速度和钟差等状态，然后根据状态估计值以及卫星星历所提供的卫星位置和速度，预测出各卫星的伪距和多普勒频移值，最后通过处理测量残余得到校正后的最优估计值。卡尔曼滤波算法在每得到一个新的测量值后，就可以对系统状态估计值更新一次，成为提高GNSS导航定位实时性的一种有效手段。
传统的卡尔曼滤波定位方法多采用传感器、气压计等辅助设备使用具有航向约束、速度约束的卡尔曼滤波算法来提高GNSS在高度方向上的定位精度。这些方法均采用外设或需要已知航向、实时速度等信息，增加了卫星定位的成本。
发明内容
发明目的：为了解决卫星定位在高度方向上的剧变问题，本发明提供一种基于高度约束的扩展卡尔曼滤波定位方法，在不借助外界辅助信息和外接传感器设备的基础上，能够提高GNSS导航定位的精度。
技术方案：为了实现上述目的，本发明中基于高度约束的扩展卡尔曼滤波定位方法，包括如下步骤：
(1)利用预测公式根据上一历元的最终状态估计值
Figure PCTCN2016087287-appb-000001
以及最终状态估计值的均方误差Pk-1估计待定位目标的在当前历元的先验状态估计值
Figure PCTCN2016087287-appb-000002
和该先验状态估计值的均方误差
Figure PCTCN2016087287-appb-000003
(2)构建高度约束条件，利用最小均方误差原则获取在高度约束条件下，当前历元的最优状态估计值
Figure PCTCN2016087287-appb-000004
以及最优状态估计值的均方误差
Figure PCTCN2016087287-appb-000005
(3)利用卡尔曼滤波的增益矩阵Kk对所述当前历元的最优状态估计值
Figure PCTCN2016087287-appb-000006
以及最优状态估计值的均方误差
Figure PCTCN2016087287-appb-000007
进行校正，得到当前历元的最终状态估计值
Figure PCTCN2016087287-appb-000008
和该最终状态估计值的均方误差阵Pk。
有益效果：本发明中基于高度约束的扩展卡尔曼滤波定位方法，从应用角度出发， 充分考虑汽车行驶的各种场景，针对各场景均存在临界角度的特点提出爬坡模型无需外设传感器设备，便能够提高卫星定位精度、解决定位高度剧变现象。
附图说明
图1为本发明中建立的用户运动约束模型示意图；
图2为本发明中基于高度约束扩展卡尔曼滤波定位方法的流程图；
图3为本发明中基于高度约束扩展卡尔曼滤波定位方法HCEKF与传统的卡尔曼滤波EKF、高度气压计和卡尔曼滤波EKF融合算法与真实运动轨迹在海拔高度微变场景下的天向误差图；
图4为本发明中基于高度约束扩展卡尔曼滤波定位方法HCEKF与传统的卡尔曼滤波EKF、高度气压计和卡尔曼滤波EKF融合算法与真实运动轨迹在海拔高度陡变场景下的天向误差图；
具体实施方式
下面结合附图进一步解释本发明。
本发明中基于高度约束的扩展卡尔曼滤波定位方法，建立高度约束条件对传统的扩展卡尔曼滤波定位方法进行优化，本实施中先具体介绍所使用的爬坡模型。以汽车作为待定位目标，在实际场景中，如图1所示，汽车在行驶时与东北向平面的夹角均小于某一角度，称该角度为临界角θ，即汽车行驶的平面和东北向平面的实际角度均小于该临界角，本实施例中设置该临界角为5°。
根据三角函数关系，可以得到高度约束条件：
Figure PCTCN2016087287-appb-000009
式(1)中，Ek、Nk分别表示待定位目标在第k历元的东向、北向坐标，Uk表示待定位目标在第k历元的天向坐标，待定位目标的东北天坐标需满足上述高度约束条件。
在扩展卡尔曼滤波定位算法过程中，所得到的待定位目标位置为地心地固直角坐标系坐标，记为(Xk[0]，Xk[1]，Xk[2])，对应的东北天坐标记为(Ek，Nk，Uk)，其中Ek，Nk和Uk分别代表状态的东向、北向和天向坐标。
地心地固直角坐标(Xk[0]，Xk[1]，Xk[2])转化为东北天坐标(Ek，Nk，Uk)，首先需要转化到大地坐标(φ，λ，h)，其变换公式为：
Figure PCTCN2016087287-appb-000010
Figure PCTCN2016087287-appb-000011
Figure PCTCN2016087287-appb-000012
大地坐标系定义一个与地球几何最吻合的椭球体，称为基准椭球体。其中，N为基准椭球体的卯酉圆曲率半径，e为椭球偏心率。
因为h的计算式(3)中含有待求的φ，而φ的计算式(4)反过来又含有待求得h，所以一般借助迭代法来逐次逼近、求解φ和h的值。迭代的计算过程如下：首先假设φ的初始值为0，经过公式(2)、(3)和公式(4)依次计算出N，h和φ，然后再将刚得到的φ重新带入以上三式，再一次更新N，h和φ的值。一般经过3～4次迭代就可以结束计算。
进一步求解待定位目标在东北天向坐标系下的位置：
Δe＝Δλ·a·cosφ  (5)
Δn＝Δφ·a  (6)
Δu＝Δh  (7)
其中，a为基准椭球体的长半径，(Δφ，Δλ，Δh)为待定位目标在第k历元和第k-1历元的大地坐标之差，(Δe，Δn，Δu)为待定位目标在第k历元和第k-1历元的东北天坐标之差。
基于上述高度约束条件，本发明中基于高度约束的扩展卡尔曼滤波定位方法，具体为：如图2所示，在卫星定位导航中，位于带定位目标上的接收机接收卫星信号用于定位，接收到的卫星信号经过数据处理得到卡尔曼定位所需要的卫星星历、伪距和多普勒频移测量值，卡尔曼定位算法一般是利用状态方程根据上一历元的状态估计值，来预测下一历元的状态估计值，本发明在这一过程中增加了高度约束条件，并利用高度约束条件来获取接收机得到的伪距和多普勒频移测量值和卡尔曼定位算法所得到的伪距和多普勒频移预测值之间的测量残余，对卡尔曼定位算法所得到的状态估计进行校正，综上所述，本发明中基于高度约束的扩展卡尔曼滤波定位方法，主要包括：预测过程、基于高度约束条件的最优状态值估计过程和校正过程。
预测过程：利用预测公式预测接收机当前的位置等状态。输入变量为上一历元的最终状态估计值
Figure PCTCN2016087287-appb-000013
最终状态估计值的均方误差Pk-1、过程噪声的协方差Qk-1，输出变量为预测的当前历元的状态估计值
Figure PCTCN2016087287-appb-000014
和该状态估计值的均方误差阵
Figure PCTCN2016087287-appb-000015
Figure PCTCN2016087287-appb-000016
Figure PCTCN2016087287-appb-000017
基于高度约束条件的最优状态值估计过程：如上述公式(2)～(7)，将预测过程所得的状态估计向量和上一历元的最终状态估计值的位置坐标转化为待定位目标在第k历元和第k-1历元的东北天坐标之差(Δe，Δn，Δu)；因此公式(1)可化简为：
Figure PCTCN2016087287-appb-000018
扩展卡尔曼滤波遵从最小均方误差原则，可以从公式(1)中选择最优状态估计值。
Figure PCTCN2016087287-appb-000019
Figure PCTCN2016087287-appb-000020
公式(11)中
Figure PCTCN2016087287-appb-000021
代表卡尔曼滤波所预测的状态估计值，
Figure PCTCN2016087287-appb-000022
代表此状态估计值的均方误差。为求解公式(11)，将满足公式(10)的所有状态估计向量的天向坐标的范围区间分成足够多的等距离的子区间，截取区间的端点值组成一系列的离散的天向坐标，在这些离散点中选取
Figure PCTCN2016087287-appb-000023
的最小值作为最优状态估计值的均方误差，此时所对应的状态估计x记为最优状态估计值
Figure PCTCN2016087287-appb-000024
校正过程：卫星接收信号经过接收机处理后得到卫星星历、伪距和多普勒频移的测量值，通过卫星星历得到的卫星位置与速度，卡尔曼滤波器预测出接收机对各颗卫星的伪距和多普勒频移值，而这些预测值和伪距及多普勒频移的测量值之间的差异形成测量残差，结合实际测量值校正所得到的约束条件下最优的状态估计值。利用状态估计值的均方误差阵Pk ～的对角线元素最小原则推导出卡尔曼滤波的增益矩阵Kk，并利用实际测量值对最优状态估计值
Figure PCTCN2016087287-appb-000025
进行验证，从而使得它的均方误差值变小，可靠性增加，进一步校正得到最终的状态估计值
Figure PCTCN2016087287-appb-000026
和该状态估计值的均方误差阵Pk。
Figure PCTCN2016087287-appb-000027
Figure PCTCN2016087287-appb-000028
Figure PCTCN2016087287-appb-000029
其中，C表示观测量与系统状态之间的关系矩阵，R为测量噪声向量的协方差矩阵，yk为伪距及多普勒频移的实际观测量。
为了验证本发明中基于高度约束的扩展卡尔曼滤波定位方法相对于现有技术的优势，分别在海拔高度微变场景和海拔高度陡变场景下进行实际的路径测试，实验采用北斗星 通C200接收机基带及处理模块得到卡尔曼滤波定位所需要的卫星星历、伪距及多普勒频移测量值；采用NovAtel公司的SPAN-CPT组合导航系统进行实际路线的标定。本实施例中对比分析了传统的扩展卡尔曼滤波定位方法，记为EKF；高度气压计和卡尔曼滤波方法融合的方法，记为EKF+高度气压计；以及本发明中所提出的基于高度约束的扩展卡尔曼滤波定位方法，记为HCEKF。
从图3、4中可以看出，无论是在海拨高度微变还是陡变的场景下，本发明所提出的基于高度约束的扩展卡尔曼滤波定位方法HCEKF相对于传统的扩展卡尔曼滤波定位方法EKF、以及其余高度气压计融合方法的天向误差都比较稳定，在整个测试时间段内的起伏不是很大，因此本发明中基于高度约束的扩展卡尔曼滤波定位方法HCEKF能够有效解决卫星定位在高度方向上的剧变问题，提高定位精度。
以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (5)

  1. 一种基于高度约束的扩展卡尔曼滤波定位方法，其特征在于，包括以下步骤：
    (1)利用预测公式根据上一历元的最终状态估计值
    Figure PCTCN2016087287-appb-100001
    以及最终状态估计值的均方误差Pk-1估计待定位目标的在当前历元的先验状态估计值
    Figure PCTCN2016087287-appb-100002
    和该先验状态估计值的均方误差
    Figure PCTCN2016087287-appb-100003
    (2)构建高度约束条件，利用最小均方误差原则获取在高度约束条件下，当前历元的最优状态估计值
    Figure PCTCN2016087287-appb-100004
    以及最优状态估计值的均方误差
    Figure PCTCN2016087287-appb-100005
    (3)利用卡尔曼滤波的增益矩阵Kk对所述当前历元的最优状态估计值
    Figure PCTCN2016087287-appb-100006
    以及最优状态估计值的均方误差
    Figure PCTCN2016087287-appb-100007
    进行校正，得到当前历元的最终状态估计值
    Figure PCTCN2016087287-appb-100008
    和该最终状态估计值的均方误差阵Pk。
  2. 根据权利要求1所述的基于高度约束的扩展卡尔曼滤波定位方法，其特征在于，步骤(1)中所述预测公式为：
    Figure PCTCN2016087287-appb-100009
    Figure PCTCN2016087287-appb-100010
    式中，A为转移矩阵，Qk-1为上一历元的过程噪声协方差。
  3. 根据权利要求1所述的基于高度约束的扩展卡尔曼滤波定位方法，其特征在于，所述高度约束条件为：
    Figure PCTCN2016087287-appb-100011
    Ek、Nk、Uk分别表示待定位目标在第k历元的东向、北向坐标和天向坐标，Ek-1、Nk-1、Uk-1分别表示待定位目标在第k-1历元的东向、北向坐标和天向坐标。
  4. 根据权利要求3所述的基于高度约束的扩展卡尔曼滤波定位方法，其特征在于，获取在高度约束条件下，当前历元的最优状态估计值
    Figure PCTCN2016087287-appb-100012
    以及最优状态估计值的均方误差Pk ～包括以下步骤：
    (31)将步骤(2)中所得的状态估计值
    Figure PCTCN2016087287-appb-100013
    和上一历元的最终状态估计值
    Figure PCTCN2016087287-appb-100014
    的位置坐标转化为待定位目标在第k历元和第k-1历元的东北天坐标之差(Δe，Δn，Δu)；
    (32)将所述高度约束条件转化简化后的高度约束条件：
    Figure PCTCN2016087287-appb-100015
    (33)将满足所述简化后的高度约束条件的所有状态估计值的天向坐标的范围区间分成若干个等距离的子区间，截取区间的端点值组成一系列的离散的天向坐标，在这些 离散点中获取状态估计值均方误差的最小值记为最优状态估计值的均方误差
    Figure PCTCN2016087287-appb-100016
    此时所对应的状态估计记为最优状态估计
    Figure PCTCN2016087287-appb-100017
  5. 根据权利要求4所述的基于高度约束的扩展卡尔曼滤波定位方法，其特征在于，在所述卡尔曼滤波定位方法中用于直接计算的是地心地固直角坐标系坐标Xk，其位置坐标记为(Xk[0]，Xk[1]，Xk[2])，设其对应的大地坐标为(φ,λ,h)，东北天坐标为(Ek,Nk,Uk)，则将地心地固直角坐标系坐标(Xk[0]，Xk[1]，Xk[2])转化为其对应的东北天坐标(Ek,Nk,Uk)包括以下步骤：
    (1)将地心地固直角坐标系坐标(Xk[0]，Xk[1]，Xk[2])转化到大地坐标(φ,λ,h)，其变换公式为：
    Figure PCTCN2016087287-appb-100018
    Figure PCTCN2016087287-appb-100019
    Figure PCTCN2016087287-appb-100020
    (2)令φ的初始值为0，利用迭代算法计算N，h和φ的值；
    (3)将待定位目标两个历元的大地坐标之差转化为东北天坐标之差：
    Δe＝Δλ·a·cosφ
    Δn＝Δφ·a
    Δu＝Δh
    式中，a为基准椭球体的长半径，(Δφ，Δλ，Δh)为在待定位目标第k历元和第k-1历元的大地坐标之差，(Δe，Δn，Δu)为待定位目标在第k历元和第k-1历元的东北天坐标之差。
PCT/CN2016/087287 2016-06-16 2016-06-27 一种基于高度约束的扩展卡尔曼滤波定位方法 WO2017215026A1 (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
US16/309,939 US10422883B2 (en) 2016-06-16 2016-06-27 Positioning method using height-constraint-based extended Kalman filter

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201610432153.2A CN105891863B (zh) 2016-06-16 2016-06-16 一种基于高度约束的扩展卡尔曼滤波定位方法
CN201610432153.2 2016-06-16

Publications (1)

Publication Number Publication Date
WO2017215026A1 true WO2017215026A1 (zh) 2017-12-21

Family

ID=56731027

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2016/087287 WO2017215026A1 (zh) 2016-06-16 2016-06-27 一种基于高度约束的扩展卡尔曼滤波定位方法

Country Status (3)

Country Link
US (1) US10422883B2 (zh)
CN (1) CN105891863B (zh)
WO (1) WO2017215026A1 (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110568401A (zh) * 2019-09-11 2019-12-13 东北大学 一种基于uwb的三维定位方法
CN110631588A (zh) * 2019-09-23 2019-12-31 电子科技大学 一种基于rbf网络的无人机视觉导航定位方法
CN111765889A (zh) * 2020-04-30 2020-10-13 江苏高科石化股份有限公司 一种生产车间移动机器人基于多胞-椭球双滤波的位姿定位方法
CN112346032A (zh) * 2020-11-10 2021-02-09 中国科学院数学与系统科学研究院 基于一致性扩展卡尔曼滤波的单红外传感器目标定轨方法

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108693773B (zh) * 2018-04-04 2021-03-23 南京天辰礼达电子科技有限公司 一种农业机械自动驾驶滑坡偏差自适应估计方法
CN109696698B (zh) * 2019-03-05 2021-03-12 湖南国科微电子股份有限公司 导航定位预测方法、装置、电子设备及存储介质
CN110006427B (zh) * 2019-05-20 2020-10-27 中国矿业大学 一种低动态高振动环境下的bds/ins紧组合导航方法
CN110793518B (zh) * 2019-11-11 2021-05-11 中国地质大学（北京） 一种海上平台的定位定姿方法及系统
CN112309115B (zh) * 2020-10-27 2022-04-15 华中科技大学 基于多传感器融合的场内外连续位置检测与停车精确定位方法
CN113176596B (zh) * 2021-04-25 2023-07-04 北京眸星科技有限公司 气压高程约束定位方法
CN116125380B (zh) * 2023-04-19 2023-06-30 齐鲁工业大学(山东省科学院) 一种基于卡尔曼滤波器的移动场景超分辨定位方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101561496A (zh) * 2009-05-21 2009-10-21 北京航空航天大学 一种伪卫星和惯性组合导航系统的非线性补偿方法
US20120162006A1 (en) * 2010-12-28 2012-06-28 Stmicroelectronics S.R.L. Cinematic parameter computing method of a satellite navigation system and receiving apparatus employing the method
CN102928858A (zh) * 2012-10-25 2013-02-13 北京理工大学 基于改进扩展卡尔曼滤波的gnss单点动态定位方法
CN104035110A (zh) * 2014-06-30 2014-09-10 北京理工大学 应用于多模卫星导航系统中的快速卡尔曼滤波定位方法
CN104316943A (zh) * 2014-09-22 2015-01-28 广东工业大学 一种伪距离和多普勒组合差分定位系统及方法
CN105510942A (zh) * 2015-12-27 2016-04-20 哈尔滨米米米业科技有限公司 一种基于卡尔曼滤波的gps单点定位系统

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6670916B2 (en) * 2002-02-19 2003-12-30 Seiko Epson Corporation Method and system for position calculation from calculated time
US9268026B2 (en) * 2012-02-17 2016-02-23 Samsung Electronics Co., Ltd. Method and apparatus for location positioning in electronic device
US9360559B2 (en) * 2012-03-21 2016-06-07 Apple Inc. GNSS navigation solution using inequality constraints
CN103744096B (zh) * 2013-12-23 2016-05-18 北京邮电大学 一种多信息融合的定位方法和装置
CN105654053B (zh) * 2015-12-29 2019-01-11 河海大学 基于改进约束ekf算法的动态振荡信号参数辨识方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101561496A (zh) * 2009-05-21 2009-10-21 北京航空航天大学 一种伪卫星和惯性组合导航系统的非线性补偿方法
US20120162006A1 (en) * 2010-12-28 2012-06-28 Stmicroelectronics S.R.L. Cinematic parameter computing method of a satellite navigation system and receiving apparatus employing the method
CN102928858A (zh) * 2012-10-25 2013-02-13 北京理工大学 基于改进扩展卡尔曼滤波的gnss单点动态定位方法
CN104035110A (zh) * 2014-06-30 2014-09-10 北京理工大学 应用于多模卫星导航系统中的快速卡尔曼滤波定位方法
CN104316943A (zh) * 2014-09-22 2015-01-28 广东工业大学 一种伪距离和多普勒组合差分定位系统及方法
CN105510942A (zh) * 2015-12-27 2016-04-20 哈尔滨米米米业科技有限公司 一种基于卡尔曼滤波的gps单点定位系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
TANG, YONGGANG ET AL.: "RDSS/SINS Tightly Coupled Integrated Navigation System for Land Vehicles", ELECTRONICS OPTICS & CONTROL, vol. 14, no. 2, 30 April 2007 (2007-04-30), pages 56, ISSN: 1671-637X *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110568401A (zh) * 2019-09-11 2019-12-13 东北大学 一种基于uwb的三维定位方法
CN110631588A (zh) * 2019-09-23 2019-12-31 电子科技大学 一种基于rbf网络的无人机视觉导航定位方法
CN111765889A (zh) * 2020-04-30 2020-10-13 江苏高科石化股份有限公司 一种生产车间移动机器人基于多胞-椭球双滤波的位姿定位方法
CN111765889B (zh) * 2020-04-30 2024-04-16 江苏高科石化股份有限公司 一种生产车间移动机器人基于多胞-椭球双滤波的位姿定位方法
CN112346032A (zh) * 2020-11-10 2021-02-09 中国科学院数学与系统科学研究院 基于一致性扩展卡尔曼滤波的单红外传感器目标定轨方法
CN112346032B (zh) * 2020-11-10 2023-07-14 中国科学院数学与系统科学研究院 基于一致性扩展卡尔曼滤波的单红外传感器目标定轨方法

Also Published As

Publication number Publication date
CN105891863A (zh) 2016-08-24
CN105891863B (zh) 2018-03-20
US20190146095A1 (en) 2019-05-16
US10422883B2 (en) 2019-09-24

Similar Documents

Publication Publication Date Title
WO2017215026A1 (zh) 一种基于高度约束的扩展卡尔曼滤波定位方法
WO2018014602A1 (zh) 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
Chang et al. Initial alignment for a Doppler velocity log-aided strapdown inertial navigation system with limited information
US9404754B2 (en) Autonomous range-only terrain aided navigation
CN108594272B (zh) 一种基于鲁棒卡尔曼滤波的抗欺骗干扰组合导航方法
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN107561562B (zh) 一种gnss-r遥感中镜面反射点快速确定方法
CN103728647B (zh) 一种基于卫星载波信号调制的弹体滚转角测量方法
CN108594271B (zh) 一种基于复合分层滤波的抗欺骗干扰的组合导航方法
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN104075715A (zh) 一种结合地形和环境特征的水下导航定位方法
CN109407126A (zh) 一种多模接收机联合定位解算的方法
CN107367744B (zh) 基于自适应测量噪声方差估计的星载gps定轨方法
CN103529461A (zh) 一种基于强跟踪滤波和埃尔米特插值法的接收机快速定位方法
WO2013080183A9 (en) A quasi tightly coupled gnss-ins integration process
CN107607971A (zh) 基于gnss共视时间比对算法的时间频率传递方法及接收机
US9391366B2 (en) Method and device for calibrating a receiver
CN110646822B (zh) 一种基于惯导辅助的整周模糊度Kalman滤波算法
CN115616643B (zh) 一种城市区域建模辅助的定位方法
CN109375248A (zh) 一种卡尔曼多模融合定位算法模型及其串行更新的方法
CN105988129A (zh) 一种基于标量估计算法的ins/gnss组合导航方法
CN110146904B (zh) 一种适用于区域电离层tec的精确建模方法
Gong et al. Airborne earth observation positioning and orientation by SINS/GPS integration using CD RTS smoothing
CN110940999A (zh) 一种基于误差模型下的自适应无迹卡尔曼滤波方法
CN116719071A (zh) 一种采用前向紧组合的gnss-ins因子图优化方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 16905140

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 16905140

Country of ref document: EP

Kind code of ref document: A1

122 Ep: pct application non-entry in european phase

Ref document number: 16905140

Country of ref document: EP

Kind code of ref document: A1

32PN Ep: public notification in the ep bulletin as address of the adressee cannot be established

Free format text: NOTING OF LOSS OF RIGHTS PURSUANT TO RULE 112(1) EPC (EPO FORM 1205A DATED 27.06.2019)

122 Ep: pct application non-entry in european phase

Ref document number: 16905140

Country of ref document: EP

Kind code of ref document: A1