CN112070170B - 一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法 - Google Patents
一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法 Download PDFInfo
- Publication number
- CN112070170B CN112070170B CN202010950223.XA CN202010950223A CN112070170B CN 112070170 B CN112070170 B CN 112070170B CN 202010950223 A CN202010950223 A CN 202010950223A CN 112070170 B CN112070170 B CN 112070170B
- Authority
- CN
- China
- Prior art keywords
- quaternion
- particle
- residual
- moment
- time
- 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.)
- Active
Links
- 239000002245 particle Substances 0.000 title claims abstract description 90
- 238000001914 filtration Methods 0.000 title claims abstract description 26
- 238000004364 calculation method Methods 0.000 title claims abstract description 24
- 238000007500 overflow downdraw method Methods 0.000 title claims abstract description 11
- 238000005259 measurement Methods 0.000 claims abstract description 43
- 239000011159 matrix material Substances 0.000 claims abstract description 30
- 238000000034 method Methods 0.000 claims abstract description 18
- 230000004927 fusion Effects 0.000 claims abstract description 4
- 238000012952 Resampling Methods 0.000 claims description 10
- 230000003044 adaptive effect Effects 0.000 claims description 9
- 238000005070 sampling Methods 0.000 claims description 7
- 238000010606 normalization Methods 0.000 claims description 6
- 230000001351 cycling effect Effects 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 abstract description 10
- 230000001133 acceleration Effects 0.000 description 8
- 230000005484 gravity Effects 0.000 description 8
- 238000004088 simulation Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 4
- 238000006243 chemical reaction Methods 0.000 description 3
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/25—Fusion techniques
- G06F18/251—Fusion techniques of input or preprocessed data
-
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
-
- 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
-
- 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
- G06F2218/00—Aspects of pattern recognition specially adapted for signal processing
- G06F2218/02—Preprocessing
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Mathematical Physics (AREA)
- Automation & Control Theory (AREA)
- Mathematical Analysis (AREA)
- Computational Mathematics (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Optimization (AREA)
- General Engineering & Computer Science (AREA)
- Artificial Intelligence (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Computing Systems (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Biology (AREA)
- Life Sciences & Earth Sciences (AREA)
- Evolutionary Computation (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- Navigation (AREA)
Abstract
本发明提出的一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法,属于数字滤波和多传感器数据融合技术领域,主要用于提高载体姿态估计的精度。该方法以标准粒子滤波算法作为框架,融合陀螺仪、加速度计及磁力计数据,通过对传感器误差分析建立较为准确的传感器测量模型,并选用四元数作为姿态参数,对残差的合理范围进行预测判断,继而自适应调整***的量测噪声矩阵,实现传感器的数据融合。本发明适用于非线性姿态测量***,具有良好的抗干扰性和姿态解算精度,可以有效解决复杂环境下载体姿态解算问题。
Description
技术领域
本发明提供的一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法,属于数字滤波和多传感器数据融合技术领域,本发明提供的方法适用于非线性姿态测量***。
背景技术
载体的姿态测量***实际上为一个非线性***,为了实现对姿态的精确解算,需要采用非线性滤波融合陀螺仪、加速度计及磁力计的数据,但是由于加速度计和磁力计本身容易受到非重力加速度和磁场干扰导致其输出数据包含了较多的噪声,过程噪声和量测噪声都是未知且具有时变特性,错误的参数估计可能导致滤波发散,所以需要使用合适的滤波算法。因此,一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法应运而生,该方法能够有效判别误差较大的传感器测量数据,并自适应调整量测噪声大小,抑制了非重力加速度和磁场对姿态估计的干扰,相比较于传统算法,该算法具有更高的姿态解算精度。
在载体实际运动过程中,加速度计和磁力计容易受到非重力加速度和磁力计的影响,导致其输出数据误差较大,从而影响滤波效果,因此本发明针对上述问题,设计了一种动态残差阈值自适应四元数粒子滤波姿态数据融合方法,选用四元数作为姿态参数,利用四元数运动学微分方程构建***的状态方程,利用加速度计和陀螺仪输出模型建立***的量测方程,对***的动态残差阈值范围进行预测,若残差超出范围,则对***量测噪声矩阵进行自适应估计,并利用自适应估计得到的量测噪声矩阵进行量测更新和状态自适应估计。
发明内容
本发明的目的在于针对上述存在的问题和不足,提出一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法(RDT-QPF),该算法能够对传感器误差进行分析并建立较为准确的传感器测量模型,对残差阈值范围进行预测,如果残差超出该范围则调整***量测噪声矩阵,获得精确的姿态角,保证载体运动过程中姿态的精确测量和有效控制。
本发明包括以下步骤:
步骤1:选择四元数q=[q0 qv]T作为四元数粒子滤波状态量,即x=q,其中q0和qv分别表示四元数标量和矢量部分,根据陀螺仪输出和陀螺仪误差模型建立四元数粒子滤波状态方程,根据加速度计输出fb和磁力计输出mb作为观测量,建立四元数粒子滤波量测方程;
步骤2:滤波器初始化,根据初始姿态角求得初始四元数q0,采样得到一系列四元数粒子集初始化每个粒子权值为1/N,其中上标i表示粒子索引,下标0表示初始时刻,N为粒子总数;
步骤3:时间更新,根据建立的动态残差阈值自适应四元数粒子滤波状态方程对k时刻的四元数粒子集进行更新得到k+1时刻的预测四元数粒子集/>
步骤4:根据k时刻的残差均值μk和方差σk+1 2信息,预测k+1时刻动态残差阈值区间,判断k+1时刻残差值rk+1是否超出动态残差阈值区间S~[μk+1-εσk+1,μk+1+εσk+1],若残差值超出动态残差阈值区间,则自适应调整***k+1时刻的量测噪声矩阵Rk+1;其中ε表示相关系数,μk+1和σk+1表示k+1时刻残差的均值和标准差;
步骤5:利用自适应估计得到的量测噪声矩阵及步骤1的量测方程进行***量测更新,计算k+1时刻粒子的重要性权值并归一化;
步骤6:重采样,若有效粒子数则对粒子集进行随机重采样操作,其中/>表示k+1时刻第i个粒子的权值,Nthrehold表示重采样粒子阈值;
步骤7:对量测更新后的粒子集进行状态估计得到最优四元数估计值并进行归一化处理,利用四元数滤波值姿态解算方法求解得到俯仰角θ,横滚角γ和航向角ψ;
步骤8:循环步骤3~步骤7,直到滤波结束。
进一步地,步骤1中定义四元数为q=[q0 qv]T=[q0 q1 q2 q3]T,其中q0和qv分别表示四元数标量和矢量部分,q1~q3为四元数的三个矢量元,四元数运动学微分方程为:
其中,表示四元数的乘法,q(t)表示t时刻的四元数矩阵,ω(t)表示t时刻的角速度,此处假设角速度矢量为四元数形式,Φω(t)为角速度组成的四元数乘法交换矩阵;
对四元数运动学微分方程求通解并离散化得:
定义Θ(T)内容如下:
其中,θ(T)表示[0,T]时段内的三轴角速度增量,即:
由此选取***状态量x如下:
x=q=[q0 q1 q2 q3]T
建立动态残差阈值自适应四元数粒子滤波状态方程如下:
其中,ΔT表示采样时间间隔;
加速度计输出数据fb与四旋翼飞行器的运动条件有关,其输出模型可表示为:
式中,为由导航坐标系(n系)到载体坐标系(b系)的姿态转换矩阵,gn为当地重力分量,e为非重力加速度,nf为加速度计高斯白噪声,/>具体形式如下:
磁力计输出为mb,其模型可表示为:
式中,mn为当地磁场强度分量,nm为磁力计高斯白噪声,
选取***观测量z如下:
其中,分别表示加速度计和磁力计的三轴输出分量;
建立动态残差阈值自适应四元数粒子滤波观测方程如下:
其中,h(·)表示***观测方程的非线性函数,vk表示***的观测噪声;
协方差矩阵:
其中,Rf和Rm分别为加速度计和磁力计的噪声协方差矩阵,03×3为3×3的零矩阵;
进一步地,步骤2中根据初始姿态角求得初始四元数q0,采样得到一系列四元数粒子集初始化每个粒子权值为1/N,其中上标i表示粒子索引,下标0表示初始时刻,N为粒子总数;
初始四元数值q0可以由初始姿态角确定:
其中,θ表示俯仰角,γ表示横滚角,ψ表示航向角;
进一步地,步骤3中根据建立的动态残差阈值自适应四元数粒子滤波状态方程对k时刻的四元数粒子集进行更新得到k+1时刻的预测四元数粒子集/>
进一步地,步骤4中具体包含以下步骤:
Step1:根据k时刻的残差均值μk和方差σk 2信息,预测k+1时刻动态残差阈值区间:
k+1时刻预测的残差均值μk+1和方差σk+1 2计算公式为:
其中,rk+1为k+1时刻残差值,为k+1时刻第i个粒子的残差值:
其中,zk+1为k+1时刻***的量测值,为k+1时刻第i个粒子的预测量测值;
Step2:判断k+1时刻残差值rk+1是否超出动态残差阈值区间S~[μk+1-εσk+1,μk+1+εσk+1],其中ε表示相关系数,μk+1和σk+1表示k+1时刻残差的均值和标准差;
Step3:若残差值超出动态残差阈值区间,则自适应调整***k+1时刻的量测噪声矩阵Rk+1,引入滤波窗口h内的残差信息对Rk+1进行调整:
其中,h表示滤波窗口的宽度;
进一步地,步骤5中利用自适应估计得到的量测噪声矩阵及步骤1的量测方程进行***量测更新,k+1时刻粒子的重要性权值可以由下式进行计算:
粒子权值归一化:
进一步地,步骤6中定义有效粒子数若Neff<Nthrehold,则对粒子集进行随机重采样操作,其中/>表示k+1时刻第i个粒子的权值,Nthrehold表示重采样粒子阈值;
进一步地,步骤7中对量测更新后的粒子集进行状态估计得到最优四元数估计值并进行归一化处理,利用四元数滤波值姿态解算方法求解得到姿态角。
采用以下公式来求得最优四元数的估计值
该公式表示最优四元数估计值就是矩阵M的最大特征值对应的特征向量;
由最优四元数估计值,可求得俯仰角θ、横滚角γ和航向角计算公式如下:
进一步地,步骤8,循环步骤3~步骤7,直到滤波结束。
本发明具有如下优点:本发明在标准粒子滤波算法的基础上,选择四元数作为姿态参数,通过对残差的阈值进行预测,然后自适应调整***的量测噪声矩阵,保证了滤波算法的稳定性,可以有效提高姿态估计精度,在加速度计和磁力计受到非重力加速度和磁场干扰的情况下,仍能获得精确的姿态角,因此,本发明具有良好的抗干扰性,能够为载体运动器的稳定运动提供重要保障。
附图说明
图1一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法流程框图
图2残差与动态阈值判别结果
图3姿态解算角度误差对比
具体实施方式
下面详细描述本发明的实施例,本实施例是示例性的,仅用于解释本发明,而不能理解为对本发明的限制。参照说明书附图对本发明的一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法作以下详细地说明。
为更好体现本发明的具体步骤实施及效果,搭建如下仿真实验:使用MATLAB仿真平台验证算法的性能。该实验环境中,可搭建以下测量模型:
陀螺仪输出模型可表示为ωb=ω+ε,式中ω为真实角速度,ε为陀螺仪的输出误差,主要包含三种分量:ε=εb+εr+ng,其中,εb为随机常值漂移,εr为陀螺仪一阶马尔科夫过程,Tg为相关时间,ωr为马尔科夫过程白噪声,ng为陀螺仪高斯白噪声。
加速度计和磁力计的输出模型为:式中/>为由导航坐标系(n系)到载体坐标系(b系)的姿态转换矩阵,gn为当地重力分量,e为非重力加速度,nf为加速度计高斯白噪声,mn为当地磁场强度分量,nm为磁力计高斯白噪声。
仿真参数设置如下:粒子数为N=1000,仿真时长为60s,传感器采样频率设置为100Hz,三轴陀螺仪噪声系数为σω=0.2778,三轴加速度计噪声系数为σf=0.124,三轴磁力计噪声系数为σm=0.244。
针对以上仿真实验数据,根据图1一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法流程框图,具体步骤实施如下:
步骤1:选择四元数q=[q0 qv]T作为四元数粒子滤波状态量,即x=q,其中q0和qv分别表示四元数标量和矢量部分,根据陀螺仪输出和陀螺仪误差模型建立四元数粒子滤波状态方程,根据加速度计输出fb和磁力计输出mb作为观测量,建立四元数粒子滤波量测方程;
定义四元数为q=[q0 qv]T=[q0 q1 q2 q3]T,其中q0和qv分别表示四元数标量和矢量部分,q1~q3为四元数的三个矢量元,四元数运动学微分方程为:
其中,表示四元数的乘法,q(t)表示t时刻的四元数矩阵,ω(t)表示t时刻的角速度,此处假设角速度矢量为四元数形式,Φω(t)为角速度组成的四元数乘法交换矩阵;
对四元数运动学微分方程求通解并离散化得:
定义Θ(T)内容如下:
其中,θ(T)表示[0,T]时段内的三轴角速度增量,即:
由此选取***状态量x如下:
x=q=[q0 q1 q2 q3]T
建立动态残差阈值自适应四元数粒子滤波状态方程如下:
其中,ΔT表示采样时间间隔;
加速度计输出数据fb与四旋翼飞行器的运动条件有关,其输出模型可表示为:
式中,为由导航坐标系(n系)到载体坐标系(b系)的姿态转换矩阵,gn为当地重力分量,e为非重力加速度,nf为加速度计高斯白噪声,/>具体形式如下:
磁力计输出为mb,其模型可表示为:
式中,mn为当地磁场强度分量,nm为磁力计高斯白噪声,
选取***观测量z如下:
其中,分别表示加速度计和磁力计的三轴输出分量;
建立动态残差阈值自适应四元数粒子滤波观测方程如下:
其中,h(·)表示***观测方程的非线性函数,vk表示***的观测噪声;
协方差矩阵:
其中,Rf和Rm分别为加速度计和磁力计的噪声协方差矩阵,03×3为3×3的零矩阵;
步骤2:滤波器初始化,根据初始姿态角求得初始四元数q0,采样得到一系列四元数粒子集初始化每个粒子权值为1/N,其中上标i表示粒子索引,下标0表示初始时刻,N为粒子总数;
本例中,初始姿态角设置为Λ0=[0° 0° 30°]T,初始四元数值q0可以由初始姿态角确定:
其中,θ表示俯仰角,γ表示横滚角,ψ表示航向角;
步骤3:时间更新,根据建立的动态残差阈值自适应四元数粒子滤波状态方程对k时刻的四元数粒子集进行更新得到k+1时刻的预测四元数粒子集/>
步骤4:根据k时刻的残差均值μk和方差σk 2信息,预测k+1时刻动态残差阈值区间,判断k+1时刻残差值rk+1是否超出动态残差阈值区间S~[μk+1-εσk+1,μk+1+εσk+1],若残差值超出动态残差阈值区间,则自适应调整***k+1时刻的量测噪声矩阵Rk+1;其中ε表示相关系数,μk+1和σk+1表示k+1时刻残差的均值和标准差,具体包含以下步骤:
Step1:根据k时刻的残差均值μk和方差σk+1 2信息,预测k+1时刻动态残差阈值区间:
k+1时刻预测的残差均值μk+1和方差σk+1 2计算公式为:
其中,rk+1为k+1时刻残差值,为k+1时刻第i个粒子的残差值:
其中,zk+1为k+1时刻***的量测值,为k+1时刻第i个粒子的预测量测值;
Step2:判断k+1时刻残差值rk+1是否超出动态残差阈值区间S~[μk+1-εσk+1,μk+1+εσk+1],其中ε表示相关系数,μk+1和σk+1表示k+1时刻残差的均值和标准差;
Step3:若残差值超出动态残差阈值区间,则自适应调整***k+1时刻的量测噪声矩阵Rk+1,引入滤波窗口h内的残差信息对Rk+1进行调整:
其中,h表示滤波窗口的宽度;
步骤5:利用自适应估计得到的量测噪声矩阵及步骤1的量测方程进行***量测更新,计算k+1时刻粒子的重要性权值并归一化;
k+1时刻粒子的重要性权值可以由下式进行计算:
粒子权值归一化:
步骤6:重采样,若有效粒子数则对粒子集进行随机重采样操作,其中/>表示k+1时刻第i个粒子的权值,Nthrehold表示重采样粒子阈值,本例中取Nthrehold=2N/3;
步骤7:对量测更新后的粒子集进行状态估计得到最优四元数估计值并进行归一化处理,利用四元数滤波值姿态解算方法求解得到俯仰角θ,横滚角γ和航向角ψ;
采用以下公式来求得最优四元数的估计值
该公式表示最优四元数估计值就是矩阵M的最大特征值对应的特征向量;
由最优四元数估计值,可求得俯仰角θ、横滚角γ和航向角计算公式如下:
步骤8:循环步骤3~步骤7,直到滤波结束。
对该方法进行效果分析,仿真过程中在25~30s向加速度计Y轴加入非重力加速度干扰,向磁力计三轴加入磁场干扰。如图2所示,y轴的acc-x,acc-y,acc-z,mag-x,mag-y,mag-z分别表示加速度计和磁力计的三轴输出残差值,在对应时间段的残差值均超过预测阈值范围,说明此时传感器测量数据误差较大,如图3所示,使用无迹卡尔曼滤波算法(UKF)和标准粒子滤波算法(QPF)解算得到的俯仰角pitch、横滚角roll和航向角yaw波动均比较大,而一种动态残差阈值自适应四元数粒子滤波姿态数据融合方法(RDT-QPF)通过自适应调整***的量测噪声协方差矩阵,滤波效果明显优于其余两种算法,对外界干扰有较好的抑制效果,提高了姿态解算的精度。
Claims (2)
1.一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法,其特征在于,具体包括以下步骤:
步骤1:选择四元数q=[q0 qv]T作为四元数粒子滤波状态量,即x=q,其中q0和qv分别表示四元数标量和矢量部分,根据陀螺仪输出和陀螺仪误差模型建立四元数粒子滤波状态方程,根据加速度计输出fb和磁力计输出mb作为观测量,建立四元数粒子滤波量测方程;
步骤2:滤波器初始化,根据初始姿态角求得初始四元数q0,采样得到一系列四元数粒子集初始化每个粒子权值为1/N,其中上标i表示粒子索引,下标0表示初始时刻,N为粒子总数;
步骤3:时间更新,根据建立的动态残差阈值自适应四元数粒子滤波状态方程对k时刻的四元数粒子集进行更新得到k+1时刻的预测四元数粒子集/>
步骤4:根据k时刻的残差均值μk和方差σk 2信息,预测k+1时刻动态残差阈值区间,判断k+1时刻残差值rk+1是否超出动态残差阈值区间S~[μk+1-εσk+1,μk+1+εσk+1],若残差值超出动态残差阈值区间,则自适应调整***k+1时刻的量测噪声矩阵Rk+1;其中ε表示相关系数,μk+1和σk+1表示k+1时刻残差的均值和标准差;
步骤5:利用自适应估计得到的量测噪声矩阵及步骤1的量测方程进行***量测更新,计算k+1时刻粒子的重要性权值并归一化;
步骤6:重采样,若有效粒子数则对粒子集进行随机重采样操作,其中/>表示k+1时刻第i个粒子的权值,Nthrehold表示重采样粒子阈值;
步骤7:对量测更新后的粒子集进行状态估计得到最优四元数估计值并进行归一化处理,利用四元数滤波值姿态解算方法求解得到俯仰角θ,横滚角γ和航向角ψ;
步骤8:循环步骤3~步骤7,直到滤波结束。
2.根据权利要求1所述的一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法,其特征在于,步骤4中根据k时刻的残差均值μk和方差σk 2信息,预测k+1时刻动态残差阈值区间,判断k+1时刻残差值rk+1是否超出动态残差阈值区间S~[μk+1-εσk+1,μk+1+εσk+1],若残差值超出动态残差阈值区间,则自适应调整***k+1时刻的量测噪声矩阵Rk+1;具体包含以下步骤:
Step1:根据k时刻的残差均值μk和方差σk 2信息,预测k+1时刻动态残差阈值区间:
k+1时刻预测的残差均值μk+1和方差σk+1 2计算公式为:
其中,为k+1时刻第i个粒子的残差值:
其中,zk+1为k+1时刻***的量测值,为k+1时刻第i个粒子的预测量测值;
Step2:判断k+1时刻残差值rk+1是否超出动态残差阈值区间S~[μk+1-εσk+1,μk+1+εσk+1],其中ε表示相关系数,μk+1和σk+1表示残差的均值和标准差;
Step3:若残差值超出动态残差阈值区间,则自适应调整***k+1时刻的量测噪声矩阵Rk+1,引入滤波窗口h内的残差信息对Rk+1进行调整:
其中,h表示滤波窗口的宽度。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010950223.XA CN112070170B (zh) | 2020-09-10 | 2020-09-10 | 一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010950223.XA CN112070170B (zh) | 2020-09-10 | 2020-09-10 | 一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112070170A CN112070170A (zh) | 2020-12-11 |
CN112070170B true CN112070170B (zh) | 2024-03-29 |
Family
ID=73663644
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010950223.XA Active CN112070170B (zh) | 2020-09-10 | 2020-09-10 | 一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112070170B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112650281B (zh) * | 2020-12-14 | 2023-08-22 | 一飞(海南)科技有限公司 | 多传感器三余度***、控制方法、无人机、介质及终端 |
CN113124863B (zh) * | 2021-04-15 | 2022-10-25 | 南京航空航天大学 | 一种基于kld采样的混合粒子联邦滤波数据处理方法 |
CN115290185A (zh) * | 2022-09-05 | 2022-11-04 | 江苏徐工国重实验室科技有限公司 | 抗振动滤波方法、装置和工程机械 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108318038A (zh) * | 2018-01-26 | 2018-07-24 | 南京航空航天大学 | 一种四元数高斯粒子滤波移动机器人姿态解算方法 |
CN109916398A (zh) * | 2019-03-22 | 2019-06-21 | 南京航空航天大学 | 一种自适应四元数粒子滤波姿态数据融合方法 |
-
2020
- 2020-09-10 CN CN202010950223.XA patent/CN112070170B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108318038A (zh) * | 2018-01-26 | 2018-07-24 | 南京航空航天大学 | 一种四元数高斯粒子滤波移动机器人姿态解算方法 |
CN109916398A (zh) * | 2019-03-22 | 2019-06-21 | 南京航空航天大学 | 一种自适应四元数粒子滤波姿态数据融合方法 |
Also Published As
Publication number | Publication date |
---|---|
CN112070170A (zh) | 2020-12-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112070170B (zh) | 一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法 | |
CN109974714B (zh) | 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法 | |
CN108362282B (zh) | 一种基于自适应零速区间调整的惯性行人定位方法 | |
CN108827299B (zh) | 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法 | |
CN111024064B (zh) | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 | |
CN108225308B (zh) | 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法 | |
CN106643737B (zh) | 风力干扰环境下四旋翼飞行器姿态解算方法 | |
CN108519090B (zh) | 一种基于优化的ukf算法的双通道组合定姿算法的实现方法 | |
CN106885566B (zh) | 一种可穿戴式运动传感器及其抗磁场干扰的方法 | |
CN110184885B (zh) | 一种基于智能手机测试路面平整度的方法 | |
CN112013836A (zh) | 一种基于改进自适应卡尔曼滤波的航姿参考***算法 | |
CN110231029B (zh) | 一种水下机器人多传感器融合数据处理方法 | |
CN108731676B (zh) | 一种基于惯性导航技术的姿态融合增强测量方法及*** | |
CN109945859B (zh) | 一种自适应h∞滤波的运动学约束捷联惯性导航方法 | |
CN112665574B (zh) | 基于动量梯度下降法的水下机器人姿态采集方法 | |
Laidig et al. | VQF: Highly accurate IMU orientation estimation with bias estimation and magnetic disturbance rejection | |
CN110132271B (zh) | 一种自适应卡尔曼滤波姿态估计算法 | |
CN106403952A (zh) | 一种动中通低成本组合姿态测量方法 | |
CN106525055B (zh) | 一种基于模型摄动的火星大气进入自适应估计方法 | |
CN104819717B (zh) | 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法 | |
CN111649747A (zh) | 一种基于imu的自适应ekf姿态测量改进方法 | |
CN107860382B (zh) | 一种在地磁异常情况下应用ahrs测量姿态的方法 | |
CN113175926B (zh) | 一种基于运动状态监测的自适应水平姿态测量方法 | |
CN110375773B (zh) | Mems惯导***姿态初始化方法 | |
CN112729348B (zh) | 一种用于imu***的姿态自适应校正方法 |
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 |