[发明专利]一种惯性和磁力计组合自适应抗干扰方法

权利要求书

1.一种惯性和磁力计组合自适应抗干扰方法，其特征在于，该方法包括以下步骤：

步骤1：建立简化磁力计输出模型和陀螺仪的输出模型；

简化磁力计输出模型：

其中，为载体系下磁力计在k时刻的输出；为地理系下真实的地磁场分量，并且它符合IGRF的模型；ε_m为磁力计的输出噪声，它符合高斯白噪声；C_m是磁力计标度因数矩阵、非正交矩阵和软磁干扰矩阵的集合，b_m是磁力计输出偏置向量和硬磁干扰向量的集合，C_m和b_m被认为是常值，并且在出厂前均被初步标定完成；

简化陀螺仪的输出模型：

其中，为载体系下陀螺仪的直接输出；是陀螺仪在k时刻的输出；为载体系下陀螺仪的常值偏置；ε_g为陀螺仪的输出噪声，它符合高斯白噪声；

步骤2：以角速率为输入量，建立利用角速率更新磁力计输出的模型；

通过角速率更新磁力计输出的模型：

其中，是载体系下磁力计在k-1时刻的输出；是由角速率的反对准矩阵；t_s是陀螺仪的更新周期；I₃是一个三维的单位矩阵；

步骤3：利用预测量和量测量之间的残差量，实时更新卡尔曼滤波器的量测噪声方差阵，得到自适应滤波器；

滤波器设置中，状态变量选择载体系下的磁场分量和载体系下陀螺仪的偏置，即为自适应滤波器的设计如下：

(1)时间更新：

(2)新息方差估计：

(3)量测更新：

其中，f(·)为状态转移函数；Φ_k,k-1为状态转移函数的雅克比矩阵；V_k为状态预测量与量测量的残差；C_k为由残差计算出的量测量噪声方差矩阵；

步骤4：当遇到长时间的干扰问题时，通过对量测噪声方差阵的放大系数进行调整，同时利用惯性传感器信息对磁力计进行快速标定，得到重标定模块；

短时间变化的干扰出现，由自适应滤波器进行抗干扰；当遇到长时间的干扰问题时，使用陀螺仪的特性来对磁力计进行快速重标定，标定完成后就可以使用磁力计对系统姿态进行修正，从而保持长时间的精度；此类干扰体现出在磁力计的偏置变化中，所以重标定系统如下设计：

状态模型：

量测模型：

其中，由前提抗干扰模块提供；k_b为变化的斜率；由于出现此类干扰时可能是突发的，也可能是缓慢出现的，所以需要根据变化的斜率判断估计出的偏置是否可用，s是出现干扰后经过的时间；

联级自适应抗干扰模型和重标定模块的步骤如下：

步骤4.1：首先运行自适应抗干扰模块；

步骤4.2：判断是否被干扰；如果无干扰，则输出抗干扰模型的输出，计算姿态；

步骤4.3：判断出被干扰后首先记录的初值然后运行重标定模块，如果判断出干扰消失，随时跳出重标定模块；

步骤4.4：判断斜率k_b是否小于阈值和重标定模块的运行时间是否超过10秒；

步骤4.5：如果条件成立，则记录重标定模块的磁力计标定结果，并其将代入抗干扰模块的磁力计标定模型中；如果条件不成立，则重置模块，返回步骤4.4。