CN114964226A - 噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法 - Google Patents

噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法 Download PDF

Info

Publication number
CN114964226A
CN114964226A CN202210471564.8A CN202210471564A CN114964226A CN 114964226 A CN114964226 A CN 114964226A CN 202210471564 A CN202210471564 A CN 202210471564A CN 114964226 A CN114964226 A CN 114964226A
Authority
CN
China
Prior art keywords
noise
state
kalman filter
extended kalman
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.)
Pending
Application number
CN202210471564.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.)
Yanshan University
Original Assignee
Yanshan 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 Yanshan University filed Critical Yanshan University
Priority to CN202210471564.8A priority Critical patent/CN114964226A/zh
Publication of CN114964226A publication Critical patent/CN114964226A/zh
Pending legal-status Critical Current

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/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和量测噪声初值
Figure BDA0003622694520000011
S2：将本时刻二次估计值
Figure BDA0003622694520000012
与状态转移矩阵
Figure BDA0003622694520000013
相乘得到下一时刻的一次预测状态
Figure BDA0003622694520000021
S3：计算新息，更新强跟踪滤波器计算过程的变量Vk+1、Mk+1、Nk+1、然后计算次优渐消因子λk+1，并将次优渐消因子拆分成两个开平方的形式；
S4：将步骤S3拆分形式的次优渐消因子分别乘在在一步预测协方差矩阵Pk+1|k的两侧，得到改进的一步预测误差协方差矩阵Pk+1|k；
S5：对下一时刻的噪声方差
Figure BDA0003622694520000022
及噪声均值
Figure BDA0003622694520000023
重新估计，并将估计值引入EKF公式中；
S6：计算卡尔曼增益矩阵Kk+1、状态
Figure BDA0003622694520000024
的二次估计和误差协方差Pk+1|k+1的二次估计，判断是否循环结束，若循环结束转至步骤S7；若未结束则循环转回至步骤S2；
S7：输出实验结果。
本发明技术方案的进一步改进在于：所述步骤S1采用旋转矩阵
Figure BDA0003622694520000025
进行构建状态方程和观测方程，具体过程如下：
Figure BDA0003622694520000026
式(15)中，θ、φ、φ分别为俯仰角、偏航角、横滚角；
Figure BDA0003622694520000027
式(16)
Figure BDA0003622694520000028
表示导航系相对于机体系的角速度在机体系的投影，
Figure BDA0003622694520000029
表示
Figure BDA00036226945200000210
的叉乘矩阵，从中提取出第三列可以得到：
Figure BDA00036226945200000211
其中，x为
Figure BDA00036226945200000212
bg,k+1＝bg,k+wg,k (18)
式(18)中，bg,k、wg,k分别表示陀螺仪k时刻的漂移和噪声，bg,k+1表示k+1时刻的陀螺仪漂移；
定义系统状态模型：
Figure BDA0003622694520000031
为了在实际计算机上运行，将式(19)使用一阶差分，使函数离散化，得到系统状态方程：
Figure BDA0003622694520000032
式(20)中Ta表示两次采样间隔时间，程序中表示积分时间。xk为k刻的状态值，ωb,k为陀螺仪k时刻测量值；
对状态模型式(20)使用泰勒级数展开，得到EKF所需要的信息，则系统状态：
Figure BDA0003622694520000033
状态转移矩阵和噪声驱动矩阵分别为：
Figure BDA0003622694520000034
观测值Zk+1为加速度计观测值可表示为：
Figure BDA0003622694520000035
系统的观测方程可表示：
Figure BDA0003622694520000036
观测矩阵为：
Hk+1＝[03×3 -gI3] (25)
本发明技术方案的进一步改进在于：所述步骤S3的具体过程为将不同时刻新息序列保持正交，并构建强跟踪滤波器，然后计算次优渐消因子λk+1，并将次优渐消因子拆分成两个开平方的形式，将次优渐消因子的拆分式引入扩展卡尔曼滤波器中。
本发明技术方案的进一步改进在于：所述步骤S5的具体过程为引入噪声自适应机制，以固定初值的噪声，实时动态调整噪声，来适应系统需要具，体公式步骤如下：
Figure BDA0003622694520000041
Figure BDA0003622694520000042
Figure BDA0003622694520000043
Figure BDA0003622694520000044
Figure BDA0003622694520000045
Figure BDA0003622694520000046
Figure BDA0003622694520000047
Figure BDA0003622694520000048
Figure BDA0003622694520000049
Figure BDA00036226945200000410
Figure BDA00036226945200000411
Figure BDA00036226945200000412
Figure BDA00036226945200000413
Pk+1|k+1＝(I-Kk+1Hk+1)Pk+1|k (14)
上式中，
Figure BDA0003622694520000051
分别为估计噪声方差、噪声均值，Sk+1为新息，R0、r0分别为噪声方差和均值的初值随着k的增加，保证系统有一定的噪声初值,在迭代过程中不至于令卡尔曼增益Kk+1过大或过小，对系统失去调节作用；vk+1、Mk+1、Nk+1为计算次优渐消因子的中间量，λk+1为次优渐消因子，
Figure BDA0003622694520000052
Pk+1|k为一步预测协方差矩阵。
由于采用了上述技术方案，本发明取得的技术效果有：
本发明利用多传感器融合技术，通过旋转矩阵构建系统状态方程和观测方程，易于工程移植。
本发明通过引入强跟踪滤波器，解决了传统扩展卡尔曼滤波器因泰勒展开近似线性化等问题导致的跟踪能力弱的问题，并通过改进次优渐消因子引入形式，保证了协方差矩阵计算结果的对称性，提高了系统跟踪速度。
本发明针对惯性器件具有一定比例的固定噪声的特点，使用噪声自适应估计器，通过加入一定比例的初值噪声初值，同时根据新息变化情况，动态调整噪声均值和方差，解决了惯性器件容易受到环境干扰导致输出值误差较大的问题。
附图说明
图1是本发明算法的框图；
图2是本发明仿真条件下扩展卡尔曼滤波器、强跟踪扩展卡尔曼滤波器、噪声自适应强跟踪扩展卡尔曼滤波器俯仰角(Pitch)对比图；
图3是本发明仿真条件下扩展卡尔曼滤波器、强跟踪扩展卡尔曼滤波器、噪声自适应强跟踪扩展卡尔曼滤波器横滚角(Roll)对比图；
图4是本发明仿真条件下噪声自适应强跟踪扩展卡尔卡尔曼滤波器估计的系统量测噪声方差；
图5是本发明实物下扩展卡尔曼滤波器、强跟踪扩展卡尔曼滤波器、噪声自适应强跟踪扩展卡尔曼滤波器横滚角(Roll)对比图；
图6是本发明实物条件下噪声自适应强跟踪扩展卡尔曼滤波器估计的系统量测噪声方差。
具体实施方式
下面结合附图及具体实施例对本发明做进一步详细说明：
一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，所述四旋翼包含单片机以及与单片机相连的陀螺仪、加速度计、地磁传感器，单片机与地面系统QGC地面站连接通讯；如图1所示，包括如下步骤：
S1：通过状态转移矩阵构建系统状态方程和观测方程，初始化系统噪声Q和量测噪声初值
Figure BDA0003622694520000061
采用旋转矩阵
Figure BDA0003622694520000062
进行构建状态方程和观测方程，具体过程如下：
Figure BDA0003622694520000063
式(15)中，θ、φ、φ分别为俯仰角、偏航角、横滚角；
Figure BDA0003622694520000064
式(16)
Figure BDA0003622694520000065
表示导航系相对于机体系的角速度在机体系的投影，
Figure BDA0003622694520000066
表示
Figure BDA0003622694520000067
的叉乘矩阵，从中提取出第三列可以得到：
Figure BDA0003622694520000068
其中，x为
Figure BDA0003622694520000069
bg,k+1＝bg,k+wg,k (18)
式(18)中，bg,k、wg,k分别表示陀螺仪k时刻的漂移和噪声，将k+1时刻陀螺仪漂移bg,k+1也作为系统状态之一；
定义系统状态模型：
Figure BDA00036226945200000610
为了在实际计算机上运行，将式(19)使用一阶差分，使函数离散化，得到系统状态方程：
Figure BDA0003622694520000071
式(20)中Ts表示两次采样间隔时间，程序中表示积分时间。xk为k刻的状态值，ωb,k为陀螺仪k时刻测量值；
对状态模型式(20)使用泰勒级数展开，得到EKF所需要的信息，则系统状态：
Figure BDA0003622694520000072
状态转移矩阵和噪声驱动矩阵分别为：
Figure BDA0003622694520000073
观测值Zk+1为加速度计观测值可表示为：
Figure BDA0003622694520000074
系统的观测方程可表示：
Figure BDA0003622694520000075
观测矩阵为：
Hk+1＝[03×3 -gI3] (25)
S2：将本时刻二次估计值
Figure BDA0003622694520000076
状态转移矩阵
Figure BDA0003622694520000077
相乘得到下一时刻的一次预测状态
Figure BDA0003622694520000081
S3：计算新息，更新强跟踪滤波器计算过程的变量Vk+1、Mk+1、Nk+1、然后计算次优渐消因子λk+1，并将次优渐消因子拆分成两个开平方的形式。
具体过程为将不同时刻新息序列保持正交，并构建强跟踪滤波器，然后计算次优渐消因子λk+1，并将次优渐消因子拆分成两个开平方的形式，将次优渐消因子的拆分式引入扩展卡尔曼滤波器中。
S4：将步骤S3拆分形式的次优渐消因子分别乘在在一步预测协方差矩阵Pk+1|k的两侧，得到改进的一步预测误差协方差矩阵Pk+1|k；
S5：对下一时刻的噪声方差
Figure BDA0003622694520000082
及噪声均值
Figure BDA0003622694520000083
重新估计，并将估计值引入EKF公式中。
由于步骤S3引入的强跟踪滤波器无法判断单一传感器的准确性，若传感器易受到外界环境影响而引起的新息变化，强跟踪滤波器可能将误差一并引入到系统中，故步骤S5针对惯性传感器的特性，引入噪声自适应机制，以固定初值的噪声，实时动态调整噪声，来适应系统需要，具体公式步骤如下：
Figure BDA0003622694520000084
Figure BDA0003622694520000085
Figure BDA0003622694520000086
Figure BDA0003622694520000087
Figure BDA0003622694520000088
Figure BDA0003622694520000089
Figure BDA00036226945200000810
Figure BDA0003622694520000091
Figure BDA0003622694520000092
Figure BDA0003622694520000093
Figure BDA0003622694520000094
Figure BDA0003622694520000095
Figure BDA0003622694520000096
Pk+1|k+1＝(I-Kk+1Hk+1)Pk+1|k (14)
上式中，
Figure BDA0003622694520000097
分别为估计噪声方差、噪声均值，R0、r0分别为噪声方差和均值的初值随着k的增加，保证系统有一定的噪声初值,在迭代过程中不至于令卡尔曼增益Kk+1过大或过小，对系统失去调节作用；Vk+1、Mk+1、Nk+1为计算次优渐消因子的中间量，λk+1为次优渐消因子，Pk+1|k为一步预测协方差矩阵。
S6：计算卡尔曼增益矩阵Kk+1、状态
Figure BDA0003622694520000098
的二次估计和误差协方差Pk+1|k+1的二次估计，判断是否循环结束，若循环结束转至步骤S7；若未结束则循环转回至步骤S2。
S7：输出实验结果。
步骤S2、S4、S6利用扩展卡尔曼滤波器估算出最优姿态角，具体步骤如下：首先，利用步骤S2计算一步预测，通过上一时刻估计的状态值与本时刻状态转移矩阵相乘得到状态的一步骤预测，其次，通过步骤S4计算雅克比矩阵
Figure BDA0003622694520000099
并计算改进的一步预测误差协方差矩阵Pk+1|k，最后通过步骤S6计算卡尔曼增益矩阵Kk+1，状态
Figure BDA00036226945200000910
的二次估计，误差协方差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和量测噪声初值
Figure FDA0003622694510000011
S2：将本时刻二次估计值
Figure FDA0003622694510000012
与状态转移矩阵
Figure FDA0003622694510000013
相乘得到下一时刻的一次预测状态
Figure FDA0003622694510000014
S3：计算新息，更新强跟踪滤波器计算过程的变量Vk+1、Mk+1、Nk+1，然后计算次优渐消因子λk+1，并将次优渐消因子拆分成两个开平方的形式；
S4：将步骤S3拆分形式的次优渐消因子分别乘在在一步预测协方差矩阵Pk+1|k的两侧，得到改进的一步预测误差协方差矩阵Pk+1|k；
S5：对下一时刻的噪声方差
Figure FDA0003622694510000015
及噪声均值
Figure FDA0003622694510000016
重新估计，并将估计值引入EKF公式中；
S6：计算卡尔曼增益矩阵Kk+1、状态
Figure FDA0003622694510000017
的二次估计和误差协方差Pk+1|k+1的二次估计，判断是否循环结束，若循环结束转至步骤S7；若未结束则循环转回至步骤S2；
S7：输出实验结果。
2.根据权利要求1所述的一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，其特征在于：所述步骤S1采用旋转矩阵
Figure FDA00036226945100000115
进行构建状态方程和观测方程，具体过程如下：
Figure FDA0003622694510000018
式(15)中，θ、
Figure FDA0003622694510000019
φ分别为俯仰角、偏航角、横滚角；
Figure FDA00036226945100000110
式(16)
Figure FDA00036226945100000111
表示导航系相对于机体系的角速度在机体系的投影，
Figure FDA00036226945100000112
表示
Figure FDA00036226945100000113
的叉乘矩阵，从中提取出第三列可以得到：
Figure FDA00036226945100000114
其中，x为
Figure FDA0003622694510000021
bg，k+1＝bg，k+wg，k (18)
式(18)中，bg，k、wg，k分别表示陀螺仪k时刻的漂移和噪声，bg，k+1表示k+1时刻的陀螺仪漂移；
定义系统状态模型：
Figure FDA0003622694510000022
为了在实际计算机上运行，将式(19)使用一阶差分，使函数离散化，得到系统状态方程：
Figure FDA0003622694510000023
式(20)中Ts表示两次采样间隔时间，程序中表示积分时间。xk为k刻的状态值，ωb，k为陀螺仪k时刻测量值；
对状态模型式(20)使用泰勒级数展开，得到EKF所需要的信息，则系统状态：
Figure FDA0003622694510000024
状态转移矩阵和噪声驱动矩阵分别为：
Figure FDA0003622694510000025
观测值Zk+1为加速度计观测值可表示为：
Figure FDA0003622694510000026
系统的观测方程可表示：
Figure FDA0003622694510000031
观测矩阵为：
Hk+1＝[03×3 -gI3] (25)。
3.根据权利要求1所述的一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，其特征在于：所述步骤S3的具体过程为将不同时刻新息序列保持正交，并构建强跟踪滤波器，然后计算次优渐消因子λk+1，并将次优渐消因子拆分成两个开平方的形式，将次优渐消因子的拆分式引入扩展卡尔曼滤波器中。
4.根据权利要求1所述的一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，其特征在于：所述步骤S5的具体过程为引入噪声自适应机制，以固定初值的噪声，实时动态调整噪声，来适应系统需要具，体公式步骤如下：
Figure FDA0003622694510000032
Figure FDA0003622694510000033
Figure FDA0003622694510000034
Figure FDA0003622694510000035
Figure FDA0003622694510000036
Figure FDA0003622694510000037
Figure FDA0003622694510000038
Figure FDA0003622694510000041
Figure FDA0003622694510000042
Figure FDA0003622694510000043
Figure FDA0003622694510000044
Figure FDA0003622694510000045
Figure FDA0003622694510000046
Pk+1|k+1＝(I-Kk+1Hk+1)Pk+1|k (14)
上式中，
Figure FDA0003622694510000047
分别为估计噪声方差、噪声均值，Sk+1为新息，R0、r0分别为噪声方差和均值的初值随着k的增加，保证系统有一定的噪声初值，在迭代过程中不至于令卡尔曼增益Kk+1过大或过小，对系统失去调节作用；Vk+1、Mk+1、Nk+1为计算次优渐消因子的中间量，λk+1为次优渐消因子，
Figure FDA0003622694510000048
Pk+1|k为一步预测协方差矩阵。
CN202210471564.8A 2022-04-29 2022-04-29 噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法 Pending CN114964226A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210471564.8A CN114964226A (zh) 2022-04-29 2022-04-29 噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210471564.8A CN114964226A (zh) 2022-04-29 2022-04-29 噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法

Publications (1)

Publication Number Publication Date
CN114964226A true CN114964226A (zh) 2022-08-30

Family

ID=82979544

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210471564.8A Pending CN114964226A (zh) 2022-04-29 2022-04-29 噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法

Country Status (1)

Country Link
CN (1) CN114964226A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116955444A (zh) * 2023-06-15 2023-10-27 共享易付(广州)网络科技有限公司 基于大数据分析的采集噪声点挖掘方法及系统
CN117879540A (zh) * 2024-03-12 2024-04-12 西南应用磁学研究所(中国电子科技集团公司第九研究所) 基于改进卡尔曼滤波的磁罗盘传感器自适应信号滤波方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116955444A (zh) * 2023-06-15 2023-10-27 共享易付(广州)网络科技有限公司 基于大数据分析的采集噪声点挖掘方法及系统
CN117879540A (zh) * 2024-03-12 2024-04-12 西南应用磁学研究所(中国电子科技集团公司第九研究所) 基于改进卡尔曼滤波的磁罗盘传感器自适应信号滤波方法

Similar Documents

Publication Publication Date Title
CN109211276B (zh) 基于gpr与改进的srckf的sins初始对准方法
CN110398257B (zh) Gps辅助的sins系统快速动基座初始对准方法
CN101033973B (zh) 微小型飞行器微惯性组合导航系统的姿态确定方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN114964226A (zh) 噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法
CN106500693B (zh) 一种基于自适应扩展卡尔曼滤波的ahrs算法
CN103822633A (zh) 一种基于二阶量测更新的低成本姿态估计方法
CN109945859B (zh) 一种自适应h∞滤波的运动学约束捷联惯性导航方法
CN109945895B (zh) 基于渐消平滑变结构滤波的惯性导航初始对准方法
CN105865446A (zh) 基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法
CN111189442B (zh) 基于cepf的无人机多源导航信息状态预测方法
CN111750865B (zh) 一种用于双功能深海无人潜器导航系统的自适应滤波导航方法
EP4053504B1 (en) Systems and methods for model based inertial navigation for a spinning projectile
CN110941285A (zh) 一种基于双ip核的无人机飞行控制系统
CN111190207B (zh) 基于pstcsdref算法的无人机ins bds组合导航方法
Liu et al. Strong tracking UKF-based hybrid algorithm and its application to initial alignment of rotating SINS with large misalignment angles
Zorina et al. Enhancement of INS/GNSS integration capabilities for aviation-related applications
Nguyen et al. Aerodynamic characteristics of quadrotor helicopter
Crocoll et al. Quadrotor inertial navigation aided by a vehicle dynamics model with in-flight parameter estimation
CN113916226B (zh) 一种基于最小方差的组合导航系统抗扰滤波方法
Yang et al. Model-free integrated navigation of small fixed-wing UAVs full state estimation in wind disturbance
Yu et al. Design of Multi-sensor Fusion Positioning and Navigation Method Based on Federated Kalman Filter for Low-Speed Rotorcraft of Atmospheric Detection
Khaghani et al. Autonomous navigation of small UAVs based on vehicle dynamic model
Ping-an et al. Research on attitude measurement method of special aircraft using geomagnetic sensor/gyroscope based on UKF
Jarrell et al. Aircraft attitude, position, and velocity determination using sensor fusion

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