[发明专利]一种基于量测噪声协方差矩阵估计的地磁导航方法

权利要求书

1.一种基于量测噪声协方差矩阵估计的地磁导航方法，所述方法包括：

步骤1)建立离散化的地磁导航系统的状态方程和量测方程：

X_k＝f(X_k-1,k-1)+w_k-1 (9)

Z_k＝h(X_k,k)+v_k (10)

式中：X_k＝(x_k,y_k,z_k,v_xk,v_yk,v_zk)^T，(x_k,y_k,z_k),(v_xk,v_yk,v_zk)分别表示卫星在地固直角坐标系下的位置与速度，k表示离散时间点， 为非负整数；

Z_k为磁强计对地磁场矢量的量测值；h(X_k,k)为地磁场矢量的地磁模型输出值，系统噪声w_k和观测噪声v_k的统计特性如下：

式中：j表示离散时间点，j为非负整数；Q_k为系统噪声协方差矩阵，为非负定阵；δ_kj为Kronecker符号；R_k为量测噪声协方差矩阵，为正定阵，R_k＝diag[(20nT)²,(20nT)²,(20nT)²]；

步骤2)基于步骤1)建立的运动方程，利用卡尔曼滤波算法计算卫星的状态值；

当采用EKF算法时，所述步骤2)具体为：

步骤201)状态一步转移矩阵Φ_k,k-1和量测矩阵H_k分别为：

步骤202)当k＝0时，滤波方程的初始条件为：

其中，为k＝0时的状态估计值，P₀为k＝0时的状态估计均方误差；

步骤203)状态一步预测为：

一步预测均方误差P_k,k-1为：

滤波增益矩阵K_k为：

步骤204)k时刻卫星状态估计为：

k时刻卫星状态向量的协方差矩阵P_k为：

当采用UKF算法时，所述步骤2)具体为：

步骤2-1)当k＝0时，选定滤波初值为：

其中，为k＝0时的状态向量估计值，P₀为k＝0时的状态估计均方误差；

步骤2-2)构造Sigma散点集，设状态向量为n维，为时刻k-1的状态向量估计值，P_k-1为该时刻状态向量的协方差矩阵，Sigma点集表示为：

式中：n＝6；为误差协方差矩阵均方根的第i列，采用Cholesky分解或者奇异值分解求出；λ＝α²(n+κ)-n为控制Sigma 点分布的相关参数，其范围为10^-4≤α≤1，κ为比例参数，取κ＝3-n；

步骤2-3)计算k时刻状态的一步预测值和一步预测均方误差矩阵P_k,k-1：

式中：W_i^(m)和W_i^(c)分别为UT变换计算均值和方差所用的加权值：

W_i^(m)＝W_i^(c)＝1/2(n+λ)i＝1,2,…，2n，

式中：β包含状态分布的先验信息，取值为2；

步骤2-4)计算量测值的预测值及互协方差矩阵P_(XZ)k,k-1和自协方差矩阵P_(ZZ)k,k-1：

其中，为中间变量；

步骤2-5)进行滤波量测更新：

其中，K_k为k时刻滤波增益矩阵，为k时刻的状态向量估计值，P_k为该时刻状态向量的协方差矩阵。

2.根据权利要求1所述的基于量测噪声协方差矩阵估计的地磁导航方法，其特征在于，所述量测噪声协方差矩阵取值为R_k＝diag[(20nT)²,(20nT)²,(20nT)²]的条件为地磁平静期和地磁微扰期。