The invention belongs to the field of data fusion of magnetometer and inertial navigation system and the field of adaptive Kalman filter, and specifically relates to an Adaptive Anti-jamming method for the combination of inertia and magnetometer. Firstly, the output model of magnetometer and gyroscope is used to derive the model of updating magnetometer output by angular rate, and then the direct output of magnetometer is substituted into filter as measurement. At the same time, the invention designs an adaptive extended Kalman filter based on robust control. The noise variance matrix of measurement information is calculated according to the residual between measurement and pre-measurement, so that interference can be effectively suppressed. At the same time, the duration of interference is judged. When the interference is judged to be long-time interference, the magnetic field value is extrapolated by inertial navigation system at the initial stage of interference, and the magnetometer is quickly re-calibrated. After calibration, the magnetometer calibration parameters are replaced by anti-interference module, so as to ensure the anti-interference effect for a long time.
【技术实现步骤摘要】
一种惯性和磁力计组合自适应抗干扰方法
本专利技术属于磁力计和惯性导航系统数据融合领域和自适应卡尔曼滤波领域，具体涉及一种惯性和磁力计组合自适应抗干扰方法。
技术介绍
随着微电子机械系统，MEMS，的发展，低成本的惯性导航系统通常被集成在了一个模块或芯片里，并且被广泛地应用于室内定位、人体运动跟踪、无人系统等。但是随着成本的降低，MEMS惯导系统的精度也随之降低，无法敏感到地球自转，所以MEMS惯导系统的对准只能依赖于其他传感器。磁力计可以利用测量到的磁场值解算出方位。所以，MEMS磁力计与惯性导航系统的组合是常用的组合导航系统之一。对于磁力计和磁力计/惯导组合的研究大多集中于对磁力计的前期标定。由于磁力计容易受到干扰的缺点，所以在使用磁力计前需要对其进行标定修正。常用的磁力计标定方法分为两类，离线标定方法和在线标定方法。离线标定磁力计的方法主要有基于椭球拟合法和基于粒子群优化的标定法。文献“Fang,J.,Sun,H.,Cao,J.,Zhang,X.,&Tao,Y.(2011).Anovelcalibrationmethodofmagneticcompas...
【技术保护点】
1.一种惯性和磁力计组合自适应抗干扰方法，其特征在于，该方法包括以下步骤：步骤1：建立磁力计输出模型和陀螺仪的输出模型；步骤2：以角速率为输入量，建立利用角速率更新磁力计输出的模型；步骤3：利用预测量和量测量之间的残差量，实时更新卡尔曼滤波器的量测噪声方差阵，得到自适应滤波器；步骤4：当遇到长时间的干扰问题时，通过对量测噪声方差阵的放大系数进行调整，同时利用惯性传感器信息对磁力计进行快速标定，得到重标定模块；步骤5：联级自适应抗干扰模型和重标定模块。
【技术特征摘要】
1.一种惯性和磁力计组合自适应抗干扰方法，其特征在于，该方法包括以下步骤：步骤1：建立磁力计输出模型和陀螺仪的输出模型；步骤2：以角速率为输入量，建立利用角速率更新磁力计输出的模型；步骤3：利用预测量和量测量之间的残差量，实时更新卡尔曼滤波器的量测噪声方差阵，得到自适应滤波器；步骤4：当遇到长时间的干扰问题时，通过对量测噪声方差阵的放大系数进行调整，同时利用惯性传感器信息对磁力计进行快速标定，得到重标定模块；步骤5：联级自适应抗干扰模型和重标定模块。2.根据权利要求1所述的一种惯性和磁力计组合自适应抗干扰方法，其特征在于，步骤1所述的磁力计输出模型为：其中，为载体系磁力计在k时刻的输出，为地理系下真实的地磁场分量，并且它符合IGRF的模型，为地理系到载体系的旋转矩阵，bbi为硬磁干扰向量的输出偏置，bb为磁力计的输出偏置，εm为磁力计的输出噪声，它符合高斯白噪声，Csi为软磁干扰矩阵，表示为下式：Csc为标度因数矩阵，表示为下式：Cno为非正交矩阵，表示为下式：磁力计的输出可被简化为：其中，Cm＝CscCnoCsi，bm＝CscCnobbi+bb；步骤1所述的陀螺仪的输出模型为：其中，为载体系下陀螺仪的直接输出，为载体系下陀螺仪的常值偏置，Cg为标度因数矩阵、非正交矩阵和失准矩阵的集合，εg为陀螺仪的输出噪声，它符合高斯白噪声，Cg已经在出厂前被标定好了，于是陀螺仪的输出模型可被简化为：3.根据权利要求2所述的一种惯性和磁力计组合自适应抗干扰方法，其特征在于，步骤2建立利用角速率更新磁力计输出的模型：其中，是磁场分量在载体系下的投影k时刻的输出，是磁场分量在载体系下的投影k-1时刻的输出，是陀螺仪在k时刻的输出，ts是系统的输出周期，I3是一个三维的单位矩阵，εm是磁力计的输出噪声，符合高斯白噪声，Cm是磁力计标度因数矩阵、非正交矩阵和软磁干扰矩阵的集合，bm是磁力计输出偏置向量和硬磁干扰向量的集合。4.根据权利要求3所述的一种惯性和磁力计组合自适应抗干扰方法，...
【专利技术属性】
技术研发人员：王伟，吴宗凯，王峰，李欣，黄平，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江,23
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。