基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法、设备、介质、产品
阅读说明：本技术 基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法、设备、介质、产品 (Strong tracking Kalman filtering single-point positioning calculation method, equipment, medium and product based on Doppler observation value constraint ) 是由 单弘煜 于 2021-08-24 设计创作，主要内容包括：本发明提供基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法,包括以下步骤：建立标准滤波方程,引入多普勒观测值,多粗差探测,强跟踪调整噪声,高精度定位。本发明涉及一种电子设备、存储介质、程序产品,用于执行基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法。本发明通过引入多普勒观测值,并采用卡尔曼滤波的方式,建立速度与位置的相应关系；采用更为复杂的抗差策略,来应对实际动态环境中的多粗差问题,并且根据实际解算状态,动态调整过程噪声,适用各种载体的动态运动场景,使滤波器更加稳健可靠,最终的定位结果也更为平滑稳定。(The invention provides a strong tracking Kalman filtering single-point positioning calculation method based on Doppler observation value constraint, which comprises the following steps of: and establishing a standard filtering equation, introducing a Doppler observed value, detecting multiple gross errors, strongly tracking and adjusting noise, and positioning with high precision. The invention relates to an electronic device, a storage medium and a program product, which are used for executing a strong tracking Kalman filtering single-point positioning calculation method based on Doppler observation value constraint. According to the method, a Doppler observation value is introduced, and a Kalman filtering mode is adopted to establish a corresponding relation between the speed and the position; the method adopts a more complex robust strategy to solve the problem of multiple gross errors in the actual dynamic environment, dynamically adjusts process noise according to the actual resolving state, is suitable for dynamic motion scenes of various carriers, enables a filter to be more stable and reliable, and enables the final positioning result to be more smooth and stable.)
技术领域
本发明涉及GNSS卫星导航定位技术领域，尤其涉及基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法、设备、介质、产品。
背景技术
单点定位技术是根据接收机的原始观测值及卫星广播星历确定接收机在地球坐标系中的绝对坐标的方法，也称为绝对定位。其优点为只需用一台接收机即可独立定位，实施较为方便自由，数据处理也较为简单，其基本原理为后方交会，具体如下：
伪距观测的观测方程为
式中，为伪距观测值，Xi,Yi,Zi为卫星位置，X,Y,Z为接收机位置，c为光速，为接收机钟差，为卫星钟差，Vion为电离层偏差，Vtrop为对流层偏差。
将上式进行泰勒级数展开并进行线性化可得误差方程：
Vi＝-liVx-miVy-niVz-cVtR+Li
式中，Vi为残余误差，li,mi,ni为三个方向线性化系数，Vx,Vy,Vz为三个方向改正数，Li为常数项，为信号发射时刻卫星位置与信号到达时刻接收机近似位置之间的距离。
以单GPS系统为例，待估参数为三个坐标参数和一个钟差参数，故当接收到的卫星数大于等于4颗时即可进行单点定位解算。
单点定位算法作为最基础的定位算法，其主要依托于伪距的数据质量和卫星的几何分布，伪距精度一般在±3m，故受限于伪距自身的精度，单点定位的精度一般在米级抖动，故在实际行人、单车、车载等大众领域无法获得较为平滑的解算结果，具体问题如下：
1)定位精度米级跳跃，连续性差
优于采用最小二程算法，且伪距的误差本身就在米级，同时考虑其他误差的随机影响，故在进行实时单点定位时，正常情况下可维持在米级精度，但其相邻历元间并无关联性，导致整体轨迹的连续性较差，定位精度在米级抖动，无法满足行人、单车、车载等大众领域需求。
2)观测环境不好时，可靠性差
一般情况下，单点定位的精度可稳定在米级，但是当周边环境出现异常，甚至多粗差时，采用标准单点定位算法将难以达到较好的解算结果，精度会显著下降，可靠性较低，无法满足可靠性要求较高的应用场景。
3)仅采用伪距观测值，忽略多普勒观测值的贡献
标准单点定位仅采用伪距观测值进行最小二程解算，并没有有效利用到可解算出速度信息的多普勒观测值，多普勒观测值的测速精度最高可达厘米级，仅采用伪距观测值，整体方程较为脆弱，当伪距出现问题时无法及时发现，从而最终导致结果的异常。
发明内容
为了克服现有技术的不足，本发明的目的在于提供基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法，解决了单点定位结果精度差，轨迹不连续且可靠性低的问题。
本发明提供基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法，包括以下步骤：
建立标准滤波方程，采用动态卡尔曼滤波算法，建立相邻历元间的位置关联性；
引入多普勒观测值，将多普勒观测值引入所述标准滤波方程，得到引入多普勒观测值后的观测方程；
多粗差探测，采用分段法进行抗差卡尔曼滤波，且对伪距观测值和多普勒观测值分开进行抗差判定；
强跟踪调整噪声，采用强跟踪自适应算法调整过程噪声，当速度较大时，增大当前观测值权重，当速度较小时，降低当前观测值权重；
高精度定位，将原始的伪距观测值和多普勒观测值代入引入多普勒观测值后的观测方程，并通过接收机实际的运动情况对过程噪声进行调整，解算得出最终的坐标位置及速度信息，传递到下一历元，最终得到每个历元的精确位置和速度。
进一步地，所述标准滤波方程包括卡尔曼滤波的状态方程和观测方程；其中，所述状态方程为：
Xk＝Φk，k-1Xk-1Γk，k-1Wk-1
所述观测方程为
Zk＝HkXk+Vk
其中，Xk为k时刻的n维状态矢量，Φk，k-1为从k-1时刻到k时刻的n*n维的一步状态转移矩阵，Xk-1为k-1时刻的n维状态矢量，Γk ，k-1为n*p维的噪声输入矩阵，Wk-1为p维的过程噪声序列；Zk为k时刻的m维观测矢量，Hk为m*n维的观测矩阵，Vk为m维的观测噪声序列。
进一步地，所述引入多普勒观测值后的观测方程为：
其中，Vs为观测矢量，Vs_为第1颗卫星的伪距观测值，Vs_为第1颗卫星的多普勒观测值，l,m,n为方向余弦，lP1,mP1,nP1为第1颗卫星伪距观测值的方向余弦，lD1,mD1,nD1为第1颗卫星多普勒观测值的方向余弦，c为光速，Δt为时间变化量，X,Y,Z为待估位置参数，t为待估钟差参数，为待估的三个方向的速度参数，为待估的钟速参数，l′为残余项，l′s_p1为第1颗卫星伪距观测值的残余项,l′s_D1为第1颗卫星多普勒观测值的残余项。
进一步地，所述多粗差探测步骤中，伪距抗差阈值为3m。
进一步地，所述多粗差探测步骤中，在求取标准化残差时，引入中位数进行计算，具体形式如下：
其中，vi为观测值残差，为标准化残差，amed为中位数，σ为方差。
进一步地，所述多粗差探测步骤中，每次对标准化残差最大的卫星观测值进行处理，再进行迭代处理和多次抗差，若抗差次数大于阈值，则判定当前历元并不可靠，不进行结果输出。
进一步地，所述强跟踪调整噪声步骤中，噪声调整模型为：
其中，QPnoise为伪距过程噪声，QDnoise为多普勒过程噪声，v为速度。
一种电子设备，包括：处理器；
存储器；以及程序，其中所述程序被存储在所述存储器中，并且被配置成由处理器执行，所述程序包括用于执行基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法。
一种计算机程序产品，包括计算机程序/指令，该计算机程序/指令被处理器执行时实现基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法。
相比现有技术，本发明的有益效果在于：
本发明提供基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法，通过引入多普勒观测值，并采用卡尔曼滤波的方式，建立速度与位置的相应关系。并且根据实际解算状态，动态调整过程噪声，使整体解算结果更为平滑，更为可靠。具体如下：
1)提升结果连续性
通过卡尔曼滤波的方式，建立相邻历元的关联性，从而采用较为精准的预报模型结合实际观测值，综合处理，获得连续平滑的定位结果。
2)提升结果可靠性
通过卡尔曼滤波器，对内部异常粗差进行精确判断，且结合速度信息，可准确识别出内部多粗差的问题，从而提升整体结果的可靠性。
3)引入多普勒动态滤波
引入多普勒观测值进行动态滤波，建立速度与位置参数的关系，并根据实际速度信息等，采用强跟踪策略，动态调整内部噪声水平，使其无论在任何速度下都能获得较好的定位结果。
上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，并可依照说明书的内容予以实施，以下以本发明的较佳实施例并配合附图详细说明如后。本发明的
具体实施方式
由以下实施例及其附图详细给出。
附图说明
此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1为本发明的基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法流程图；
图2为本发明实施例的单点定位结果示意图。
具体实施方式
下面，结合附图以及具体实施方式，对本发明做进一步描述，需要说明的是，在不相冲突的前提下，以下描述的各实施例之间或各技术特征之间可以任意组合形成新的实施例。
基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法，建立历元间及速度和位置间的相互关系，采用强跟踪卡尔曼滤波算法进行滤波解算，并考虑多粗差的影响，从而提升定位结果的连续性和可靠性。如图1所示，包括以下步骤：
建立标准滤波方程，采用动态卡尔曼滤波算法，建立相邻历元间的位置关联性。标准滤波方程包括卡尔曼滤波的状态方程和观测方程；其中，状态方程为：
Xk＝Φk，k-1Xk-1+Γk，k-1Wk-1
观测方程为
Zk＝HkXk+Vk
其中，Xk为k时刻的n维状态矢量，Φk，k-1为从k-1时刻到k时刻的n*n维的一步状态转移矩阵，Xk-1为k-1时刻的n维状态矢量，Γk，k-1为n*p维的噪声输入矩阵，Wk-1为p维的过程噪声序列；Zk为k时刻的m维观测矢量，Hk为m*n维的观测矩阵，Vk为m维的观测噪声序列。
动态定位中的采样率一般至少为1s，甚至更高，所以可以看成是常速度模型。本实施例引入多普勒观测值，则待估参数为三个坐标参数X，Y，Z、三个速度参数一个钟差参数t以及一个钟速参数具体如下：
状态方程：
Xk＝Φk，k-1Xk-1+Γk，k-1Wk-1
状态矢量：
状态转移矩阵：
系统过程噪声输入矩阵Γk,k-1默认为单位阵，且会参照强跟踪调整噪声步骤采用强跟踪算法进行动态调整。
引入多普勒观测值，将多普勒观测值引入滤波方程中，从而增加额外观测量，提升滤波器稳定性，且对速度待估参数给予一定的噪声，具有一定的约束作用，使整个滤波器更为稳定可靠。引入多普勒观测值后的观测方程如下：
其中，Vs为观测矢量，Vs_为第1颗卫星的伪距观测值，Vs_为第1颗卫星的多普勒观测值，l,m,n为方向余弦，lP1,mP1,nP1为第1颗卫星伪距观测值的方向余弦，lD1,mD1,nD1为第1颗卫星多普勒观测值的方向余弦，c为光速，Δt为时间变化量，X,Y,Z为待估位置参数，t为待估钟差参数，为待估的三个方向的速度参数，为待估的钟速参数，l′为残余项，l′s_p1为第1颗卫星伪距观测值的残余项,l′s_D1为第1颗卫星多普勒观测值的残余项。
多粗差探测，采用分段法进行抗差卡尔曼滤波，且对伪距观测值和多普勒观测值分开进行抗差判定，伪距本身系统偏差小于3m，因此以3m作为伪距抗差阈值。多普勒观测值优于不同接收机的精度不同，可根据实际情况进行阈值设定。对为了反映所有观测值的整体情况，在求取标准化残差时，引入中位数进行计算，具体形式如下：
其中，vi为观测值残差，为标准化残差，amed为中位数，σ为方差。
每次对标准化残差最大的卫星观测值进行处理，如删除或者给较大的噪声。进而进行迭代处理，进行多次抗差，若抗差次数过多，则判断当前历元并不可靠，不进行结果输出。
强跟踪调整噪声，采用强跟踪自适应算法调整过程噪声，当速度较大时，放大过程噪声，让当前观测值权重更高，可以快速的反映坐标变化；当速度较小时，让当前观测值权重降低，可以防止观测值的异常引起的跳变。同理速度参数也可进行同样的处理，具体模型如下：
其中，QPnoise为伪距过程噪声，QDnoise为多普勒过程噪声，v为速度。该模型建立了速度与过程噪声的相关性，速度越大，过程噪声越大，可适用各种载体的动态应用场景。
高精度定位，将原始的伪距观测值和多普勒观测值代入引入多普勒观测值后的观测方程，并通过接收机实际的运动情况对过程噪声等参数进行自适应的调整。解算得出最终的坐标位置及速度信息等，传递到下一历元，最终得到每个历元的精确位置和速度。采用传统方法获得的单点定位结果如图2中的左侧所示，采用本发明提供的方法获得的单点定位结果如图2中的右侧所示。可以看出本发明的定位结果更为平滑稳定。
本发明提供基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法，通过引入多普勒观测值，并采用卡尔曼滤波的方式，建立速度与位置的相应关系。采用更为复杂的抗差策略，来应对实际动态环境中的多粗差问题，并且根据实际解算状态，动态调整过程噪声，适用各种载体的动态运动场景，使滤波器更加稳健可靠，最终的定位结果也更为平滑稳定。
本发明采用动态卡尔曼滤波算法，建立相邻历元间的位置关联性，从而采用较为精准的预报模型结合实际观测值综合处理，获得连续平滑的定位结果。现有技术一般采用最小二程算法，历元间相互独立，定位精度在米级跳动，整体连续性较差。
本发明采用三段法抗差策略，通过迭代抗差方式可以较为准确的识别多粗差对解算的影响，提升算法可靠性。现有技术一般采用RAIM算法，仅适用于单粗差的剔除，且迭代次数较多，可靠性较低。
本发明引入多普勒观测值，建立位置与速度的相应关系，加强滤波器强度，提升整体算法性能。现有技术一般仅将多普勒作为独立的测速模块，未与定位模块融合，造成信息浪费，从而影响算法性能。
本发明根据实际速度信息和定位精度信息，采用强跟踪自适应策略，滤波器内部可根据实际载体运动情况动态调整内部噪声，使滤波器可以快速适配更多场景，提升动态环境下的适应性。现有技术一般采用固定的过程噪声，没有根据实际情况进行调整，整体滤波器适应性较差。
一种电子设备，包括：处理器；
存储器；以及程序，其中所述程序被存储在所述存储器中，并且被配置成由处理器执行，所述程序包括用于执行基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法。
一种计算机程序产品，包括计算机程序/指令，该计算机程序/指令被处理器执行时实现基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法。
以上，仅为本发明的较佳实施例而已，并非对本发明作任何形式上的限制；凡本行业的普通技术人员均可按说明书附图所示和以上而顺畅地实施本发明；但是,凡熟悉本专业的技术人员在不脱离本发明技术方案范围内，利用以上所揭示的技术内容而做出的些许更动、修饰与演变的等同变化，均为本发明的等效实施例；同时,凡依据本发明的实质技术对以上实施例所作的任何等同变化的更动、修饰与演变等，均仍属于本发明的技术方案的保护范围之内。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：针对无人机探测定位的北斗相参系统及设备