CN117118398A - 一种基于自适应似然分布的离散四元数粒子滤波数据处理方法 - Google Patents

一种基于自适应似然分布的离散四元数粒子滤波数据处理方法 Download PDF

Info

Publication number
CN117118398A
CN117118398A CN202311064146.8A CN202311064146A CN117118398A CN 117118398 A CN117118398 A CN 117118398A CN 202311064146 A CN202311064146 A CN 202311064146A CN 117118398 A CN117118398 A CN 117118398A
Authority
CN
China
Prior art keywords
quaternion
time
weight
particles
particle
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
CN202311064146.8A
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.)
Qinhuai Innovation Research Institute Of Nanjing University Of Aeronautics And Astronautics
Nanjing University of Aeronautics and Astronautics
Original Assignee
Qinhuai Innovation Research Institute Of Nanjing University Of Aeronautics And Astronautics
Nanjing University of Aeronautics and Astronautics
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 Qinhuai Innovation Research Institute Of Nanjing University Of Aeronautics And Astronautics, Nanjing University of Aeronautics and Astronautics filed Critical Qinhuai Innovation Research Institute Of Nanjing University Of Aeronautics And Astronautics
Priority to CN202311064146.8A priority Critical patent/CN117118398A/zh
Publication of CN117118398A publication Critical patent/CN117118398A/zh
Pending legal-status Critical Current

Links

Classifications

    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
    • H03H21/00—Adaptive networks
    • H03H21/0012—Digital adaptive filters
    • H03H21/0043—Adaptive algorithms
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/13—Differential equations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Computational Mathematics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Theoretical Computer Science (AREA)
  • General Engineering & Computer Science (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Algebra (AREA)
  • Operations Research (AREA)
  • Computing Systems (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Probability & Statistics with Applications (AREA)
  • Navigation (AREA)

Abstract

本发明提出的一种基于自适应似然分布的离散四元数粒子滤波数据处理方法，属于数据处理和多传感器数据融合技术领域，主要用于解决粒子滤波器在姿态估计中观测精度难以保证，不可靠的观测信息使精度较低的观测值的权重增加，影响滤波精度的问题，该方法以粒子滤波为框架，融合陀螺仪、加速度计，利用自适应模型调整似然分布的范围。本发明基于离散四元数粒子滤波算法，考虑量测信息在姿态估计问题中的影响，将残差最新的量测信息引入重要性密度函数，基于自适应似然分布减少测量误差对四元数分布参数估计的影响，增加先验和似然的重叠面积，该方法在估计精度和计算成本之间取得了良好的平衡，适用于姿态估计、数据融合等应用场合。

Description

一种基于自适应似然分布的离散四元数粒子滤波数据处理 方法
技术领域
本发明为了解决粒子滤波算法在飞行器姿态估计中存在的计算负担和计算精度问题，提出了一种基于自适应似然分布的离散四元数粒子滤波直接姿态估计算法，属于数据处理和多传感器数据融合技术领域，适用于姿态估计、数据融合等应用场合。
背景技术
IMU已被广泛应用于各种应用场景，例如行人导航、运动捕捉、姿态估计以及机器人感知和操纵，然而，由于陀螺仪不可避免的漂移，传感器数据融合技术是实现目标长期稳定估计所必需的。在刚体转动过程中，通常使用的参数表示方法有欧拉角法、四元数法、修正的Rodrigue参数法和旋转矩阵法。在这些姿态表示中，四元数是最有吸引力的，它在运动方程中具有非奇异和双线性性质，并且在某些条件下比旋转矩阵更有效。四元数估计算法分为两个分支:间接估计法和直接估计法。间接估计使用姿态误差参数作为状态向量，依赖于假设噪声为高斯分布的局部非线性线性化，如扩展卡尔曼滤波，无迹四元数滤波以及间接高斯粒子滤波等，这种近似受到许多限制，而直接估计方法使用单位四元数作为状态向量，单位四元数构成了包括的双覆盖的旋转群的良好表示，并且只带来一个冗余度。为了提高数据处理的估计精度和实时性，人们进行了丰富的尝试，例如Oshman和Carmi提出的遗传算法四元数粒子滤波器以及Zhou基于确定性采样策略提出的高密度不确定性匹配离散四元数粒子滤波器，都在确保滤波精度的前提下提高了数据处理的实时性能。
由于在实际测量中，观测精度难以保证，不可靠的观测信息会使精度较低的观测值的权重增加，使异常观测的存在进一步影响动力学模型。测量误差可以由残差反映，因此在测量更新阶段，使用基于残差的似然分布自适应调整可以有效地减少测量误差对四元数分布的影响。但是目前，在四元数粒子滤波的研究中，考虑观测信息的精度对似然分布的影响的相关尝试还很少。
发明内容
在本发明中，提出了一种基于自适应似然分布的离散四元数粒子滤波器(Discrete Quaternion Particle Filtering Based On Adaptive LikelihoodDistribution)，以解决粗大观测影响滤波精度的问题。该方法基于离散四元数粒子滤波算法，考虑量测信息在姿态估计问题中的影响，将残差的最新的测量信息引入重要性密度函数，基于自适应似然分布减少测量误差对四元数分布参数估计的影响，使尾部更平坦，增加先验和似然的重叠面积，有效提高数据处理精度。通过与DQPF算法的比较，验证了该算法的有效性。
本发明的具体过程如下：
步骤1：根据陀螺仪输出数据建立四元数微分方程作为系统状态方程，根据加速度计及磁力计的输出建立系统观测方程，选取四元数q和陀螺仪偏置误差β作为系统状态量；
步骤2：确定数据处理初始化参数，根据初始加速度计输出y0＝[ax0 ay0 az0]T和初始重力加速度r0＝[0 0 -g]T得到初始中心四元数利用确定性采样方法得到初始化四元数粒子/>利用高斯分布初始化陀螺仪随机游走噪声/>其中，i表示粒子个数索引，N表示每层粒子数，I表示层数，M表示粒子总数，上标-表示先验，下标0表示初始时刻，T表示矩阵转置；
步骤3：时间更新，由k时刻的四元数粒子和k+1时刻的陀螺仪输出/>计算得到k+1时刻的先验粒子/>其中，M表示粒子数，上标+表示后验，下标k+1表示k+1时刻；
步骤4：计算中心四元数，转移四元数粒子，由步骤3得到的先验粒子和k时刻的权重/>计算得到k+1时刻的中心四元数/>再由k时刻的粒子/>k时刻的中心四元数/>和k+1时刻的中心四元数/>得到k+1时刻四元数粒子/>由k时刻的βk计算k+1时刻的先验陀螺仪随机游走/>
步骤5：计算先验权重，根据下一时刻的观测值yk+1，由步骤1中的观测方程对步骤4中得到的四元数粒子集进行量测更新并进行线性化处理得到k+1时刻的权重
步骤6：计算后验权重，根据预测残差向量εk+1，比例常数K和经验阈值δ构造自适应因子λ，基于似然分布自适应模型对步骤5中的先验权重/>优化得到后验权重/>归一化得到/>其中，/>为状态转移矩阵，/>为k时刻的最优四元数；
步骤7：解算三轴姿态角，根据步骤4中得到的k+1时刻四元数粒子集和步骤6中得到的归一化权重/>得到k+1时刻最优四元数/>并根据最优四元数计算姿态角，其中，姿态角包括俯仰角θ、横滚角γ、航向角ψ，根据步骤6中的/>计算陀螺仪偏差的后验分布；
步骤8：根据步骤4得到的k+1时刻的四元数粒子k+1时刻的中心四元数步骤6得到的k+1时刻的权重/>以及步骤7得到的最优四元数/>和陀螺仪偏差的后验分布，循环执行步骤3、4、5、6、7得到下一时刻滤波估计值。
进一步地，步骤1中定义四元数为q≡[q1 ρT]T，其中，T表示矩阵转置；q1＝cos(θ/2)， 是旋转轴，θ是旋转角度。
记qk为k时刻的四元数，qk+1为k+1时刻四元数，则系统状态方程为：
其中， 为陀螺仪k+1时刻的量测值，/>为k+1时刻先验陀螺仪随机游走噪声。
k时刻观测量yk由加速度计输出和磁力计输出组成，系统的观测方程为：
其中，为导航坐标系/>到机体坐标系/>的姿态矩阵，gn为当地重力加速度，mn为当地磁场分量；v0、m0分别为加速度计零均值白噪声、磁力计零均值白噪声。
进一步地，步骤2中得到初始时刻中心四元数计算公式如下：
其中，初始旋转角度
利用确定性采样方法得到初始化四元数粒子，过程如下：
Step1：利用递归分区等面积算法在S2平面上生成N个均匀点集
Step2：利用指数映射将S2平面的映射到S3平面，公式如下：
其中，等式右边的表示S2平面上的点集，/>表示/>的范数；
Step3：：在中心四元数周围采集N×I个确定性粒子，公式如下：
其中，i＝1,2,…,I,m＝1,2,…,N,R为高密度区域范围，←表示生成一个四元数粒子集。
高密度区域范围R通过如下公式获得：
其中，表示/>的标准差，σ通过如下步骤获得：
step1：以初始四元数为中心进行单位高斯采样得到M个随机样本；
step2：根据公式求所需概率区间的协方差，所的协方差便是σ，其中，/>表示维度为D的向量，/>则是这些向量的平均值，D是维度，在这里是4维。
陀螺仪随机游走噪声β初始粒子分布为
进一步地，步骤3中得到k+1时刻先验粒子公式如下：
其中是在k+1时刻考虑陀螺随机游走后的陀螺角速度输出矢量，/>表示的反对称矩阵，exp(g)表示指数映射。
进一步地，步骤4中得到k+1时刻的四元数粒子可通过如下步骤，具体如下：
Step1：由步骤3得到的先验粒子和k时刻的权重/>估计k+1时刻的中心四元数/>k+1时刻的中心四元数/>等于四元数二阶矩Pqqk+1的最大特征值λmax对应的单位规范化特征向量：
其中，E{g}表示期望。
Step2：根据上一时刻的中心四元数和计算得到的下一时刻中心四元数/>对每个粒子使用公式/>便可得更新后新的四元数/>其中，M为粒子总数，/>表示四元数乘法。
从陀螺仪偏差的预测分布/>中得出，计算公式如下：
进一步地，步骤5中包括以下步骤：
Step1：以步骤3得到的p(qk+1|qk)作为重要抽样分布
Step2：在获得k+1时刻加速度计和磁力计观测值yk+1后，计算粒子的权值
其中，p(qk+1|qk)为k+1时刻的先验分布，为重要抽样分布。
进一步地，步骤6中得到经过自适应似然分布优化的权重包括以下步骤：
Step1：构造自适应因子λ，公式如下：
Σεk+1＝εk+1εT k+1
其中，R为旋转矩阵，为k+1时刻的最佳观测量，yk+1为k+1时刻的实际观测量，v0为加速度计噪声，m0为磁力计噪声，εk+1为k+1时刻的残差，Σεk+1为协方差矩阵，tr(∑εk+1)为协方差矩阵Σεk+1的迹，tr(∑εk+1)越大，则量测信息的准确性越低。
if tr(∑εk+1)≥δ：
λ＝K/tr(∑εk+1)
else
λ＝0
其中，δ为权重调整阈值，K为比例常数，两者均是根据经验选取。
Step2：构造自适应似然分布模型公式如下：
其中，下标w-max代表权重最大粒子，下标w-min代表权重最小粒子，Ω(i)为欧氏距离。
权值归一化得到归一化权值
进一步地，步骤7根据步骤6得到的权值和步骤4得到k+1时刻的四元数粒子/>估计k+1时刻的最优四元数估计值/>最优四元数估计值/>等于四元数二阶矩Pqqk+1的最大特征值λmax对应的单位规范化特征向量：
其中，E{g}表示期望。
根据最优四元数估计值解算出滤波后姿态角：
θ＝arcsin(2(q3q4+q1q2))
βk+1的后验期望和方差计算公式如下：
进一步地，步骤8循环执行步骤3、4、5、6、7，得到下一时刻滤波估计值。
本发明具有如下优点：提出了一种基于自适应似然分布的离散四元数粒子滤波器，以解决测量信息的观测粗糙度问题。该方法基于离散四元数粒子滤波算法，考虑量测信息在姿态估计问题中的影响，将残差的最新的测量信息引入重要性密度函数，判断此时的测量信息在每次更新中是否存在较大偏差，在观测误差较大的时刻调整似然分布，以确保粒子接近真实分布，增加先验和似然的重叠面积，削弱粗差对参数估计的影响，有效提高数据处理精度。通过与DQPF算法的比较，在相同粒子数的情况下，A-DQPF比DQPF具有更高的精度，因此，本发明在数据处理的精度和计算成本之间取得了良好的平衡，为数据融合等领域解决问题提供了新方法。
附图说明
图1一种基于自适应似然分布的离散四元数粒子滤波算法的框架
图2静态情况下，粒子数为50的A-DQPF和DQPF的俯仰角、滚转角和偏航角的估计比较
图3基于自适应似然分布的离散四元数粒子滤波算法(A-DQPF)和离散四元数粒子滤波(DQPF)的均方根误差和单步运行时间比较
具体实施方式
下面详细描述本发明的实施例，本实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。参照说明书附图对本发明的一种基于自适应似然分布的离散四元数粒子滤波数据处理方法作以下详细地说明：
为更好体现本发明的具体步骤实施及效果，搭建如下仿真实验：通过仿真工作检查A-DQPF算法的性能。
仿真参数设置如下：陀螺仪每个轴上0.1度/时，噪声密度为偏差稳定性为σv＝36°/h，加速度计偏差稳定为σa＝20mg，三轴磁力计偏差稳定性为σm＝1μT，I＝5,N＝20，这些传感器采样间隔为0.01秒，一共采样250秒。
针对以上仿真实验数据，根据图1滤波算法结构框图，具体步骤实施如下：
步骤1：根据陀螺仪输出数据建立四元数微分方程作为系统状态方程，根据加速度计及磁力计的输出建立系统观测方程，选取四元数q和陀螺仪偏置误差β作为系统状态量；
步骤2：确定数据处理初始化参数，根据初始加速度计输出y0＝[ax0 ay0 az0]T和初始重力加速度r0＝[0 0 -g]T得到初始中心四元数利用确定性采样方法得到初始化四元数粒子/>利用高斯分布初始化陀螺仪随机游走噪声/>其中，i表示粒子个数索引，N表示每层粒子数，I表示层数，M表示粒子总数，上标-表示先验，下标0表示初始时刻，T表示矩阵转置；
步骤3：时间更新，由k时刻的四元数粒子和k+1时刻的陀螺仪输出/>计算得到k+1时刻的先验粒子/>其中，M表示粒子数，上标+表示后验；
步骤4：计算中心四元数，转移四元数粒子，由步骤3得到的先验粒子和k时刻的权重/>计算得到k+1时刻的中心四元数/>再由k时刻的粒子/>k时刻的中心四元数/>和k+1时刻的中心四元数/>得到k+1时刻四元数粒子/>由k时刻的βk计算k+1时刻的先验陀螺仪随机游走/>其中，下标k+1表示k+1时刻；
步骤5：计算先验权重，根据下一时刻的观测值yk+1，由步骤1中的观测方程对步骤4中得到的四元数粒子集进行量测更新并进行线性化处理得到k+1时刻的权重
步骤6：计算后验权重，根据预测残差向量εk+1，比例常数K和经验阈值δ构造自适应因子λ，基于似然分布自适应模型对步骤5中的先验权重/>优化得到后验权重/>归一化得到/>其中，/>为状态转移矩阵，/>为k时刻的最优四元数；
步骤7：解算三轴姿态角，根据步骤4中得到的k+1时刻四元数粒子集和步骤6中得到的归一化权重/>得到k+1时刻最优四元数/>并根据最优四元数计算姿态角，其中，姿态角包括俯仰角θ、横滚角γ、航向角ψ，根据步骤6中的/>计算陀螺仪偏差的后验分布；
步骤8：根据步骤4得到的k+1时刻的四元数粒子k+1时刻的中心四元数步骤6得到的k+1时刻的权重/>以及步骤7得到的最优四元数/>和陀螺仪偏差的后验分布，循环执行步骤3、4、5、6、7得到下一时刻滤波估计值。
进一步地，步骤1中定义四元数为q≡[q1 ρT]T，其中，T表示矩阵转置；q1＝cos(θ/2)， 是旋转轴，θ是旋转角度。
记qk为k时刻的四元数，qk+1为k+1时刻四元数，则系统状态方程为：
其中， 为陀螺仪k+1时刻的量测值，/>为k+1时刻先验陀螺仪随机游走噪声。
k时刻观测量yk由加速度计输出和磁力计输出组成，系统的观测方程为：
其中，为导航坐标系/>到机体坐标系/>的姿态矩阵，gn为当地重力加速度，mn为当地磁场分量；v0、m0分别为加速度计零均值白噪声、磁力计零均值白噪声。
进一步地，步骤2中得到初始时刻中心四元数计算公式如下：
其中，初始旋转角度
利用确定性采样方法得到初始化四元数粒子，过程如下：
Step1：利用递归分区等面积算法在S2平面上生成N个均匀点集
Step2：利用指数映射将S2平面的映射到S3平面，公式如下：
其中，等式右边的表示S2平面上的点集，/>表示/>的范数；
Step3：：在中心四元数周围采集N×I个确定性粒子，公式如下：
其中，i＝1,2,…,I,m＝1,2,…,N,R为高密度区域范围，←表示生成一个四元数粒子集。
高密度区域范围R通过如下公式获得：
其中，表示/>的标准差，σ通过如下步骤获得：
step1：以初始四元数为中心进行单位高斯采样得到M个随机样本；
step2：根据公式求所需概率区间的协方差，所的协方差便是σ，其中，/>表示维度为D的向量，/>则是这些向量的平均值，D是维度，在这里是4维。
陀螺仪随机游走噪声β初始粒子分布为
进一步地，步骤3中得到k+1时刻先验粒子公式如下：
其中是在k+1时刻考虑陀螺随机游走后的陀螺角速度输出矢量，/>表示的反对称矩阵，exp(g)表示指数映射。
进一步地，步骤4中得到k+1时刻的四元数粒子可通过如下步骤，具体如下：
Step1：由步骤3得到的先验粒子和k时刻的权重/>估计k+1时刻的中心四元数/>k+1时刻的中心四元数/>等于四元数二阶矩Pqqk+1的最大特征值λmax对应的单位规范化特征向量：
其中，E{g}表示期望。
Step2：根据上一时刻的中心四元数和计算得到的下一时刻中心四元数/>对每个粒子使用公式/>便可得更新后新的四元数/>其中，M为粒子总数，/>表示四元数乘法。
从陀螺仪偏差的预测分布/>中得出，计算公式如下：
进一步地，步骤5中包括以下步骤：
Step1：以步骤3得到的p(qk+1|qk)作为重要抽样分布
Step2：在获得k+1时刻加速度计和磁力计观测值yk+1后，计算粒子的权值
其中，p(qk+1|qk)为k+1时刻的先验分布，为重要抽样分布。
进一步地，步骤6中得到经过自适应似然分布优化的权重包括以下步骤：
Step1：构造自适应因子λ，公式如下：
Σεk+1＝εk+1εT k+1
其中，R为旋转矩阵，为k+1时刻的最佳观测量，yk+1为k+1时刻的实际观测量，v0为加速度计噪声，m0为磁力计噪声，εk+1为k+1时刻的残差，Σεk+1为协方差矩阵，tr(∑εk+1)为协方差矩阵Σεk+1的迹，tr(∑εk+1)越大，则量测信息的准确性越低。
if tr(∑εk+1)≥δ：
λ＝K/tr(∑εk+1)
else
λ＝0
其中，δ为权重调整阈值，K为比例常数，两者均是根据经验选取。
Step2：构造自适应似然分布模型公式如下：
其中，下标w-max代表权重最大粒子，下标w-min代表权重最小粒子，Ω(i)为欧氏距离。
权值归一化得到归一化权值
进一步地，步骤7根据步骤6得到的权值和步骤4得到k+1时刻的四元数粒子/>估计k+1时刻的最优四元数估计值/>最优四元数估计值/>等于四元数二阶矩Pqqk+1的最大特征值λmax对应的单位规范化特征向量：
/>
其中，E{g}表示期望。
根据最优四元数估计值解算出滤波后姿态角：
θ＝arcsin(2(q3q4+q1q2))
βk+1的后验期望和方差计算公式如下：
进一步地，步骤8循环执行步骤3、4、5、6、7，得到下一时刻滤波估计值。

Claims (1)

1.一种基于自适应似然分布的离散四元数粒子滤波数据处理方法，其特征在于，具体包括以下步骤：
步骤1：根据陀螺仪输出数据建立四元数微分方程作为系统状态方程，根据加速度计及磁力计的输出建立系统观测方程，选取四元数q和陀螺仪偏置误差β作为系统状态量；
步骤2：确定数据处理初始化参数，根据初始加速度计输出y0＝[ax0 ay0 az0]T和初始重力加速度r0＝[0 0 -g]T得到初始中心四元数利用确定性采样方法得到初始化四元数粒子/>利用高斯分布初始化陀螺仪随机游走噪声/>其中，i表示粒子个数索引，N表示每层粒子数，I表示层数，M表示粒子总数，上标-表示先验，下标0表示初始时刻，T表示矩阵转置；
步骤3：时间更新，由k时刻的四元数粒子和k+1时刻的陀螺仪输出/>计算得到k+1时刻的先验粒子/>其中，M表示粒子数，上标+表示后验，下标k+1表示k+1时刻；
步骤4：计算中心四元数，转移四元数粒子，由步骤3得到的先验粒子和k时刻的权重/>计算得到k+1时刻的中心四元数/>再由k时刻的粒子/>k时刻的中心四元数/>和k+1时刻的中心四元数/>得到k+1时刻四元数粒子/>由k时刻的βk计算k+1时刻的先验陀螺仪随机游走/>
步骤5：计算先验权重，根据下一时刻的观测值yk+1，由步骤1中的观测方程对步骤4中得到的四元数粒子集进行量测更新并进行线性化处理得到k+1时刻的先验权重
步骤6：计算后验权重，根据预测残差向量εk+1，比例常数K和经验阈值δ构造自适应因子λ，构造似然分布自适应模型对步骤5中的先验权重/>优化得到后验权重/>归一化得到/>其中，/>为状态转移矩阵，/>为k时刻的最优四元数；其中，生成后验权重/>的方法如下：
Step1：根据k+1时刻的最佳观测量和实际观测量计算残差，将协方差矩阵的迹tr(∑εk+1)作为自适应因子λ的统计量，根据自适应因子统计量tr(∑εk+1)和阈值δ构造自适应因子λ，如果tr(∑εk+1)≥δ，则λ＝K/tr(∑εk+1)，如果tr(∑εk+1)＜δ，则λ＝0，其中δ为权重调整阈值，K为比例常数，两者均是根据经验选取。
Step2：构造似然分布自适应模型由先验权重/>得到后验权重/>
其中，M为粒子总数，下标w-max代表权重最大的粒子，下标w-min代表权重最小的粒子，为k+1时刻权重最大粒子的量测新息，/>为k+1时刻权重最大粒子的量测新息，为k+1时刻权重最大粒子对应的先验权重，Ω(i)为欧氏距离。
步骤7：解算三轴姿态角，根据步骤4中得到的k+1时刻四元数粒子集和步骤6中得到的归一化权重/>得到k+1时刻最优四元数/>并根据最优四元数计算姿态角，其中，姿态角包括俯仰角θ、横滚角γ、航向角ψ，根据步骤6中的/>计算陀螺仪偏差的后验分布；
步骤8：根据步骤4得到的k+1时刻的四元数粒子k+1时刻的中心四元数/>步骤6得到的k+1时刻的权重/>以及步骤7得到的最优四元数/>和陀螺仪偏差的后验分布，循环执行步骤3、4、5、6、7得到下一时刻滤波估计值。
CN202311064146.8A 2023-08-22 2023-08-22 一种基于自适应似然分布的离散四元数粒子滤波数据处理方法 Pending CN117118398A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311064146.8A CN117118398A (zh) 2023-08-22 2023-08-22 一种基于自适应似然分布的离散四元数粒子滤波数据处理方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311064146.8A CN117118398A (zh) 2023-08-22 2023-08-22 一种基于自适应似然分布的离散四元数粒子滤波数据处理方法

Publications (1)

Publication Number Publication Date
CN117118398A true CN117118398A (zh) 2023-11-24

Family

ID=88804986

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311064146.8A Pending CN117118398A (zh) 2023-08-22 2023-08-22 一种基于自适应似然分布的离散四元数粒子滤波数据处理方法

Country Status (1)

Country Link
CN (1) CN117118398A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117674771A (zh) * 2024-01-31 2024-03-08 成都理工大学 一种具有辨识噪声性能的抗差自适应滤波方法及其应用

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117674771A (zh) * 2024-01-31 2024-03-08 成都理工大学 一种具有辨识噪声性能的抗差自适应滤波方法及其应用
CN117674771B (zh) * 2024-01-31 2024-04-26 成都理工大学 一种具有辨识噪声性能的抗差自适应滤波方法及其应用

Similar Documents

Publication Publication Date Title
CN108759833B (zh) 一种基于先验地图的智能车辆定位方法
CN109974714B (zh) 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法
CN108827310B (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN111238467B (zh) 一种仿生偏振光辅助的无人作战飞行器自主导航方法
Poddar et al. PSO aided adaptive complementary filter for attitude estimation
CN117118398A (zh) 一种基于自适应似然分布的离散四元数粒子滤波数据处理方法
CN110726415B (zh) 一种自适应的水下多波束同步定位与建图方法
CN115143954B (zh) 一种基于多源信息融合的无人车导航方法
CN109840517A (zh) 一种mems陀螺噪声估计和滤波方法
CN108645404A (zh) 一种小型多旋翼无人机姿态解算方法
Zhang et al. Information fusion based on artificial intelligence method for SINS/GPS integrated navigation of marine vessel
CN116642482A (zh) 基于固态激光雷达和惯性导航的定位方法、设备和介质
CN110763234B (zh) 一种水下机器人海底地形匹配导航路径规划方法
CN110595434B (zh) 基于mems传感器的四元数融合姿态估计方法
CN116449405A (zh) 一种基于密度聚类自适应滤波的组合导航方法
CN115900708A (zh) 基于gps引导式粒子滤波的机器人多传感器融合定位方法
CN111649747A (zh) 一种基于imu的自适应ekf姿态测量改进方法
CN115265532A (zh) 一种用于船用组合导航中的辅助滤波方法
Lai et al. Improved arithmetic of two-position fast initial alignment for SINS using unscented Kalman filter
Zheng et al. An rnn-based learnable extended kalman filter design and application
CN116255988B (zh) 一种基于舰船动力学组合导航复合抗干扰自适应滤波方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN110048693B (zh) 基于四元数分布的并行高斯粒子滤波数据处理方法
Vertzberger et al. Adaptive attitude estimation using a hybrid model-learning approach
CN113503891B (zh) 一种sinsdvl对准校正方法、系统、介质及设备

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
CB03 Change of inventor or designer information
CB03 Change of inventor or designer information

Inventor after: Zhou Dihe

Inventor after: An Zimin

Inventor after: You Xia

Inventor after: Tian Xiangrui

Inventor before: Zhou Dihe

Inventor before: An Zhimin

Inventor before: You Xia

Inventor before: Tian Xiangrui