CN113376672A - 基于模糊卡尔曼滤波的北斗伪距定位算法 - Google Patents

基于模糊卡尔曼滤波的北斗伪距定位算法 Download PDF

Info

Publication number
CN113376672A
CN113376672A CN202110628632.2A CN202110628632A CN113376672A CN 113376672 A CN113376672 A CN 113376672A CN 202110628632 A CN202110628632 A CN 202110628632A CN 113376672 A CN113376672 A CN 113376672A
Authority
CN
China
Prior art keywords
kalman filtering
fuzzy
estimation
variance
robust
Prior art date
Legal status (The legal status 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 status listed.)
Withdrawn
Application number
CN202110628632.2A
Other languages
English (en)
Inventor
曹学瑶
胡黄水
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Changchun University of Technology
Original Assignee
Changchun University of Technology
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 Changchun University of Technology filed Critical Changchun University of Technology
Priority to CN202110628632.2A priority Critical patent/CN113376672A/zh
Publication of CN113376672A publication Critical patent/CN113376672A/zh
Withdrawn legal-status Critical Current

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
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Feedback Control In General (AREA)
  • Navigation (AREA)

Abstract

针对最小二乘无法对观测数据中的粗差进行处理以及扩展卡尔曼滤波对初始位置敏感且量测噪声方差不变的问题，提出一种结合抗差M估计和模糊卡尔曼滤波的定位算法来提高北斗导航的伪距定位精度。该算法首先利用抗差M估计对用户端的初始位置进行定位解算，然后将解算出的位置坐标作为模糊卡尔曼滤波的初始值，最后通过模糊推理系统（FIS）得到的修正值在线调整模糊卡尔曼滤波的量测噪声，进而提高北斗导航系统的定位精度。仿真结果表明所提算法相比扩展卡尔曼滤波精度大约提高了60%。

Description

基于模糊卡尔曼滤波的北斗伪距定位算法
技术领域
本发明涉及一种模糊卡尔曼伪距定位算法，特别是一种基于抗差M估计的模糊卡尔曼伪距定位算法。
背景技术
北斗卫星导航系统是我国自主创建研发的卫星导航系统，不仅在交通运输、农业林业、水力监测等领域都得到普遍应用，还可以为用户端提供全天候、全天时的高精度定位服务。但提高北斗定位精度仍然受到很多因素的限制。如：复杂的信道环境，导致信号无法传播；系统误差和随机误差对伪距测量的干扰导致定位精度不高。
目前已经提出大量关于伪距定位的算法，如牛顿迭代法、极大似然估计、最小二乘法、卡尔曼滤波算法等。这些算法在提高北斗导航伪距定位精度上得到了广泛的应用。
发明内容
本发明主要针对解决最小二乘无法对观测数据中的粗差无法进行处理以及卡尔曼滤波对初始位置敏感且量测噪声方差不变的问题，提出一种基于模糊卡尔曼滤波算法进行伪距定位。
本发明一种基于模糊卡尔曼滤波的北斗伪距定位算法算法由两部分构成，即抗差M估计以及模糊卡尔曼滤波。抗差M估计基于Huber影响函数抑制观测值粗差；模糊卡尔曼滤波基于扩展卡尔曼滤波加入模糊控制系统实时调节滤波增益，进而提高定位精度。
所述抗差M估计算法考虑到粗差对定位精度的影响，在最小二乘法的基础上加入Hunber影响函数自动为含有粗差的观测值分配小的加权值，达到抑制粗差的目的。
所述模糊卡尔曼滤波算法考虑到量测噪声具有随机性,但扩展卡尔曼滤波中的量测噪声的方差从始自终都采用初始计算时的先验值,因此引入模糊控制系统调节量测噪声的协方差矩阵来减小滤波发散。
附图说明
图1是模糊逻辑控制器的隶属度函数示意图；
图2是扩展卡尔曼滤波算法在X、Y、Z三个方向的误差对比图；
图3是模糊卡尔曼滤波在X、Y、Z三个方向的误差对比图；
图4是扩展卡尔曼滤波与模糊卡尔曼滤波运动轨迹与接收机真实轨迹对比图。
具体实施方式
下面结合附图对本发明作进一步详细说明，本发明一种基于模糊卡尔曼滤波的北斗伪距定位算法由两部分构成，即抗差M估计以及模糊卡尔曼滤波。抗差M估计基于Huber影响函数抑制观测值粗差；模糊卡尔曼滤波基于扩展卡尔曼滤波加入模糊控制系统实时调节滤波增益，进而提高定位精度。
抗差M估计基于最二乘法实现的。在北斗导航系统中，最小二乘法是伪距定位系统的观测方程。初始值
Figure 455237DEST_PATH_IMAGE001
处线性化得到的系统观测模型为：
Figure 506370DEST_PATH_IMAGE002
，
Figure 166021DEST_PATH_IMAGE003
为m×l的矢量，其表示状态残差，即真实位置与初始化后得到的位置之间的差值， m为当前历元下的卫星个数。
Figure 62433DEST_PATH_IMAGE004
为m×l的矢量，其表示伪距残差即伪距的观测值与估计值之间的差值；
Figure 497832DEST_PATH_IMAGE005
矩阵是一个
Figure 251024DEST_PATH_IMAGE006
的观测矩阵；
Figure 70075DEST_PATH_IMAGE007
表示m×l的误差矢量，其元素一般假设为相互独立的高斯随机过程。首先用最小二乘法解算此线性方程，目标函数为：
Figure 206396DEST_PATH_IMAGE008
（1）
初始值处的最小二乘解和伪距残差矢量为：
Figure 325662DEST_PATH_IMAGE009
当测量噪声为高斯分布时，最小二乘估计为最优估计。但北斗导航系统中观测量中含有粗差会直接影响最小二乘定位精度，因此采用M估计抑制粗差。其目标函数为：
Figure 453018DEST_PATH_IMAGE010
（4）
函数
Figure 789059DEST_PATH_IMAGE011
是可导偶函数。M估计可以按照不同情况将
Figure 89591DEST_PATH_IMAGE011
构造成相应的加权函数；
Figure 797784DEST_PATH_IMAGE012
是
Figure 532259DEST_PATH_IMAGE004
方差的估计值，计算公式如下：
Figure 857061DEST_PATH_IMAGE013
（5）
采用Huber函数给含有粗差的观测值赋予小的权值：
Figure 571070DEST_PATH_IMAGE014
（6）
式中k是根据M估计方差性能确定的常数。抗差M估计的表达式为：
Figure 163464DEST_PATH_IMAGE015
（7）
其中
Figure 694939DEST_PATH_IMAGE016
为等价权矩阵，公式为：
Figure 975879DEST_PATH_IMAGE017
（8）
扩展卡尔曼滤波有两个系统模型：系统观测模型以及系统状态模型。
状态方程：
Figure 821475DEST_PATH_IMAGE018
（9）
观测方程：
Figure 940479DEST_PATH_IMAGE019
（10）
xk和 xk-1分别表示为历元k和历元k-1下的系统状态向量；Ak,k-1表示为历元 k-1到历元k的状态转移矩阵；yk表示为历元k的观测量；Bk表示为状态量和观测量之间的关系矩阵；
Figure 783801DEST_PATH_IMAGE020
和
Figure 316151DEST_PATH_IMAGE021
表示高斯白噪声，均值为零，且相互独立。
扩展卡尔曼滤波包含预测和更新两个过程，其预测方程为：
状态先验估计值
Figure 699859DEST_PATH_IMAGE022
（11）
求解先验估计值的协方差
Figure 565047DEST_PATH_IMAGE023
（12）
Qk是系统噪声序列的方差阵，其更新方程为：
滤波增益矩阵
Figure 376008DEST_PATH_IMAGE024
（13）
Rk为测量噪声序列的方差阵
对状态更新
Figure 130075DEST_PATH_IMAGE025
（14）
对协方差阵更新
Figure 583053DEST_PATH_IMAGE026
（15）
量测噪声具有随机性,但扩展卡尔曼滤波中的量测噪声的方差从始自终都采用初始计算时的先验值,因此采用模糊卡尔曼滤波调整量测噪声的协方差矩阵来减小滤波发散。
新息（rk）是观测量估计值与观测量真实值的差，公式为：
Figure 178114DEST_PATH_IMAGE027
由扩展卡尔曼滤波递推公式可以看出，Kk决定了对新息利用程度。因此当外部量测噪声增大时通过减少Kk来降低对新息使用。而Kk本身由Rk、
Figure 658511DEST_PATH_IMAGE028
决定,则通过可以增加Rk来减少Kk。 tk为测量噪声调整因子。则式改写为
Figure 198077DEST_PATH_IMAGE029
由模糊推理系统（FIS）可以计算出调整因子tk。tk可以对量测噪声方差进行调整。模糊推理系统采用单输入单输出Mamdani模糊逻辑控制器，将每一步的残差实测方差与理论方差的比值作为输入，输出为tk。模糊规则为：
Figure 189167DEST_PATH_IMAGE030
Fk是残差的实测方差阵,它受新息影响公式为：
Figure 701051DEST_PATH_IMAGE031
（20）
N为统计数。Dk是残差的理论方差阵，其公式为:
Figure 290033DEST_PATH_IMAGE032
（21）
Ck是残差的实测方差矩阵和残差的理论方差矩阵的商,其公式为：
Figure 785736DEST_PATH_IMAGE033
（22）
每进行一次滤波FIS更新一次输出值即tk，对量测噪声方差矩阵进行在线调整。
为了验证本发明一种基于模糊卡尔曼滤波的北斗伪距定位算法的精度，使用MATLAB仿真工具，与扩展卡尔曼滤波算法进行比较分析，仿真误差对比结果如表1所述。
表1仿真误差对比结果
表1 误差对比
实验方案 X平均误差/m Y平均误差/m Z平均误差/m
扩展卡尔曼滤波 14.45 27.46 18.72
模糊卡尔曼滤波 -3.96 -17.75 -12.21
仿真结果表明扩展卡尔曼滤波在X、Y、Z 三个方向的平均误差为14.45m、27.46m、18.72m，定位误差较大，滤波不平滑。
仿真结果表明模糊卡尔曼滤波算法的误差更小，滤波结果也更加平滑。模糊卡尔曼滤波的定位解算结果相较于扩展卡尔曼滤波的定位精度有了明显提高，且收敛过程也明显缩短，实现了更加精确的定位。
最后将本发明将扩展卡尔曼滤波运动轨迹、真实轨迹以及模糊卡尔曼滤波进行误差对比。仿真表明扩展卡尔曼滤波与真实值之间存在的误差较大且拟合度不高; 而模糊卡尔曼滤波运动轨迹更加集中而且均匀的分布在真实轨迹四周，拟合程度较好。故模糊卡尔曼滤波对定位误差具有极好的改进。

Claims (1)

1.基于模糊卡尔曼滤波的北斗伪距定位算法，其特征在于：抗差M估计以及模糊卡尔曼滤波两个部分，抗差M估计通过采用Huber函数给含有粗差的观测值分配小的权值，有
Figure 280878DEST_PATH_IMAGE001
其中
Figure 562955DEST_PATH_IMAGE002
函数是可导偶函数，k是根据抗差M估计方差性能确定的常数，并构造等价权函数来抑制粗差对未知参数估值的影响，有
Figure 762730DEST_PATH_IMAGE003
其中
Figure 613006DEST_PATH_IMAGE004
为等价权矩阵；
Figure 298065DEST_PATH_IMAGE005
表示伪距残差矢量；
Figure 288892DEST_PATH_IMAGE006
是
Figure 313480DEST_PATH_IMAGE007
方差的估计值，而模糊卡尔曼滤波算法是基于扩展卡尔曼滤波引入模糊控制系统（FIS)，利用模糊推理系统得到的调整因子tk对量测噪声方差实时调整，进而提高定位精度，因为滤波增益Kk本身由测量噪声序列的方差Rk、先验估计值的协方差
Figure 928132DEST_PATH_IMAGE008
决定,通过可以增加
Figure 333443DEST_PATH_IMAGE009
来减少Kk ，具体步骤为：
Figure 957323DEST_PATH_IMAGE010
其中Bk表示为状态量和观测量之间的关系矩阵，因此当外部量测噪声增大时通过减少Kk来降低对新息使用，进而提高定位精度。
CN202110628632.2A 2021-06-07 2021-06-07 基于模糊卡尔曼滤波的北斗伪距定位算法 Withdrawn CN113376672A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110628632.2A CN113376672A (zh) 2021-06-07 2021-06-07 基于模糊卡尔曼滤波的北斗伪距定位算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110628632.2A CN113376672A (zh) 2021-06-07 2021-06-07 基于模糊卡尔曼滤波的北斗伪距定位算法

Publications (1)

Publication Number Publication Date
CN113376672A true CN113376672A (zh) 2021-09-10

Family

ID=77575944

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110628632.2A Withdrawn CN113376672A (zh) 2021-06-07 2021-06-07 基于模糊卡尔曼滤波的北斗伪距定位算法

Country Status (1)

Country Link
CN (1) CN113376672A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115790669A (zh) * 2023-02-08 2023-03-14 河海大学 基于最大熵平滑变结构滤波的dvl误差自适应标定方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115790669A (zh) * 2023-02-08 2023-03-14 河海大学 基于最大熵平滑变结构滤波的dvl误差自适应标定方法

Similar Documents

Publication Publication Date Title
CN108304612B (zh) 基于噪声补偿的迭代平方根ckf的汽车雷达目标跟踪方法
CN108255791B (zh) 基于分布式传感器一致性的机动目标跟踪方法
CN110061716B (zh) 一种基于最小二乘和多重渐消因子的改进kalman滤波方法
CN106599368A (zh) 基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法
CN111181529B (zh) 一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法
CN107607972A (zh) 一种基于实数编码自适应遗传算法的整周模糊度快速获取方法
CN107515414B (zh) 一种电离层闪烁下基于矢量跟踪的自适应卡尔曼滤波方法
CN113376672A (zh) 基于模糊卡尔曼滤波的北斗伪距定位算法
CN112986906A (zh) 一种半正定规划的rss-toa联合定位方法
CN116520380A (zh) 一种基于自适应卡尔曼滤波的动态目标组合定位方法
CN114567288B (zh) 基于变分贝叶斯的分布协同非线性系统状态估计方法
CN109115228B (zh) 一种基于加权最小二乘容积卡尔曼滤波的目标定位方法
CN110532517A (zh) 基于改进的arukf的燃气管道参数估计方法
CN104407366B (zh) 一种对伪距进行平滑处理的方法
CN116088303B (zh) 一种不确定复杂动态网络状态时变递推估计方法
CN112986977A (zh) 一种克服雷达扩展卡尔曼航迹滤波发散的方法
CN115047505B (zh) 基于载波相位差分辅助的gnss定位方法及导航方法
CN110940999A (zh) 一种基于误差模型下的自适应无迹卡尔曼滤波方法
CN113835064B (zh) 一种协同校正源观测信息的加权多维标度tdoa定位方法
Gao et al. Improved innovation-based adaptive estimation for measurement noise uncertainty in SINS/GNSS integration system
CN112954637B (zh) 一种锚节点位置不确定情况下的目标定位方法
CN113671551B (zh) Rtk定位解算方法
CN110649911B (zh) 一种基于α散度的分布式非线性卡尔曼滤波方法
Ou et al. An improved node localization based on adaptive iterated unscented Kalman filter for WSN
CN110572875A (zh) 一种基于改进的机器学习算法的无线定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
WW01 Invention patent application withdrawn after publication

Application publication date: 20210910

WW01 Invention patent application withdrawn after publication