CN111578936A - 基于imm-ukf的惯性/超短基线多参数标定方法 - Google Patents

基于imm-ukf的惯性/超短基线多参数标定方法 Download PDF

Info

Publication number
CN111578936A
CN111578936A CN202010385732.2A CN202010385732A CN111578936A CN 111578936 A CN111578936 A CN 111578936A CN 202010385732 A CN202010385732 A CN 202010385732A CN 111578936 A CN111578936 A CN 111578936A
Authority
CN
China
Prior art keywords
model
vector
time
state
matrix
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.)
Granted
Application number
CN202010385732.2A
Other languages
English (en)
Other versions
CN111578936B (zh
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.)
Southeast University
Original Assignee
Southeast University
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 Southeast University filed Critical Southeast University
Priority to CN202010385732.2A priority Critical patent/CN111578936B/zh
Publication of CN111578936A publication Critical patent/CN111578936A/zh
Application granted granted Critical
Publication of CN111578936B publication Critical patent/CN111578936B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • 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/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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

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

Abstract

本发明公开了一种基于IMM‑UKF的惯性/超短基线多参数标定方法，具体步骤包括：以空间任意点为原点建立投影坐标系，在此基础上建立惯性、超短基线、卫星定位、深度计各传感器位置、姿态间的矢量关系及方位表达，选定9维待估计参数为状态量，构建系统与观测模型；设置IMM中各模型初始概率并计算各滤波器初始状态及协方差矩阵；根据系统及观测模型采用三个UKF分别进行滤波，并用贝叶斯假设检验方法模型更新；根据权重输出交互，输出最终滤波结果。与现有惯性/超短基线标定方法相比，本发明能实现应答器位置、惯性/超短基线杆臂和安装误差角9维参数的同时标定，有效提高复杂环境下惯性/超短基线标定参数的收敛速度、精度及鲁棒性。

Description

基于IMM-UKF的惯性/超短基线多参数标定方法
技术领域
本发明是一种基于IMM-UKF的惯性/超短基线多参数标定方法，属于组合导航标定技术，特别适用于水下组合导航领域。
背景技术
在水下组合导航领域中，SINS(即：捷联惯性导航系统)凭借其高度自主性与抗干扰性应用广泛，但其误差随时间积累。USBL(即：超短基线定位系统)具有简便性、灵活性和高适应性，特别是在GNSS(即：全球导航卫星系统)数据不可用时，可为水下航行器提供绝对位置信息。基于SINS/USBL的组合导航是未来水下组合导航的发展方向之一。在SINS/USBL系统中，安装误差角与杆臂误差会影响定位精度，需对其进行在线标定。现有标定技术采用迭代算法，计算复杂度较高，在复杂水下环境下适应性与鲁棒性较低，且无法同时估计应答器位置、SINS/USBL杆臂、SINS/USBL安装误差角。针对以上情况亟需一种更有效的标定方法。
发明内容
发明目的：为了提高标定效率并增强标定适应性，本发明提出了一种基于IMM-UKF的惯性/超短基线多参数标定方法。
技术方案：为实现本发明的目的，本发明所采用的技术方案是：
一种基于IMM-UKF的惯性/超短基线多参数标定方法，该方法包括如下步骤：
S1：以空间任意点为原点建立投影坐标系，在此基础上建立惯性系统、超短基线定位系统、卫星定位系统、深度计各传感器位置、姿态之间的矢量关系及方位表达，选定9维待估计参数为状态量，构建系统模型及观测模型；
S2：设置IMM中各模型的初始概率并计算各滤波器的初始状态及协方差矩阵；
S3：对步骤S1中构建的系统模型及观测模型采用三个UKF分别进行滤波，并利用贝叶斯假设检验方法进行模型更新；
S4：根据权重进行输出交互过程，输出最终滤波结果。
所述的基于IMM-UKF的惯性/超短基线多参数标定方法，所述步骤S1具体包括以下过程：
S1.1以空间任意点R为原点建立投影坐标系，在此基础上建立惯性系统、超短基线定位系统、卫星定位系统、深度计各传感器位置、姿态之间的矢量关系及方位表达：
Figure BDA0002483855200000021
Figure BDA0002483855200000022
Figure BDA0002483855200000023
其中，
Figure BDA0002483855200000024
为USBL斜率，
Figure BDA0002483855200000025
分别为n0系下点R到应答器的矢量在x，y，z方向分量，
Figure BDA0002483855200000026
为n0系下点R到SINS/GNSS的矢量，Cij为
Figure BDA0002483855200000027
矩阵i行j列元素，
Figure BDA0002483855200000028
Figure BDA0002483855200000029
分别为b系下SINS/GNSS与USBL的杆臂误差在x，y，z方向分量，υ1，υ2，υ3为对应量测噪声，
Figure BDA00024838552000000210
为n0系到b系的旋转矩阵，
Figure BDA00024838552000000211
为USBL坐标系下USBL到应答器的矢量，φ为失准角，(×)为叉乘，
Figure BDA00024838552000000212
为b系下SINS/GNSS与USBL的杆臂误差，
Figure BDA00024838552000000213
为n0系下点R到应答器的矢量，
Figure BDA00024838552000000214
为深度计提供的高度信息，hSINS/GNSS为SINS/GNSS高度信息；
S1.2选定9维待估计参数为状态量，构建系统模型与观测模型：
选定状态向量为：
Figure BDA00024838552000000215
其中，Xk为当前时刻状态向量，
Figure BDA00024838552000000216
分别为n0系下点R到应答器的矢量在x，y，z方向分量，
Figure BDA00024838552000000217
分别为b系下SINS/GNSS与USBL的杆臂误差在x，y，z方向分量，φx，φy，φz分别为x，y，z方向的失准角；
建立系统模型：
Xk＝Xk-1+Wk-1
其中，Xk为当前时刻状态向量，Xk-1为上一时刻状态向量，Wk-1为过程噪声；
选定USBL的斜率、倾斜角和来自深度计的高度信息为观测向量，建立观测模型：
Figure BDA00024838552000000218
其中，Zk为量测向量，
Figure BDA00024838552000000219
为USBL斜率，
Figure BDA00024838552000000220
为n0系到b系的旋转矩阵，
Figure BDA00024838552000000221
为n0系下点R到SINS/GNSS的矢量，
Figure BDA00024838552000000222
为USBL坐标系下USBL到应答器的矢量，
Figure BDA00024838552000000223
为深度计提供的高度信息，hSINS/GNSS为SINS/GNSS高度信息，Hk为量测函数，Xk为当前时刻状态向量，Vk为量测噪声。
所述的基于IMM-UKF的惯性/超短基线多参数标定方法，所述步骤S2具体包括以下过程：
S2.1根据模型转换概率遵循马尔可夫过程，求模型预测概率：
Figure BDA0002483855200000031
其中，μi→j(k-1)为模型预测概率，Pi→j为P{mj(k)|mi(k-1)}的简写，代表从k-1时刻到k时刻，模型mi(k-1)到模型mj(k)的马尔可夫转移概率，一般采用预先设置的常数，μi(k-1)为模型mi(k-1)在k-1时刻的模型匹配概率；
S2.2给出各滤波器的初始状态及协方差矩阵：
Figure BDA0002483855200000032
Figure BDA0002483855200000033
Figure BDA0002483855200000034
其中，
Figure BDA0002483855200000035
为k-1时刻第j滤波器的初始状态，POj(k-1)为k-1时刻第j滤波器的初始协方差矩阵，
Figure BDA0002483855200000036
为k-1时刻第i滤波器的状态估计，Pi(k-1)为k-1时刻第i滤波器的协方差矩阵，μi→j(k-1)为模型预测概率。
所述的基于IMM-UKF的惯性/超短基线多参数标定方法，所述步骤S3具体包括以下过程：
S3.1离散化状态方程：
Xj(k)＝Xj(k-1)+Wj(k-1)
其中，Xj(k)为k时刻第j滤波器的状态向量，Xj(k-1)为k-1时刻第j滤波器的状态向量，Wj(k-1)为k-1时刻第j滤波器的过程噪声；
S3.2选k-1时刻的sigma点：
Figure BDA0002483855200000037
Figure BDA0002483855200000038
λ＝a2(n+κ)-n
其中，
Figure BDA0002483855200000041
为k-1时刻第j滤波器的第i个sigma点，
Figure BDA0002483855200000042
为k-1时刻第j滤波器的初始状态，POj(k-1)为k-1时刻第j滤波器的初始协方差矩阵，(·)(i)为矩阵平方根的第i列，γ为常量，n为状态向量维数，λ为计算参数，a为设定正数小量，κ＝3-n；
S3.3预测sigma点样本均值与协方差：
Figure BDA0002483855200000043
Figure BDA0002483855200000044
Figure BDA0002483855200000045
Figure BDA0002483855200000046
Figure BDA0002483855200000047
Wi (m)＝Wi (c)＝1/2(n+λ)i＝0,1,2,...,2n
其中，
Figure BDA0002483855200000048
为一步更新后的sigma点，
Figure BDA0002483855200000049
为k-1时刻第j滤波器的第i个sigma点，
Figure BDA00024838552000000410
为预测均值，Pj(k/k-1)为预测协方差矩阵，Qj(k-1)为过程噪声方差矩阵，在参数估计中为小量，Wi (m)和Wi (c)为权值，n为状态向量维数，λ为计算参数，a为设定正数小量，β＝2；
S3.4预测sigma点的状态估计：
Figure BDA00024838552000000411
其中，
Figure BDA00024838552000000412
为sigma点的状态估计，
Figure BDA00024838552000000413
为预测均值，γ为常量，Pj(k/k-1)为预测协方差矩阵；
S3.5根据状态量测互协方差矩阵与预测量测协方差矩阵计算增益矩阵：
Figure BDA00024838552000000414
Figure BDA00024838552000000415
Figure BDA0002483855200000051
Figure BDA0002483855200000052
Figure BDA0002483855200000053
其中，Kj(k)为增益矩阵，P(xz)j(k,k-1)为状态量测互协方差矩阵，P(zz)j(k,k-1)为量测方差矩阵，Wi (c)为权值，
Figure BDA0002483855200000054
为sigma点的状态估计，
Figure BDA0002483855200000055
为预测均值，
Figure BDA0002483855200000056
为sigma点的量测估计，
Figure BDA0002483855200000057
为量测均值，Rj(k)为量测噪声方差矩阵，Hk为量测函数，Wi (m)为权值；
S3.6计算滤波结果与估计协方差矩阵：
Figure BDA0002483855200000058
Figure BDA0002483855200000059
其中，
Figure BDA00024838552000000510
为k时刻第j滤波器的状态估计，
Figure BDA00024838552000000511
为预测均值，Kj(k)为增益矩阵，Zj(k)为k时刻第j滤波器的量测向量，
Figure BDA00024838552000000512
为量测均值，Pj(k)为第j滤波器协方差矩阵，Pj(k/k-1)为预测协方差矩阵，P(zz)j(k,k-1)为量测方差矩阵；
S3.7采用贝叶斯假设检验方法进行模型更新：
计算模型的似然函数：
Figure BDA00024838552000000513
其中，fj(k)为似然函数，
Figure BDA00024838552000000514
为sigma点的量测估计，
Figure BDA00024838552000000515
为量测均值，P(zz)j(k,k-1)为量测方差矩阵，m为量测向量的维数；
并计算模型在模型集中的权重，进行模型概率更新：
Figure BDA00024838552000000516
其中，μj(k)为第j滤波器的模型概率，fj(k)为似然函数，Pi→j为P{mj(k)|mi(k-1)}的简写，代表从k-1时刻到k时刻，模型mi(k-1)到模型mj(k)的马尔可夫转移概率，一般采用预先设置的常数，μi(k-1)为模型mi(k-1)在k-1时刻的模型匹配概率。
所述的基于IMM-UKF的惯性/超短基线多参数标定方法，所述步骤S4具体包括以下过程：
根据权重，将各模型的估计值进行加权融合，计算联合状态估计与协方差矩阵，输出交互式多模型最终结果：
Figure BDA0002483855200000061
Figure BDA0002483855200000062
其中，
Figure BDA0002483855200000063
为联合状态估计，
Figure BDA0002483855200000064
为k时刻第j滤波器的状态估计，μj(k)为第j滤波器的模型概率，Pk为联合协方差矩阵，Pj(k)为第j滤波器协方差矩阵。
有益效果：
1、本发明采用五维观测方程同时估计应答器位置、SINS/USBL杆臂、SINS/USBL安装误差角，提高了估计效率；
2、本发明采用IMM-UKF算法，涵盖复杂水下环境下的观测值统计特性变化，增强了标定鲁棒性。
附图说明
图1为本发明的基于IMM-UKF的SINS/USBL多参数标定方法流程图；
图2为本发明的校正机制示意图。
具体实施方式
下面结合附图，对本发明的技术方案作进一步说明。应当了解，以下提供的实施例仅是为了详尽地且完全地公开本发明，并且向所属技术领域的技术人员充分传达本发明的技术构思，本发明还可以用许多不同的形式来实施，并且不局限于此处描述的实施例。对于表示在附图中的示例性实施方式中的术语并不是对本发明的限定。
本发明提供的一种基于IMM-UKF的惯性/超短基线多参数标定方法，实现原理如图1所示。
其流程主要包括以下步骤：
步骤S1，以空间任意点为原点建立投影坐标系，在此基础上建立惯性系统、超短基线定位系统、卫星定位系统、深度计各传感器位置、姿态之间的矢量关系及方位表达，选定9维待估计参数为状态量，构建系统模型及观测模型。
具体包括以下过程：
S1.1以空间任意点R为原点建立投影坐标系，在此基础上建立惯性系统、超短基线定位系统、卫星定位系统、深度计各传感器位置、姿态之间的矢量关系及方位表达：
Figure BDA0002483855200000071
Figure BDA0002483855200000072
Figure BDA0002483855200000073
其中，
Figure BDA0002483855200000074
为USBL斜率，
Figure BDA0002483855200000075
分别为n0系下点R到应答器的矢量在x，y，z方向分量，
Figure BDA0002483855200000076
为n0系下点R到SINS/GNSS的矢量，Cij为
Figure BDA0002483855200000077
矩阵i行j列元素，
Figure BDA0002483855200000078
Figure BDA0002483855200000079
分别为b系下SINS/GNSS与USBL的杆臂误差在x，y，z方向分量，υ1，υ2，υ3为对应量测噪声，
Figure BDA00024838552000000710
为n0系到b系的旋转矩阵，
Figure BDA00024838552000000711
为USBL坐标系下USBL到应答器的矢量，φ为失准角，(×)为叉乘，
Figure BDA00024838552000000712
为b系下SINS/GNSS与USBL的杆臂误差，
Figure BDA00024838552000000713
为n0系下点R到应答器的矢量，
Figure BDA00024838552000000714
为深度计提供的高度信息，hSINS/GNSS为SINS/GNSS高度信息；
S1.2选定9维待估计参数为状态量，构建系统模型与观测模型：
选定状态向量为：
Figure BDA00024838552000000715
其中，Xk为当前时刻状态向量，
Figure BDA00024838552000000716
分别为n0系下点R到应答器的矢量在x，y，z方向分量，
Figure BDA00024838552000000717
分别为b系下SINS/GNSS与USBL的杆臂误差在x，y，z方向分量，φx，φy，φz分别为x，y，z方向的失准角；
建立系统模型：
Xk＝Xk-1+Wk-1
其中，Xk为当前时刻状态向量，Xk-1为上一时刻状态向量，Wk-1为过程噪声；
选定USBL的斜率、倾斜角和来自深度计的高度信息为观测向量，建立观测模型：
Figure BDA00024838552000000718
其中，Zk为量测向量，
Figure BDA00024838552000000719
为USBL斜率，
Figure BDA00024838552000000720
为n0系到b系的旋转矩阵，
Figure BDA00024838552000000721
为n0系下点R到SINS/GNSS的矢量，
Figure BDA00024838552000000722
为USBL坐标系下USBL到应答器的矢量，
Figure BDA00024838552000000723
为深度计提供的高度信息，hSINS/GNSS为SINS/GNSS高度信息，Hk为量测函数，Xk为当前时刻状态向量，Vk为量测噪声。
步骤S2，设置IMM中各模型的初始概率并计算各滤波器的初始状态及协方差矩阵。
具体包括以下过程：
S2.1根据模型转换概率遵循马尔可夫过程，求模型预测概率：
Figure BDA0002483855200000081
其中，μi→j(k-1)为模型预测概率，Pi→j为P{mj(k)|mi(k-1)}的简写，代表从k-1时刻到k时刻，模型mi(k-1)到模型mj(k)的马尔可夫转移概率，一般采用预先设置的常数，μi(k-1)为模型mi(k-1)在k-1时刻的模型匹配概率；
S2.2给出各滤波器的初始状态及协方差矩阵：
Figure BDA0002483855200000082
Figure BDA0002483855200000083
Figure BDA0002483855200000084
其中，
Figure BDA0002483855200000085
为k-1时刻第j滤波器的初始状态，POj(k-1)为k-1时刻第j滤波器的初始协方差矩阵，
Figure BDA0002483855200000086
为k-1时刻第i滤波器的状态估计，Pi(k-1)为k-1时刻第i滤波器的协方差矩阵，μi→j(k-1)为模型预测概率。
步骤S3，对步骤S1中构建的系统模型及观测模型采用三个UKF分别进行滤波，并利用贝叶斯假设检验方法进行模型更新。
具体包括以下过程：
S3.1离散化状态方程：
Xj(k)＝Xj(k-1)+Wj(k-1)
其中，Xj(k)为k时刻第j滤波器的状态向量，Xj(k-1)为k-1时刻第j滤波器的状态向量，Wj(k-1)为k-1时刻第j滤波器的过程噪声；
S3.2选k-1时刻的sigma点：
Figure BDA0002483855200000091
Figure BDA0002483855200000092
λ＝a2(n+κ)-n
其中，
Figure BDA0002483855200000093
为k-1时刻第j滤波器的第i个sigma点，
Figure BDA0002483855200000094
为k-1时刻第j滤波器的初始状态，POj(k-1)为k-1时刻第j滤波器的初始协方差矩阵，(·)(i)为矩阵平方根的第i列，γ为常量，n为状态向量维数，λ为计算参数，a为设定正数小量，κ＝3-n；
S3.3预测sigma点样本均值与协方差：
Figure BDA0002483855200000095
Figure BDA0002483855200000096
Figure BDA0002483855200000097
Figure BDA0002483855200000098
Figure BDA0002483855200000099
Wi (m)＝Wi (c)＝1/2(n+λ)i＝0,1,2,...,2n
其中，
Figure BDA00024838552000000910
为一步更新后的sigma点，
Figure BDA00024838552000000911
为k-1时刻第j滤波器的第i个sigma点，
Figure BDA00024838552000000912
为预测均值，Pj(k/k-1)为预测协方差矩阵，Qj(k-1)为过程噪声方差矩阵，在参数估计中为小量，Wi (m)和Wi (c)为权值，n为状态向量维数，λ为计算参数，a为设定正数小量，β＝2；
S3.4预测sigma点的状态估计：
Figure BDA00024838552000000913
其中，
Figure BDA00024838552000000914
为sigma点的状态估计，
Figure BDA00024838552000000915
为预测均值，γ为常量，Pj(k/k-1)为预测协方差矩阵；
S3.5根据状态量测互协方差矩阵与预测量测协方差矩阵计算增益矩阵：
Figure BDA0002483855200000101
Figure BDA0002483855200000102
Figure BDA0002483855200000103
Figure BDA0002483855200000104
Figure BDA0002483855200000105
其中，Kj(k)为增益矩阵，P(xz)j(k,k-1)为状态量测互协方差矩阵，P(zz)j(k,k-1)为量测方差矩阵，Wi (c)为权值，
Figure BDA0002483855200000106
为sigma点的状态估计，
Figure BDA0002483855200000107
为预测均值，
Figure BDA0002483855200000108
为sigma点的量测估计，
Figure BDA0002483855200000109
为量测均值，Rj(k)为量测噪声方差矩阵，Hk为量测函数，Wi (m)为权值；
S3.6计算滤波结果与估计协方差矩阵：
Figure BDA00024838552000001010
Figure BDA00024838552000001011
其中，
Figure BDA00024838552000001012
为k时刻第j滤波器的状态估计，
Figure BDA00024838552000001013
为预测均值，Kj(k)为增益矩阵，Zj(k)为k时刻第j滤波器的量测向量，
Figure BDA00024838552000001014
为量测均值，Pj(k)为第j滤波器协方差矩阵，Pj(k/k-1)为预测协方差矩阵，P(zz)j(k,k-1)为量测方差矩阵；
S3.7采用贝叶斯假设检验方法进行模型更新：
计算模型的似然函数：
Figure BDA00024838552000001015
其中，fj(k)为似然函数，
Figure BDA00024838552000001016
为sigma点的量测估计，
Figure BDA00024838552000001017
为量测均值，P(zz)j(k,k-1)为量测方差矩阵，m为量测向量的维数；
并计算模型在模型集中的权重，进行模型概率更新：
Figure BDA00024838552000001018
其中，μj(k)为第j滤波器的模型概率，fj(k)为似然函数，Pi→j为P{mj(k)|mi(k-1)}的简写，代表从k-1时刻到k时刻，模型mi(k-1)到模型mj(k)的马尔可夫转移概率，一般采用预先设置的常数，μi(k-1)为模型mi(k-1)在k-1时刻的模型匹配概率。
步骤S4，根据权重进行输出交互过程，输出最终滤波结果。
具体包括以下过程：
根据权重，将各模型的估计值进行加权融合，计算联合状态估计与协方差矩阵，输出交互式多模型最终结果：
Figure BDA0002483855200000111
Figure BDA0002483855200000112
其中，
Figure BDA0002483855200000113
为联合状态估计，
Figure BDA0002483855200000114
为k时刻第j滤波器的状态估计，μj(k)为第j滤波器的模型概率，Pk为联合协方差矩阵，Pj(k)为第j滤波器协方差矩阵。
本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (5)

1.一种基于IMM-UKF的惯性/超短基线多参数标定方法，其特征在于，包括以下步骤：
S1：以空间任意点为原点建立投影坐标系，在此基础上建立惯性系统、超短基线定位系统、卫星定位系统、深度计各传感器位置、姿态之间的矢量关系及方位表达，选定9维待估计参数为状态量，构建系统模型及观测模型；
S2：设置IMM中各模型的初始概率并计算各滤波器的初始状态及协方差矩阵；
S3：对步骤S1中构建的系统模型及观测模型采用三个UKF分别进行滤波，并利用贝叶斯假设检验方法进行模型更新；
S4：根据权重进行输出交互过程，输出最终滤波结果。
2.根据权利要求1所述的基于IMM-UKF的惯性/超短基线多参数标定方法，其特征在于，所述步骤S1具体包括以下过程：
S1.1以空间任意点R为原点建立投影坐标系，在此基础上建立惯性系统、超短基线定位系统、卫星定位系统、深度计各传感器位置、姿态之间的矢量关系及方位表达：
Figure FDA0002483855190000011
Figure FDA0002483855190000012
其中，
Figure FDA0002483855190000013
为USBL斜率，
Figure FDA0002483855190000014
分别为n0系下点R到应答器的矢量在x，y，z方向分量，
Figure FDA0002483855190000015
为n0系下点R到SINS/GNSS的矢量，Cij为
Figure FDA0002483855190000016
矩阵i行j列元素，
Figure FDA0002483855190000017
Figure FDA0002483855190000018
分别为b系下SINS/GNSS与USBL的杆臂误差在x，y，z方向分量，υ1，υ2，υ3为对应量测噪声，
Figure FDA0002483855190000019
为n0系到b系的旋转矩阵，
Figure FDA00024838551900000110
为USBL坐标系下USBL到应答器的矢量，φ为失准角，(×)为叉乘，
Figure FDA00024838551900000111
为b系下SINS/GNSS与USBL的杆臂误差，
Figure FDA00024838551900000112
为n0系下点R到应答器的矢量，
Figure FDA00024838551900000113
为深度计提供的高度信息，hSINS/GNSS为SINS/GNSS高度信息；
S1.2选定9维待估计参数为状态量，构建系统模型与观测模型：
选定状态向量为：
Figure FDA00024838551900000114
其中，Xk为当前时刻状态向量，
Figure FDA0002483855190000021
分别为n0系下点R到应答器的矢量在x，y，z方向分量，
Figure FDA0002483855190000022
分别为b系下SINS/GNSS与USBL的杆臂误差在x，y，z方向分量，φx，φy，φz分别为x，y，z方向的失准角；
建立系统模型：
Xk＝Xk-1+Wk-1
其中，Xk为当前时刻状态向量，Xk-1为上一时刻状态向量，Wk-1为过程噪声；
选定USBL的斜率、倾斜角和来自深度计的高度信息为观测向量，建立观测模型：
Figure FDA0002483855190000023
其中，Zk为量测向量，
Figure FDA0002483855190000024
为USBL斜率，
Figure FDA0002483855190000025
为n0系到b系的旋转矩阵，
Figure FDA0002483855190000026
为n0系下点R到SINS/GNSS的矢量，
Figure FDA0002483855190000027
为USBL坐标系下USBL到应答器的矢量，
Figure FDA0002483855190000028
为深度计提供的高度信息，hSINS/GNSS为SINS/GNSS高度信息，Hk为量测函数，Xk为当前时刻状态向量，Vk为量测噪声。
3.根据权利要求2所述的基于IMM-UKF的惯性/超短基线多参数标定方法，其特征在于，所述步骤S2具体包括以下过程：
S2.1根据模型转换概率遵循马尔可夫过程，求模型预测概率：
Figure FDA0002483855190000029
其中，μi→j(k-1)为模型预测概率，Pi→j为P{mj(k)|mi(k-1)}的简写，代表从k-1时刻到k时刻，模型mi(k-1)到模型mj(k)的马尔可夫转移概率，一般采用预先设置的常数，μi(k-1)为模型mi(k-1)在k-1时刻的模型匹配概率；
S2.2给出各滤波器的初始状态及协方差矩阵：
Figure FDA00024838551900000210
Figure FDA0002483855190000031
Figure FDA0002483855190000032
其中，
Figure FDA0002483855190000033
为k-1时刻第j滤波器的初始状态，POj(k-1)为k-1时刻第j滤波器的初始协方差矩阵，
Figure FDA0002483855190000034
为k-1时刻第i滤波器的状态估计，Pi(k-1)为k-1时刻第i滤波器的协方差矩阵，μi→j(k-1)为模型预测概率。
4.根据权利要求2所述的基于IMM-UKF的惯性/超短基线多参数标定方法，其特征在于，所述步骤S3具体包括以下过程：
S3.1离散化状态方程：
Xj(k)＝Xj(k-1)+Wj(k-1)
其中，Xj(k)为k时刻第j滤波器的状态向量，Xj(k-1)为k-1时刻第j滤波器的状态向量，Wj(k-1)为k-1时刻第j滤波器的过程噪声；
S3.2选k-1时刻的sigma点：
Figure FDA0002483855190000035
Figure FDA0002483855190000036
λ＝a2(n+κ)-n
其中，
Figure FDA0002483855190000037
为k-1时刻第j滤波器的第i个sigma点，
Figure FDA0002483855190000038
为k-1时刻第j滤波器的初始状态，POj(k-1)为k-1时刻第j滤波器的初始协方差矩阵，(·)(i)为矩阵平方根的第i列，γ为常量，n为状态向量维数，λ为计算参数，a为设定正数小量，κ＝3-n；
S3.3预测sigma点样本均值与协方差：
Figure FDA0002483855190000039
Figure FDA00024838551900000310
Figure FDA0002483855190000041
Figure FDA0002483855190000042
Figure FDA0002483855190000043
Wi (m)＝Wi (c)＝1/2(n+λ) i＝0,1,2,...,2n
其中，
Figure FDA0002483855190000044
为一步更新后的sigma点，
Figure FDA0002483855190000045
为k-1时刻第j滤波器的第i个sigma点，
Figure FDA0002483855190000046
为预测均值，Pj(k/k-1)为预测协方差矩阵，Qj(k-1)为过程噪声方差矩阵，在参数估计中为小量，Wi (m)和Wi (c)为权值，n为状态向量维数，λ为计算参数，a为设定正数小量，β＝2；
S3.4预测sigma点的状态估计：
Figure FDA0002483855190000047
其中，
Figure FDA0002483855190000048
为sigma点的状态估计，
Figure FDA0002483855190000049
为预测均值，γ为常量，Pj(k/k-1)为预测协方差矩阵；
S3.5根据状态量测互协方差矩阵与预测量测协方差矩阵计算增益矩阵：
Figure FDA00024838551900000410
Figure FDA00024838551900000411
Figure FDA00024838551900000412
Figure FDA00024838551900000413
Figure FDA00024838551900000414
其中，Kj(k)为增益矩阵，P(xz)j(k,k-1)为状态量测互协方差矩阵，P(zz)j(k,k-1)为量测方差矩阵，Wi (c)为权值，
Figure FDA00024838551900000415
为sigma点的状态估计，
Figure FDA00024838551900000416
为预测均值，
Figure FDA00024838551900000417
为sigma点的量测估计，
Figure FDA0002483855190000051
为量测均值，Rj(k)为量测噪声方差矩阵，Hk为量测函数，Wi (m)为权值；
S3.6计算滤波结果与估计协方差矩阵：
Figure FDA0002483855190000052
Figure FDA0002483855190000053
其中，
Figure FDA0002483855190000054
为k时刻第j滤波器的状态估计，
Figure FDA0002483855190000055
为预测均值，Kj(k)为增益矩阵，Zj(k)为k时刻第j滤波器的量测向量，
Figure FDA0002483855190000056
为量测均值，Pj(k)为第j滤波器协方差矩阵，Pj(k/k-1)为预测协方差矩阵，P(zz)j(k,k-1)为量测方差矩阵；
S3.7采用贝叶斯假设检验方法进行模型更新：
计算模型的似然函数：
Figure FDA0002483855190000057
其中，fj(k)为似然函数，
Figure FDA0002483855190000058
为sigma点的量测估计，
Figure FDA0002483855190000059
为量测均值，P(zz)j(k,k-1)为量测方差矩阵，m为量测向量的维数；
并计算模型在模型集中的权重，进行模型概率更新：
Figure FDA00024838551900000510
其中，μj(k)为第j滤波器的模型概率，fj(k)为似然函数，Pi→j为P{mj(k)|mi(k-1)}的简写，代表从k-1时刻到k时刻，模型mi(k-1)到模型mj(k)的马尔可夫转移概率，一般采用预先设置的常数，μi(k-1)为模型mi(k-1)在k-1时刻的模型匹配概率。
5.根据权利要求2所述的基于基于IMM-UKF的惯性/超短基线多参数标定方法，其特征在于，所述步骤S4具体包括以下过程：
根据权重，将各模型的估计值进行加权融合，计算联合状态估计与协方差矩阵，输出交互式多模型最终结果：
Figure FDA00024838551900000511
Figure FDA0002483855190000061
其中，
Figure FDA0002483855190000062
为联合状态估计，
Figure FDA0002483855190000063
为k时刻第j滤波器的状态估计，μj(k)为第j滤波器的模型概率，Pk为联合协方差矩阵，Pj(k)为第j滤波器协方差矩阵。
CN202010385732.2A 2020-05-09 2020-05-09 基于imm-ukf的惯性/超短基线多参数标定方法 Active CN111578936B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010385732.2A CN111578936B (zh) 2020-05-09 2020-05-09 基于imm-ukf的惯性/超短基线多参数标定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010385732.2A CN111578936B (zh) 2020-05-09 2020-05-09 基于imm-ukf的惯性/超短基线多参数标定方法

Publications (2)

Publication Number Publication Date
CN111578936A true CN111578936A (zh) 2020-08-25
CN111578936B CN111578936B (zh) 2022-08-02

Family

ID=72118725

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010385732.2A Active CN111578936B (zh) 2020-05-09 2020-05-09 基于imm-ukf的惯性/超短基线多参数标定方法

Country Status (1)

Country Link
CN (1) CN111578936B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112562006A (zh) * 2020-11-24 2021-03-26 南昌航空大学 一种基于强化学习的大视场摄像机标定方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20020097184A1 (en) * 2000-02-17 2002-07-25 Mayersak Joseph R. Location of radio frequency emitting targets
CN104252178A (zh) * 2014-09-12 2014-12-31 西安电子科技大学 一种基于强机动的目标跟踪方法
CN105509765A (zh) * 2014-09-23 2016-04-20 北京自动化控制设备研究所 一种惯性/dvl/usbl安装误差标定方法
CN109828296A (zh) * 2019-03-08 2019-05-31 哈尔滨工程大学 一种ins/usbl非线性紧组合综合校正方法
CN110057365A (zh) * 2019-05-05 2019-07-26 哈尔滨工程大学 一种大潜深auv下潜定位方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20020097184A1 (en) * 2000-02-17 2002-07-25 Mayersak Joseph R. Location of radio frequency emitting targets
CN104252178A (zh) * 2014-09-12 2014-12-31 西安电子科技大学 一种基于强机动的目标跟踪方法
CN105509765A (zh) * 2014-09-23 2016-04-20 北京自动化控制设备研究所 一种惯性/dvl/usbl安装误差标定方法
CN109828296A (zh) * 2019-03-08 2019-05-31 哈尔滨工程大学 一种ins/usbl非线性紧组合综合校正方法
CN110057365A (zh) * 2019-05-05 2019-07-26 哈尔滨工程大学 一种大潜深auv下潜定位方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112562006A (zh) * 2020-11-24 2021-03-26 南昌航空大学 一种基于强化学习的大视场摄像机标定方法
CN112562006B (zh) * 2020-11-24 2023-04-07 南昌航空大学 一种基于强化学习的大视场摄像机标定方法

Also Published As

Publication number Publication date
CN111578936B (zh) 2022-08-02

Similar Documents

Publication Publication Date Title
CN109373999B (zh) 基于故障容错卡尔曼滤波的组合导航方法
EP3734388B1 (en) Method and apparatus for performing simultaneous localization and mapping
CN103217175B (zh) 一种自适应容积卡尔曼滤波方法
CN103644903B (zh) 基于分布式边缘无味粒子滤波的同步定位与地图构建方法
CN110954132B (zh) Grnn辅助自适应卡尔曼滤波进行导航故障识别的方法
CN109724599A (zh) 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法
CN110132308B (zh) 一种基于姿态确定的usbl安装误差角标定方法
CN106772524B (zh) 一种基于秩滤波的农业机器人组合导航信息融合方法
CN102135621B (zh) 一种多星座组合导航系统的故障识别方法
CN112729344B (zh) 无需参照物的传感器外参标定方法
CN103940433A (zh) 一种基于改进的自适应平方根ukf算法的卫星姿态确定方法
Ko et al. Particle filter approach for localization of an underwater robot using time difference of arrival
CN110375731A (zh) 一种混合交互式多模型滤波方法
CN111983620A (zh) 一种面向水下机器人搜寻探摸的目标定位方法
Zhang et al. A novel and robust calibration method for the underwater transponder position
CN111578936B (zh) 基于imm-ukf的惯性/超短基线多参数标定方法
CN113310505A (zh) 传感器系统的外参标定方法、装置及电子设备
Fernandes et al. Gnss/mems-ins integration for drone navigation using ekf on lie groups
CN110703205A (zh) 基于自适应无迹卡尔曼滤波的超短基线定位方法
CN113434806B (zh) 一种抗差自适应多模型滤波方法
CN101960322A (zh) 在三维空间中基于使用声音传感器的粒子滤波器的物体跟踪方法
CN115218927B (zh) 基于二次卡尔曼滤波的无人机imu传感器故障检测方法
CN114741659B (zh) 一种自适应模型在线重构建鲁棒滤波方法、设备及系统
CN113093092B (zh) 一种水下鲁棒自适应单信标定位方法
CN110514209B (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
GR01 Patent grant
GR01 Patent grant