CN114964226B - 噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法 - Google Patents
噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法 Download PDFInfo
- Publication number
- CN114964226B CN114964226B CN202210471564.8A CN202210471564A CN114964226B CN 114964226 B CN114964226 B CN 114964226B CN 202210471564 A CN202210471564 A CN 202210471564A CN 114964226 B CN114964226 B CN 114964226B
- Authority
- CN
- China
- Prior art keywords
- noise
- state
- kalman filter
- extended kalman
- strong tracking
- 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
- 238000000034 method Methods 0.000 title claims abstract description 29
- 239000011159 matrix material Substances 0.000 claims abstract description 39
- 238000005562 fading Methods 0.000 claims abstract description 27
- 238000004364 calculation method Methods 0.000 claims abstract description 7
- 230000003044 adaptive effect Effects 0.000 claims description 15
- 230000008569 process Effects 0.000 claims description 11
- 238000005259 measurement Methods 0.000 claims description 10
- 230000007704 transition Effects 0.000 claims description 10
- 238000002474 experimental method Methods 0.000 claims description 7
- 238000010276 construction Methods 0.000 claims description 3
- 238000012804 iterative process Methods 0.000 claims description 3
- 230000007246 mechanism Effects 0.000 claims description 3
- 230000001105 regulatory effect Effects 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims 1
- 238000004088 simulation Methods 0.000 description 7
- 230000008859 change Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000002054 transplantation Methods 0.000 description 1
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/18—Stabilised platforms, e.g. by gyroscope
-
- 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/183—Compensation of inertial measurements, e.g. for temperature effects
-
- 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
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
-
- 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
-
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Mathematical Physics (AREA)
- Automation & Control Theory (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Analysis (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Computational Mathematics (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Databases & Information Systems (AREA)
- Algebra (AREA)
- Computing Systems (AREA)
- Operations Research (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Navigation (AREA)
Abstract
本发明涉及一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，属于无人机技术领域，包括构建状态方程和观测方程，初始化系统噪声和量测噪声初值；计算下一时刻的一次预测状态；计算新息，更新强跟踪滤波器，将次优渐消因子拆分；计算得到改进的一步预测误差协方差矩阵对下一时刻的噪声方差及噪声均值重新估计，将估计值引入EKF公式；计算卡尔曼增益矩阵、状态的二次估计和误差协方差的二次估计，判断是否循环结束；输出结果。本发明引入强跟踪滤波器，解决了传统扩展卡尔曼滤波器因泰勒展开近似线性化等问题导致的跟踪能力弱的问题，通过改进次优渐消因子引入形式，保证了协方差矩阵计算结果的对称性，提高了系统跟踪速度。
Description
技术领域
本发明涉及一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，属于无人机技术领域。
背景技术
无人机是一种特殊的不载人飞行器，利用无线遥控设备或预设机载程序进行自主控制。随着微机电子的发展，以MEMS器件为核心的惯性测量系统成为无人机导航测量装置的主流应用，MEMS具有器件尺寸小、响应快、成本低廉等优势。基于MEMS传感器的IMU得到广泛应用，如：无人机、无人驾驶汽车、轨迹跟踪、捷联惯性导航等具有广泛的工程应用市场。
四旋翼飞行器飞行的基础是精确的姿态解算，而MEMS传感器中陀螺仪会产生漂移现象；加速度计动态性能差，易受到外界震动影响导致产生较大噪声的测量值，使系统引入较大误差；磁力计易受到外部磁场信号干扰，对于周围环境比较敏感。上述三种误差的产生不能预料，是随机产生的，所以对四旋翼的姿态解算需要进行深入研究。
发明内容
本发明的目的是提供一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，能够实时获取高精度姿态，进而更好的控制飞行器飞行状态。
为了实现上述目的，本发明采用的技术方案是：
一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，包括如下步骤：
S1：通过状态转移矩阵构建系统状态方程和观测方程，初始化系统噪声Q和量测噪声初值
S2：将本时刻二次估计值与状态转移矩阵相乘得到下一时刻的一次预测状态
S3：计算新息，更新强跟踪滤波器计算过程的变量Vk+1、Mk+1、Nk+1、然后计算次优渐消因子λk+1，并将次优渐消因子拆分成两个开平方的形式；
S4：将步骤S3拆分形式的次优渐消因子分别乘在在一步预测协方差矩阵Pk+1|k的两侧，得到改进的一步预测误差协方差矩阵Pk+1|k；
S5：对下一时刻的噪声方差及噪声均值重新估计，并将估计值引入EKF公式中；
S6：计算卡尔曼增益矩阵Kk+1、状态的二次估计和误差协方差Pk+1|k+1的二次估计，判断是否循环结束，若循环结束转至步骤S7；若未结束则循环转回至步骤S2；
S7：输出实验结果。
本发明技术方案的进一步改进在于：所述步骤S1采用旋转矩阵进行构建状态方程和观测方程，具体过程如下：
式(15)中，θ、φ、φ分别为俯仰角、偏航角、横滚角；
式(16)表示导航系相对于机体系的角速度在机体系的投影，表示的叉乘矩阵，从中提取出第三列可以得到：
其中，x为
bg,k+1＝bg,k+wg,k (18)
式(18)中，bg,k、wg,k分别表示陀螺仪k时刻的漂移和噪声，bg,k+1表示k+1时刻的陀螺仪漂移；
定义系统状态模型：
为了在实际计算机上运行，将式(19)使用一阶差分，使函数离散化，得到系统状态方程：
式(20)中Ta表示两次采样间隔时间，程序中表示积分时间。xk为k刻的状态值，ωb,k为陀螺仪k时刻测量值；
对状态模型式(20)使用泰勒级数展开，得到EKF所需要的信息，则系统状态：
状态转移矩阵和噪声驱动矩阵分别为：
观测值Zk+1为加速度计观测值可表示为：
系统的观测方程可表示：
观测矩阵为：
Hk+1＝[03×3 -gI3] (25)
本发明技术方案的进一步改进在于：所述步骤S3的具体过程为将不同时刻新息序列保持正交，并构建强跟踪滤波器，然后计算次优渐消因子λk+1，并将次优渐消因子拆分成两个开平方的形式，将次优渐消因子的拆分式引入扩展卡尔曼滤波器中。
本发明技术方案的进一步改进在于：所述步骤S5的具体过程为引入噪声自适应机制，以固定初值的噪声，实时动态调整噪声，来适应系统需要具，体公式步骤如下：
Pk+1|k+1＝(I-Kk+1Hk+1)Pk+1|k (14)
上式中，分别为估计噪声方差、噪声均值，Sk+1为新息，R0、r0分别为噪声方差和均值的初值随着k的增加，保证系统有一定的噪声初值,在迭代过程中不至于令卡尔曼增益Kk+1过大或过小，对系统失去调节作用；vk+1、Mk+1、Nk+1为计算次优渐消因子的中间量，λk+1为次优渐消因子，Pk+1|k为一步预测协方差矩阵。
由于采用了上述技术方案，本发明取得的技术效果有：
本发明利用多传感器融合技术，通过旋转矩阵构建系统状态方程和观测方程，易于工程移植。
本发明通过引入强跟踪滤波器，解决了传统扩展卡尔曼滤波器因泰勒展开近似线性化等问题导致的跟踪能力弱的问题，并通过改进次优渐消因子引入形式，保证了协方差矩阵计算结果的对称性，提高了系统跟踪速度。
本发明针对惯性器件具有一定比例的固定噪声的特点，使用噪声自适应估计器，通过加入一定比例的初值噪声初值，同时根据新息变化情况，动态调整噪声均值和方差，解决了惯性器件容易受到环境干扰导致输出值误差较大的问题。
附图说明
图1是本发明算法的框图；
图2是本发明仿真条件下扩展卡尔曼滤波器、强跟踪扩展卡尔曼滤波器、噪声自适应强跟踪扩展卡尔曼滤波器俯仰角(Pitch)对比图；
图3是本发明仿真条件下扩展卡尔曼滤波器、强跟踪扩展卡尔曼滤波器、噪声自适应强跟踪扩展卡尔曼滤波器横滚角(Roll)对比图；
图4是本发明仿真条件下噪声自适应强跟踪扩展卡尔卡尔曼滤波器估计的系统量测噪声方差；
图5是本发明实物下扩展卡尔曼滤波器、强跟踪扩展卡尔曼滤波器、噪声自适应强跟踪扩展卡尔曼滤波器横滚角(Roll)对比图；
图6是本发明实物条件下噪声自适应强跟踪扩展卡尔曼滤波器估计的系统量测噪声方差。
具体实施方式
下面结合附图及具体实施例对本发明做进一步详细说明：
一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，所述四旋翼包含单片机以及与单片机相连的陀螺仪、加速度计、地磁传感器，单片机与地面系统QGC地面站连接通讯；如图1所示，包括如下步骤：
S1：通过状态转移矩阵构建系统状态方程和观测方程，初始化系统噪声Q和量测噪声初值
采用旋转矩阵进行构建状态方程和观测方程，具体过程如下：
式(15)中，θ、φ、φ分别为俯仰角、偏航角、横滚角；
式(16)表示导航系相对于机体系的角速度在机体系的投影，表示的叉乘矩阵，从中提取出第三列可以得到：
其中，x为
bg,k+1＝bg,k+wg,k (18)
式(18)中，bg,k、wg,k分别表示陀螺仪k时刻的漂移和噪声，将k+1时刻陀螺仪漂移bg,k+1也作为系统状态之一；
定义系统状态模型：
为了在实际计算机上运行，将式(19)使用一阶差分，使函数离散化，得到系统状态方程：
式(20)中Ts表示两次采样间隔时间，程序中表示积分时间。xk为k刻的状态值，ωb,k为陀螺仪k时刻测量值；
对状态模型式(20)使用泰勒级数展开，得到EKF所需要的信息，则系统状态：
状态转移矩阵和噪声驱动矩阵分别为：
观测值Zk+1为加速度计观测值可表示为：
系统的观测方程可表示：
观测矩阵为：
Hk+1＝[03×3 -gI3] (25)
S2：将本时刻二次估计值状态转移矩阵相乘得到下一时刻的一次预测状态
S3：计算新息，更新强跟踪滤波器计算过程的变量Vk+1、Mk+1、Nk+1、然后计算次优渐消因子λk+1，并将次优渐消因子拆分成两个开平方的形式。
具体过程为将不同时刻新息序列保持正交，并构建强跟踪滤波器，然后计算次优渐消因子λk+1，并将次优渐消因子拆分成两个开平方的形式，将次优渐消因子的拆分式引入扩展卡尔曼滤波器中。
S4：将步骤S3拆分形式的次优渐消因子分别乘在在一步预测协方差矩阵Pk+1|k的两侧，得到改进的一步预测误差协方差矩阵Pk+1|k；
S5：对下一时刻的噪声方差及噪声均值重新估计，并将估计值引入EKF公式中。
由于步骤S3引入的强跟踪滤波器无法判断单一传感器的准确性，若传感器易受到外界环境影响而引起的新息变化，强跟踪滤波器可能将误差一并引入到系统中，故步骤S5针对惯性传感器的特性，引入噪声自适应机制，以固定初值的噪声，实时动态调整噪声，来适应系统需要，具体公式步骤如下：
Pk+1|k+1＝(I-Kk+1Hk+1)Pk+1|k (14)
上式中，分别为估计噪声方差、噪声均值，R0、r0分别为噪声方差和均值的初值随着k的增加，保证系统有一定的噪声初值,在迭代过程中不至于令卡尔曼增益Kk+1过大或过小，对系统失去调节作用；Vk+1、Mk+1、Nk+1为计算次优渐消因子的中间量，λk+1为次优渐消因子，Pk+1|k为一步预测协方差矩阵。
S6：计算卡尔曼增益矩阵Kk+1、状态的二次估计和误差协方差Pk+1|k+1的二次估计，判断是否循环结束，若循环结束转至步骤S7；若未结束则循环转回至步骤S2。
S7：输出实验结果。
步骤S2、S4、S6利用扩展卡尔曼滤波器估算出最优姿态角，具体步骤如下：首先，利用步骤S2计算一步预测，通过上一时刻估计的状态值与本时刻状态转移矩阵相乘得到状态的一步骤预测，其次，通过步骤S4计算雅克比矩阵并计算改进的一步预测误差协方差矩阵Pk+1|k，最后通过步骤S6计算卡尔曼增益矩阵Kk+1，状态的二次估计，误差协方差Pk+1|k+1的二次估计，为下一次迭代更新做准备。
本发明的噪声自适应强跟踪扩展卡尔曼滤波器考虑到了惯性器件的噪声对系统的影响，强跟踪滤波器解决了扩展卡尔曼滤波器因泰勒展开，近似线性化的模型不准确，再加之历史值占比大，导致跟踪能力差的问题，同时针对惯性器件的特点对噪声自适应估计器进行动态估计，通过引入噪声初值，当系统在平稳状态向运动状态过渡时，卡尔曼增益能快速调节，不会因传感器噪声的影响将误差引入系统，提高姿态的解算精度。
图2、图3分别是仿真环境下扩展卡尔曼滤波器、强跟踪扩展卡尔曼滤波器、噪声自适应强跟踪扩展卡尔曼滤波器俯仰角(Pitch)、横滚角(Roll)对比图；可以明显看出，扩展卡尔曼滤波器跟踪速度最慢，状态产生突变后，很慢进入期望目标附近，强跟踪扩展卡尔曼滤波器，则较快的到达期望目标，但由于强跟踪滤波器的特点，惯性器件加速度计受外界环境影响较大，会将误差引入系统中，图中表现为跟踪速度快，但超调量较大，本申请提出的噪声自适应强跟踪扩展卡尔曼滤波器在跟踪速度上不低于强跟踪滤波器，但几乎无超调，平滑进入期望状态。
图4是噪声自适应强跟踪扩展卡尔曼滤波器估计的噪声方差值，对比图3图2来看，在系统状态有较大突变时刻，系统通过新息Sk+1实时估计的量测方程也有较大变化，来适应系统噪声变化情况。
图5是实物下扩展卡尔曼滤波器、强跟踪扩展卡尔曼滤波器、噪声自适应强跟踪扩展卡尔曼滤波器横滚角(Roll)对比图，从图5中可以看出，实物环境下和仿真环境下具有一致结果。
图6是实物条件下噪声自适应强跟踪扩展卡尔曼滤波器估计的系统量测噪声方差，对比图5，从图6中可以看出，在系统状态有较大变化时候，因为噪声自适应估计器的作用，使系统实时动态调节观测噪声，但与图4比较，其调节力度明显增大，这是因为仿真环境下传感器的噪声值是人为规定的高斯噪声，但实际情况中，在线实验噪声方差变化幅度更大，如图6噪声方差变化幅度达到250％，其原因是实物实验数据来源于真实传感器，以及更多环境干扰等因素的叠加，故在实物实验中有较大体现。整体在线实物实验与离线仿真实验具有一致结果。
Claims (4)
1.一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，其特征在于，包括如下步骤：
S1：通过状态转移矩阵构建系统状态方程和观测方程，初始化系统噪声Q和量测噪声初值
S2：将本时刻二次估计值与状态转移矩阵相乘得到下一时刻的一次预测状态
S3：计算新息，更新强跟踪滤波器计算过程的变量Vk+1、Mk+1、Nk+1，然后计算次优渐消因子λk+1，并将次优渐消因子拆分成两个开平方的形式；
S4：将步骤S3拆分形式的次优渐消因子分别乘在在一步预测协方差矩阵Pk+1|k的两侧，得到改进的一步预测误差协方差矩阵Pk+1|k；
S5：对下一时刻的噪声方差及噪声均值重新估计，并将估计值引入EKF公式中；
S6：计算卡尔曼增益矩阵Kk+1、状态的二次估计和误差协方差Pk+1|k+1的二次估计，判断是否循环结束，若循环结束转至步骤S7；若未结束则循环转回至步骤S2；
S7：输出实验结果。
2.根据权利要求1所述的一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，其特征在于：所述步骤S1采用旋转矩阵进行构建状态方程和观测方程，具体过程如下：
式(15)中，θ、φ分别为俯仰角、偏航角、横滚角；
式(16)表示导航系相对于机体系的角速度在机体系的投影，表示的叉乘矩阵，从中提取出第三列可以得到：
其中，x为
bg，k+1＝bg，k+wg，k (18)
式(18)中，bg，k、wg，k分别表示陀螺仪k时刻的漂移和噪声，bg，k+1表示k+1时刻的陀螺仪漂移；
定义系统状态模型：
为了在实际计算机上运行，将式(19)使用一阶差分，使函数离散化，得到系统状态方程：
式(20)中Ts表示两次采样间隔时间，程序中表示积分时间；xk为k刻的状态值，ωb，k为陀螺仪k时刻测量值；
对状态模型式(20)使用泰勒级数展开，得到EKF所需要的信息，则系统状态：
状态转移矩阵和噪声驱动矩阵分别为：
观测值Zk+1为加速度计观测值可表示为：
系统的观测方程可表示：
观测矩阵为：
Hk+1＝[03×3 -gI3] (25)。
3.根据权利要求1所述的一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，其特征在于：所述步骤S3的具体过程为将不同时刻新息序列保持正交，并构建强跟踪滤波器，然后计算次优渐消因子λk+1，并将次优渐消因子拆分成两个开平方的形式，将次优渐消因子的拆分式引入扩展卡尔曼滤波器中。
4.根据权利要求1所述的一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，其特征在于：所述步骤S5的具体过程为引入噪声自适应机制，以固定初值的噪声，实时动态调整噪声，来适应系统需要具，体公式步骤如下：
Pk+1|k+1＝(I-Kk+1Hk+1)Pk+1|k (14)
上式中，分别为估计噪声方差、噪声均值，Sk+1为新息，R0、r0分别为噪声方差和均值的初值随着k的增加，保证系统有一定的噪声初值，在迭代过程中不至于令卡尔曼增益Kk+1过大或过小，对系统失去调节作用；Vk+1、Mk+1、Nk+1为计算次优渐消因子的中间量，λk+1为次优渐消因子，Pk+1|k为一步预测协方差矩阵。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210471564.8A CN114964226B (zh) | 2022-04-29 | 2022-04-29 | 噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210471564.8A CN114964226B (zh) | 2022-04-29 | 2022-04-29 | 噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114964226A CN114964226A (zh) | 2022-08-30 |
| CN114964226B true CN114964226B (zh) | 2024-09-20 |
Family
ID=82979544
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210471564.8A Active CN114964226B (zh) | 2022-04-29 | 2022-04-29 | 噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114964226B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116955444B (zh) * | 2023-06-15 | 2024-08-23 | 刘富 | 基于大数据分析的采集噪声点挖掘方法及系统 |
| CN117879540B (zh) * | 2024-03-12 | 2024-06-18 | 西南应用磁学研究所(中国电子科技集团公司第九研究所) | 基于改进卡尔曼滤波的磁罗盘传感器自适应信号滤波方法 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108225308A (zh) * | 2017-11-23 | 2018-06-29 | 东南大学 | 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法 |
| CN109000642A (zh) * | 2018-05-25 | 2018-12-14 | 哈尔滨工程大学 | 一种改进的强跟踪容积卡尔曼滤波组合导航方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112284414B (zh) * | 2020-10-10 | 2022-06-28 | 东南大学 | 一种基于多渐消因子自适应动机座旋转调制精对准方法 |
-
2022
- 2022-04-29 CN CN202210471564.8A patent/CN114964226B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108225308A (zh) * | 2017-11-23 | 2018-06-29 | 东南大学 | 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法 |
| CN109000642A (zh) * | 2018-05-25 | 2018-12-14 | 哈尔滨工程大学 | 一种改进的强跟踪容积卡尔曼滤波组合导航方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114964226A (zh) | 2022-08-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110398257B (zh) | Gps辅助的sins系统快速动基座初始对准方法 | |
| CN109211276B (zh) | 基于gpr与改进的srckf的sins初始对准方法 | |
| CN111024064B (zh) | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 | |
| CN107525503B (zh) | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 | |
| CN114964226B (zh) | 噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法 | |
| CN106597017B (zh) | 一种基于扩展卡尔曼滤波的无人机角加速度估计方法及装置 | |
| CN108827299A (zh) | 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法 | |
| CN106643737A (zh) | 风力干扰环境下四旋翼飞行器姿态解算方法 | |
| CN109945859B (zh) | 一种自适应h∞滤波的运动学约束捷联惯性导航方法 | |
| CN105136145A (zh) | 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法 | |
| CN105929836B (zh) | 用于四旋翼飞行器的控制方法 | |
| CN103822633A (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
| CN106500693A (zh) | 一种基于自适应扩展卡尔曼滤波的ahrs算法 | |
| CN111189442B (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
| CN106441291B (zh) | 一种基于强跟踪sdre滤波的组合导航系统及导航方法 | |
| CN110941285A (zh) | 一种基于双ip核的无人机飞行控制系统 | |
| CN111750865A (zh) | 一种用于双功能深海无人潜器导航系统的自适应滤波导航方法 | |
| CN111190207B (zh) | 基于pstcsdref算法的无人机ins bds组合导航方法 | |
| CN114966629A (zh) | 一种基于ekf算法框架的车体激光雷达外参标定方法 | |
| CN113916226B (zh) | 一种基于最小方差的组合导航系统抗扰滤波方法 | |
| CN111982126B (zh) | 一种全源BeiDou/SINS弹性状态观测器模型设计方法 | |
| Crocoll et al. | Quadrotor inertial navigation aided by a vehicle dynamics model with in-flight parameter estimation | |
| CN114384932B (zh) | 一种基于距离测量的无人机导航对接方法 | |
| CN112683265B (zh) | 一种基于快速iss集员滤波的mimu/gps组合导航方法 | |
| CN110470294B (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 |