CN107894234A - 一种基于双向滤波的监控导航方法 - Google Patents

一种基于双向滤波的监控导航方法 Download PDF

Info

Publication number
CN107894234A
CN107894234A CN201711170397.9A CN201711170397A CN107894234A CN 107894234 A CN107894234 A CN 107894234A CN 201711170397 A CN201711170397 A CN 201711170397A CN 107894234 A CN107894234 A CN 107894234A
Authority
CN
China
Prior art keywords
navigation
monitoring
filtering
state
ckf
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.)
Pending
Application number
CN201711170397.9A
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.)
Harbin Institute of Technology
Original Assignee
Harbin Institute 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 Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN201711170397.9A priority Critical patent/CN107894234A/zh
Publication of CN107894234A publication Critical patent/CN107894234A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明提出了一种基于双向滤波的监控导航方法，本发明将容积卡尔曼滤波(Cubature Kalman Filter，CKF)非线性滤波器和Rauch‑Tung‑Striebel(RTS)后向平滑算法应用于监控导航算法中，首先利用CKF对监控导航系统的状态进行滤波，然后利用当前时刻的滤波结果通过RTS后向平滑算法得到上一时刻监控系统的预测值，随后采用上一时刻的观测值对监控系统进行量测更新，最后再次利用CKF滤波对当前时刻的状态进行估计。本发明可以同时解决监控导航系统的非线性问题和系统噪声不确定性问题，从而提高监控导航系统的导航定位精度。

Description

一种基于双向滤波的监控导航方法
技术领域
本发明设计的是一种基于双向滤波的监控导航方法，更确切地说，是利用改进的双向滤波算法来解决监控导航系统中系统非线性和系统噪声未知的问题，从而提高监控导航系统定位精度的一种导航方法。
背景技术
在多传感器信息融合理论中，利用多个传感器量测中的冗余和互补信息，通过多源信息融合技术可以有效的提升系统精度，因此根据不同传感器的技术特点，对多传感器进行合理配置，实现优势互补，组成监控导航系统，实现对系统状态误差的校正，可以显著提高导航系统的精度。
在监控导航系统中，系统状态误差的估计与校正至关重要。目前，卡尔曼滤波(Kalman filter，KF)是最常用的滤波算法，但是由于在实际应用中真实系统均具有非线性，而KF只能处理线性系统，因此非线性滤波方法的研究成为研究热点之一。
工程上使用最多的非线性滤波算法是扩展卡尔曼滤波(Extended KalmanFilter，EKF)算法和无迹卡尔曼滤波(Unscented Kalman Filter，UKF)。EKF是通过泰勒级数展开，将当前的非线性系统进行线性化；虽然EKF由于其简单方便的优势被广泛使用于很多非线性系统中，但是当系统为强非线性时，很容易产生线性化误差，造成滤波器精度下降。而UKF采用无迹变换(Unscented Transformation，UT)，通过具有不同权值的采样点经非线性函数直接传播来逼近高斯状态分布的均值和方差，避免了局部线性化与雅可比矩阵的计算，因而可以获得更高的滤波精度；然而UKF算法在高维系统中容易出现协方差非正定的情况，进而导致滤波不稳定甚至发散。为克服以上问题，利用相同权值采样点近似系统高斯分布的容积卡尔曼滤波(Cubature Kalman Filter，CKF)由于其估计精度高，不容易发散且计算量小的优点，迅速被应用到各种领域。
然而，无论是KF、EKF、UKF还是CKF算法，均存在这一个较为严重的问题：只有在系统的数学模型精确已知且外部干扰噪声(包括系统噪声和观测噪声)为零均值、方差一定的白噪声时，滤波算法的结果才是最优的。这在实际应用中，由于系统本身元器件的误差漂移、载体机动引起的动态误差以及外部环境不确定因素的影响，我们所能得到的系统模型和系统噪声的统计特性都是基于先验知识的，而这些一般都存在各种各样的误差，很慢满足滤波最优的条件，因此得到的滤波结果会存在较大的状态估计误差，严重的甚至会导致滤波发散。
因此为解决上述问题，本发明提出了一种基于双向滤波的监控导航方法。本发明将CKF非线性滤波器和Rauch-Tung-Striebel(RTS)后向平滑算法应用于监控导航中，首先利用CKF对监控导航系统的状态进行滤波，然后利用当前时刻的滤波结果通过RTS后向平滑算法得到上一时刻监控系统的预测值，随后采用上一时刻的观测值对监控系统进行量测更新，最后再次利用CKF滤波对当前时刻的状态进行估计。本发明可以同时解决监控导航系统的非线性问题和系统噪声不确定性问题，从而提高监控导航系统的导航定位精度。
发明内容
本发明的目的是提供一种基于双向滤波的监控导航方法，来解决监控导航系统中系统非线性和系统噪声未知的问题，从而满足运载体的高精度导航定位精度需求。
本发明的目的是通过以下步骤来实现的：
步骤1：安装监控导航系统的量测传感器以及运载体自身搭载的传感器，并对监控导航系统中的各个传感器进行充分预热，采集多传感器的数据；
步骤2：充分考虑实际系统的非线性特征，建立非线性监控导航系统的非线性状态方程以及量测方程；
步骤3：利用本发明所提的双向滤波监控导航方法对系统的状态量进行估计，从而提高监控导航系统的导航定位精度。
在上述方法的步骤(3)中利用本发明所提的双向滤波监控导航方法对系统的状态量进行估计，具体方法为：
1)首先利用非线性滤波器CKF完成监控导航系统的时间更新和量测更新，对监控导航系统的状态进行估计；
2)然后利用得到的滤波结果和RTS后向平滑算法计算上一时刻监控系统的预测值；
3)随后采用上一时刻的观测值对监控系统进行量测更新；
4)最后再次利用CKF滤波对当前时刻的状态进行估计。
本发明的优势在于：(1)建立监控导航系统的非线性模型并采用非线性滤波器CKF对监控导航系统状态量进行估计，消除了由系统非线性引起的滤波估计误差，提高监控导航系统精度；(2)将CKF滤波与RTS平滑相结合，有效抑制系统噪声特性未知对状态估计的影响，进一步提高监控导航系统的精度和适用性。
附图说明
图1为本发明方法流程示意图；
图2为仿真中运载体的真实运动轨迹示意图；
图3为利用本发明和传统CKF得到的运动轨迹对比；
图4为利用本发明和传统CKF得到的运动速度对比。
具体实施方式
以下结合具体实施案例，对本发明进行详细说明，本发明的流程示意图如图1所示。
1、安装监控导航系统的量测传感器以及运载体自身搭载的传感器，其中自身搭载里程计来测量运载体的运动情况，两个激光雷达作为观测传感器来实时观测运载体的运动，并提供雷达与运载体之间的角度信息；对监控导航系统中的各个传感器进行充分预热，采集各个传感器的数据。
2、充分考虑实际系统的非线性特征，建立非线性监控导航系统的非线性状态方程以及量测方程：
其中f(xk-1)和h(xk)分别为已知的非线性状态和量测函数，系统噪声wk-1～N(0,Q)，观测噪声vk～N(0,R)。系统的状态向量为
X＝[x y vx vy]T
其中，(x,y)是运载体的位置，(vx,vy)是运载体的东向、北向速度。
两观测传感器的位置分别为S1(S1x,S1y)、S2(S2x,S2y)，则观测向量为：
其中[η1 η2]T为观测噪声。
3、利用本发明所提的双向滤波监控导航方法对系统的状态量进行估计，具体方法为：
1)首先利用非线性滤波器CKF完成k时刻监控导航系统的时间更新和量测更新，对监控导航系统的状态进行估计，具体步骤如下
时间更新：
量测更新：
Yi,k|k-1＝h(Xi,k|k-1)
2)然后分解k时刻的状态协方差矩阵并利用得到的k时刻滤波结果和RTS后向平滑算法计算k-1时刻监控系统的预测值；
其中cholupdata是Cholesky一阶更新。
3)随后利用1)中的CKF量测更新和k-1时刻的观测值对k-1时刻监控系统进行量测更新；
4)最后再次利用3)所得k-1时刻的估计值与CKF滤波对当前时刻的状态进行估计。从而最终实现高精度监控导航系统性能。
本发明的效果可以通过如下仿真得到验证：
采用典型的方位跟踪(Bearings Only Tracking，BOT)模型来验证本发明的性能，仿真条件设置如下：
选取运载体的东向、北向位置和速度信息最为系统状态变量Xk＝[x y vx vy]T，则系统状态方程如下所示：
Xk＝FXk-1+wk-1
其中系统状态转移矩阵为：
系统有2个观测传感器，其位置分别为S1(-1,-2)、S2(1,1)，则量测方程为：
其中量测噪声η1＝η2＝0.05。
仿真总时间为100s，采样间隔为0.2s，运载体的真实运动估计如图2所示。根据上述仿真设置，利用本发明所述方法与传统CKF方法得到的载体运动轨迹和速度曲线对比图分别如图3和图4所示。
从图3和图4可以看出，利用传统CKF算法得到的运动轨迹和速度曲线与真值相比均存在较大误差，而利用本发明所述方法得到的运动轨迹和速度曲线与真实曲线更加贴近。为了定量分析误差的大小，分别计算两种方法所得到的定位误差和速度误差的均方根误差(RMSE)值，计算公式如下所示：
其中N为数据总长度。计算结果如表1所示：
表1两种方法得到的定位误差和速度误差的RMSE值对比
由图3、图4和表1可以看出，与传统CKF方法相比，利用本发明所述方法得到的定位误差和速度误差更小，因此利用本发明所述方法可以有效地提高运载体监控导航系统的导航能力。

Claims (2)

1.一种基于非线性容积卡尔曼滤波(Cubature Kalman Filter，CKF)和Rauch-Tung-Striebel(RTS)后向平滑的双向滤波监控导航方法，主要包括以下步骤：
步骤1：安装监控导航系统的量测传感器以及运载体自身搭载的传感器，并对监控导航系统中的各个传感器进行充分预热，采集多传感器的数据；
步骤2：充分考虑实际系统的非线性特征，建立非线性监控导航系统的非线性状态方程以及量测方程；
步骤3：利用本发明所提的双向滤波监控导航方法对系统的状态量进行估计，从而提高监控导航系统的导航定位精度。
2.根据权利要求1步骤3所述的利用本发明所提的双向滤波监控导航方法对系统的状态量进行估计，具体方法为：
1)首先利用非线性滤波器CKF完成监控导航系统的时间更新和量测更新，对监控导航系统的状态进行估计；
2)然后利用得到的滤波结果和RTS后向平滑算法计算上一时刻监控系统的预测值；
3)随后采用上一时刻的观测值对监控系统进行量测更新；
4)最后再次利用CKF滤波对当前时刻的状态进行估计。
CN201711170397.9A 2017-11-22 2017-11-22 一种基于双向滤波的监控导航方法 Pending CN107894234A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711170397.9A CN107894234A (zh) 2017-11-22 2017-11-22 一种基于双向滤波的监控导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711170397.9A CN107894234A (zh) 2017-11-22 2017-11-22 一种基于双向滤波的监控导航方法

Publications (1)

Publication Number Publication Date
CN107894234A true CN107894234A (zh) 2018-04-10

Family

ID=61805700

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711170397.9A Pending CN107894234A (zh) 2017-11-22 2017-11-22 一种基于双向滤波的监控导航方法

Country Status (1)

Country Link
CN (1) CN107894234A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118243106A (zh) * 2024-04-01 2024-06-25 哈尔滨工业大学（威海） 一种基于平方根分解的扩维容积eckf算法的多auv协同导航的滤波方法、系统及存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6356837B1 (en) * 2001-03-19 2002-03-12 Alpine Electronics, Inc. Navigation system
CN103033186A (zh) * 2012-12-30 2013-04-10 东南大学 一种用于水下滑翔器的高精度组合导航定位方法
CN103727941A (zh) * 2014-01-06 2014-04-16 东南大学 基于载体系速度匹配的容积卡尔曼非线性组合导航方法
CN105509739A (zh) * 2016-02-04 2016-04-20 济南大学 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法
CN205384029U (zh) * 2016-02-04 2016-07-13 济南大学 采用固定区间crts平滑的ins/uwb紧组合导航系统
CN106940193A (zh) * 2017-02-13 2017-07-11 哈尔滨工业大学 一种基于Kalman滤波的船舶自适应摇摆标定方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6356837B1 (en) * 2001-03-19 2002-03-12 Alpine Electronics, Inc. Navigation system
CN103033186A (zh) * 2012-12-30 2013-04-10 东南大学 一种用于水下滑翔器的高精度组合导航定位方法
CN103727941A (zh) * 2014-01-06 2014-04-16 东南大学 基于载体系速度匹配的容积卡尔曼非线性组合导航方法
CN105509739A (zh) * 2016-02-04 2016-04-20 济南大学 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法
CN205384029U (zh) * 2016-02-04 2016-07-13 济南大学 采用固定区间crts平滑的ins/uwb紧组合导航系统
CN106940193A (zh) * 2017-02-13 2017-07-11 哈尔滨工业大学 一种基于Kalman滤波的船舶自适应摇摆标定方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
DINGJIE WANG, HANFENG LV, JIE WU: "Augmented Cubature Kalman filter for nonlinear RTK/MIMU integrated navigation with non-additive noise", 《MEASUREMENT》 *
张智等: "后向平滑平方根容积卡尔曼滤波算法及其应用", 《探测与控制学报》 *
燕欢等: "后向平滑容积滤波算法处理与分析组合导航数据", 《导航定位学报》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118243106A (zh) * 2024-04-01 2024-06-25 哈尔滨工业大学（威海） 一种基于平方根分解的扩维容积eckf算法的多auv协同导航的滤波方法、系统及存储介质
CN118243106B (zh) * 2024-04-01 2024-08-27 哈尔滨工业大学（威海） 一种基于平方根分解的扩维容积eckf算法的多auv协同导航的滤波方法、系统及存储介质

Similar Documents

Publication Publication Date Title
CN106885576B (zh) 一种基于多点地形匹配定位的auv航迹偏差估计方法
CN103278813B (zh) 一种基于高阶无迹卡尔曼滤波的状态估计方法
CN107084714B (zh) 一种基于RoboCup3D的多机器人协作目标定位方法
CN106443661A (zh) 基于无迹卡尔曼滤波的机动扩展目标跟踪方法
CN106443622A (zh) 一种基于改进联合概率数据关联的分布式目标跟踪方法
CN103927436A (zh) 一种自适应高阶容积卡尔曼滤波方法
CN104075711B (zh) 一种基于CKF的IMU/Wi‑Fi信号紧组合室内导航方法
CN109507706B (zh) 一种gps信号丢失的预测定位方法
CN103900574A (zh) 一种基于迭代容积卡尔曼滤波姿态估计方法
CN107633256A (zh) 一种多源测距下联合目标定位与传感器配准方法
CN105865452A (zh) 一种基于间接卡尔曼滤波的移动平台位姿估计方法
CN110231620B (zh) 一种噪声相关系统跟踪滤波方法
CN102999696A (zh) 噪声相关系统基于容积信息滤波的纯方位跟踪方法
CN103776449A (zh) 一种提高鲁棒性的动基座初始对准方法
CN109115228A (zh) 一种基于加权最小二乘容积卡尔曼滤波的目标定位方法
CN114236480A (zh) 一种机载平台传感器系统误差配准算法
WO2016165336A1 (zh) 一种导航的方法和终端
CN114139109A (zh) 一种目标跟踪方法、系统、设备、介质及数据处理终端
CN107894234A (zh) 一种基于双向滤波的监控导航方法
CN107966697B (zh) 一种基于渐进无迹卡尔曼的移动目标跟踪方法
CN104614751B (zh) 基于约束信息的目标定位方法
Seo et al. Indoor dead reckoning localization using ultrasonic anemometer with IMU
CN104268597A (zh) 基于ahcif的集中式测量值扩维融合方法
CN104320108A (zh) 基于ahcif的集中式测量值加权融合方法
CN115580261A (zh) 基于鲁棒策略和动态增强ckf结合的状态估计方法

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20180410

RJ01 Rejection of invention patent application after publication