CN108844536B - 一种基于量测噪声协方差矩阵估计的地磁导航方法 - Google Patents

Abstract

本发明公开了一种基于量测噪声协方差矩阵估计的地磁导航方法，所述方法包括：步骤1)建立离散化的地磁导航系统的状态方程和量测方程，其中量测噪声协方差矩阵R_k＝diag[(20nT)²,(20nT)²,(20nT)²]；步骤2)基于步骤1)建立的运动方程，利用卡尔曼滤波算法计算卫星的状态值。通过本发明的方法估计出的量测噪声协方差矩阵能够使扩展卡尔曼滤波和无迹卡尔曼滤波在实测数据条件下收敛并得到较好的导航精度。这种方法所确定的量测噪声协方差矩阵普适性好，具有确定的物理意义，将地磁场模型误差作为确定量测噪声协方差矩阵的主要因素，更符合地磁导航的环境；本发明的方法既可以应用在地磁平静期和地磁微扰期，量测噪声协方差矩阵经过适当调整也可以用在地磁活跃期时。

Description

一种基于量测噪声协方差矩阵估计的地磁导航方法

技术领域

本发明涉及近地卫星自主导航领域，具体涉及一种基于量测噪声协方差矩阵估计的地磁导航方法。

背景技术

地磁导航通过星上磁强计的测量值与地磁模型的输出值(预测值)进行比较而得到轨道修正信息，利用该修正信息可以实现轨道的确定，进而达到航天器自主导航的目的。地磁导航是近地航天器的一种重要的自主导航方式，可实现航天器位置、速度以及姿态信息的自主确定。与传统的GPS卫星导航相比，具有抗干扰能力强、隐蔽性强的优势；与惯性导航相比，地磁导航误差不随时间累积，并且导航系统体积小、功耗低。因此在近地轨道卫星、水下航行器等具有很好的应用前景。

利用地磁场为近地卫星进行导航时，量测噪声协方差矩阵是导航滤波算法中的重要参数，该参数的取值决定了滤波算法的精度甚至决定了导航滤波算法能否收敛与稳定。然而，地磁场模型IGRF和WMM描述的主磁场是一种大尺度的空间矢量场，模型精度较低。地磁场模型误差的存在导致滤波器系统的量测噪声不仅仅由磁强计的测量噪声决定，而是由磁强计的量测噪声和地磁场模型误差共同决定。由于地磁模型误差的存在，使得系统量测噪声的统计特性获取变得更加复杂，进而导致量测噪声协方差矩阵难以精确选取。

基于以上分析，选取合理的量测噪声协方差矩阵的值对于保证地磁导航精度具有重要意义。从国内外研究资料可以看出，目前在进行地磁导航研究时，量测噪声协方差矩阵主要是通过取经验值及参数调试得到，没有一个固定的可靠的选取方法。通过经验和参数调试所选取的量测噪声协方差矩阵的普适性不强，没有明确的物理意义。

发明内容

本发明的目的在于克服目前地磁导航中量测噪声协方差矩阵的估计方法存在的上述缺陷，提出了基于地磁场模型误差估计滤波器中量测噪声协方差矩阵的方法。该方法首先利用Swarm卫星的实测数据估算地磁场模型误差，又考虑到磁强计量测误差与地磁场模型误差相比比重较小，得出系统的量测噪声主要由地磁场模型误差决定，从而以地磁场模型误差为参考确定量测噪声协方差矩阵，从而改进了地磁导航方法。利用Swarm卫星的实测数据进行仿真校验，结果证明了所提出的新的量测噪声协方差矩阵估计方法在扩展卡尔曼滤波(EKF)和无迹卡尔曼滤波(UKF)中使用的有效性。综上，这种方法所确定的量测噪声协方差矩阵普适性好，具有确定的物理意义，将地磁场模型误差作为确定量测噪声协方差矩阵的主要因素，更符合地磁导航的环境。

为了实现上述目的，本发明一种基于量测噪声协方差矩阵估计的地磁导航方法，所述方法包括：

步骤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表示离散时间点；为非负整数；Q_k为系统噪声协方差矩阵，为非负定阵；δ_kj为Kronecker符号；R_k为量测噪声协方差矩阵，为正定阵，R_k＝diag[(20nT)²,(20nT)²,(20nT)²]；

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

作为上述方法的一种改进，当采用EKF算法时，所述步骤2)具体为：

步骤201)状态一步转移矩阵Φ_k,k-1和量测矩阵Η_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为该时刻状态向量的协方差矩阵。

作为上述方法的一种改进，所述量测噪声协方差矩阵取值为R_k＝diag[(20nT)²,(20nT)²,(20nT)²]的条件为地磁平静期和地磁微扰期。

本发明的优势在于：

1、通过本发明的方法估计出的量测噪声协方差矩阵能够使扩展卡尔曼滤波(EKF)和无迹卡尔曼滤波(UKF)在实测数据条件下收敛并得到较好的导航精度。这种方法所确定的量测噪声协方差矩阵普适性好，具有确定的物理意义，将地磁场模型误差作为确定量测噪声协方差矩阵的主要因素，更符合地磁导航的环境；

2、本发明的方法既可以应用在地磁平静期和地磁微扰期，量测噪声协方差矩阵经过适当调整也可以用在地磁活跃期时。

附图说明

图1为本发明的地磁导航方法的流程图；

图2(a)为EKF地磁导航位置矢量误差曲线；

图2(b)为EKF地磁导航位置误差曲线；

图2(c)为EKF地磁导航速度矢量误差曲线；

图2(d)为EKF地磁导航速度误差曲线；

图3(a)为UKF地磁导航位置矢量误差曲线；

图3(b)为UKF地磁导航位置误差曲线；

图3(c)为UKF地磁导航速度矢量误差曲线；

图3(d)为UKF地磁导航速度误差曲线。

具体实施方式

本发明是针对近地卫星地磁导航中量测噪声协方差矩阵难以准确估计提出的测噪声协方差矩阵估计方法，基本原理是：由于地磁场模型精度较低，即地磁场模型的输出值与地磁场的真实值之间存在比较大的偏差，系统的量测噪声可以分解为地磁模型误差和磁强计的量测误差两部分；进一步分析这两部分的具体数值，得以得出磁强计的量测误差远小于地磁模型误差。所以可以参考地磁模型误差估计导航滤波算法中的量测噪声协方差矩阵。

下面结合附图和具体实施例对本发明的方法进行详细的说明。

地磁导航方法的流程如下图1所示：首先利用卫星的姿态信息将磁强计的测量数据由磁强计的本体坐标系转换到地球固连坐标系下，之后将转换后的实测地磁场数据和地磁场模型的输出值输入系统的量测方程，然后建立以地球固连坐标系下的轨道动力学方程为基础的状态方程，最后将系统的状态方程和量测方程输入到扩展卡尔曼滤波器或无迹卡尔曼滤波器中，从而得到精度较高的导航信息。

1、坐标转换

磁强计本体坐标系与地球固连标系的转换关系如下式所示：

式中，X_b,Y_b,Z_b表示磁强计本体坐标系下的磁测数据；X_T,Y_T,Z_T表示地球固连坐标系下的磁测数据；矩阵P(t)表示坐标系之间的转换矩阵，是以时间为自变量的函数。

2、计算地磁场模型输出值

地磁主磁场近似于一个磁偶极子，可利用高斯球谐分析法来对其进行建模。通过求解主磁场标量位所满足的拉普拉斯方程，得到其球谐表达式：

式中：a表示地球的平均半径；

是地磁模型高斯球谐系数，由地球主磁场模型提供；

是施密特归一化缔合勒让德多项式；r为地心距，θ为地心余纬，λ为地心经度；N为球谐级数的最大截断阶数。

主磁场的地磁场强度矢量可以表示成主磁场标量磁位的负梯度。在地固球坐标系中，地磁场沿北东地坐标系三轴分量为B_x,B_y,B_z，具体的表达式如下所示：

3、滤波状态方程和量测方程

地球固连坐标系系下轨道动力学方程，即地磁导航系统的状态方程为：

式中：X(t)＝(x,y,z,v_x,v_y,v_z)^T，(x,y,z)、(v_x,v_y,v_z)分别表示卫星在地固直角坐标系下X_e、Y_e、Z_e三个方向的位置与速度，地磁场模型是基于地心固连坐标系构造出的位置函数，采用卫星在地心固连坐标系的位置和速度作为状态变量，会使模型结构更加简单，滤波算法稳定性好，更加容易收敛，是具有工程应用价值的使用方法；F(X(t),t)为只考虑J2摄动项时，卫星在地球固连坐标系下的轨道动力学方程；w(t)为系统噪声，近似为高斯白噪声。

F(X(t),t)的具体表达式如下：

式中：J₂＝0.00108263为地球引力二阶带谐项系数；R_e为地球平均半径；μ为地球引力常数，ω_e为地球自转角速度。

地磁导航系统的量测方程为：

Z(t)＝h(X(t),t)+v(t) (8)

式中：Z(t)为磁强计对地磁场矢量的量测值；h(X(t),t)为地磁场矢量的地磁模型输出值，三个方向的具体表达式为式(3)，式(4)，式(5)；v(t)为系统的量测噪声，近似为高斯白噪声。

将状态方程(6)和量测方程(8)离散化后分别得：

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

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

系统噪声w_k和观测噪声v_k的统计特性如下：

式中：Q_k为系统噪声协方差矩阵，为非负定阵；R_k为量测噪声协方差矩阵，为正定阵；δ_kl为Kronecker符号，f(X_k-1,k-1)为离散化的F(X(t),t)。

4、量测噪声协方差矩阵的估计

4.1分析系统的量测噪声

在地磁场模型IGRF和WMM的精度较低的条件下，对系统的量测噪声进行详细分析：

式中：v(t)为系统的量测噪声，Z(t)为磁强计对地磁场矢量的量测值，Z_true(t)为地磁场矢量的真值，h(X(t),t)为地磁场矢量的地磁模型输出值。v_sensor(t)为磁强计的量测误差，v_model(t)为地磁场模型的模型误差，定义如下：

v_sensor(t)＝Z(t)-Z_true(t) (13)

v_model(t)＝Z_true(t)-h(X(t),t) (14)

显然，量测噪声v(t)由磁强计的量测误差v_sensor(t)和地磁场模型误差v_model(t)共同决定。下面具体分析地磁场模型误差v_model(t)和量测误差v_sensor(t)。

4.2分析地磁模型误差和磁强计量测误差

由于Swarm卫星是极轨卫星，卫星的磁测数据几乎涵盖了所有纬度，所以Swarm卫星的实测数据可以全面反映出地磁场模型的误差特征。利用Swarm卫星在2015年5月22日、2016年6月13日和2017年7月5日三天的实测数据估算出的地磁场模型IGRF-12和WMM2015的平均绝对误差(MAE)见表1。从表1可以得出，两种地磁场模型的三个方向的地磁场分量的平均绝对误差都约为20nT。

表1地磁场模型IGRF-12和WMM2015的平均绝对误差

表2中为磁测卫星中的磁强计精度：

表2磁强计的精度

4.3基于地磁模型误差估计量测噪声协方差矩阵

对比表1和表2可以得出：

v(t)≈v_model(t) (15)

系统的量测噪声主要由地磁场模型误差决定，所以可以将地磁场模型的平均绝对误差作为参考，确定量测噪声协方差矩阵R_k。这里认为量测噪声协方差矩阵不随时间变化，统一用符号R表示。根据表1，地磁场模型IGRF-12的精度比地磁场模型WMM2015的精度略高一些，因此本发明采用IGRF-12，量测噪声协方差矩阵的值可以取为：

R＝diag[(20nT)²,(20nT)²,(20nT)²] (16)

本方法所估算的地磁模型误差为地磁平静期和地磁微扰期时的地磁模型误差，地磁活跃期的地磁模型误差比平静期和微扰期的地磁模型误差更大。在实际应用中，应当把式(16)作为基础参考值，地磁平静期和地磁微扰期可以直接使用该值，地磁活跃期时根据地磁活跃程度适当增大协方差矩阵的值。

5、滤波器算法

5.1EKF算法

首先给出状态一步转移矩阵Φ_k,k-1和量测矩阵Η_k：

滤波方程初始条件：

状态一步预测：

一步预测均方误差：

滤波增益矩阵：

状态估计：

估计均方误差：

5.2UKF算法

选定滤波初值：

构造Sigma散点集，设状态向量为n维，

为时刻k-1的状态向量估计值，P_k-1为该时刻状态向量的协方差矩阵，Sigma点集可以表示为：

式中：n＝6；

为误差协方差矩阵均方根的第i列，可以采用Cholesky分解或者奇异值分解(特征值分解)求出，Cholesky分解要求协方差矩阵为正定的；λ＝α²(n+κ)-n，为控制sigma点分布的相关参数，也称可调节尺度参数，作用是确定sigma点在其均值附近分布状况，其范围为10^-4≤α≤1，κ为比例参数，一般取κ＝3-n。

计算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最优。

同理，由

可得量测值的预测值

及互协方差矩阵P_(XZ)k,k-1和自协方差矩阵P_(ZZ)k,k-1：

进行滤波量测更新：

本发明提出地磁导航方法能够使扩展卡尔曼滤波(EKF)和无迹卡尔曼滤波(UKF)在实测数据条件下收敛并得到较好的导航精度。如图2(a)、图2(b)、图2(c)和图2(d)所示，本发明估计出的量测噪声协方差矩阵能够使扩展卡尔曼滤波(EKF)在实测数据条件下收敛并得到较好的导航精度。如图3(a)、图3(b)、图3(c)和图3(d)所示，本发明估计出的量测噪声协方差矩阵能够使无迹卡尔曼滤波(UKF)在实测数据条件下收敛并得到较好的导航精度。

本发明的创新点在于：

1、发明将近地卫星地磁导航中的系统量测噪声分为了地磁模型误差和磁强计量测噪声两部分。

2、本发明通过分析实测数据得出：近地卫星地磁导航中磁强计量测噪声与地磁模型误差相比很小，系统的量测噪声主要由地磁模型误差决定。

3、本发明以地磁模型误差为参考估计近地卫星地磁导航滤波算法中的量测噪声协方差矩阵，由于地磁模型误差可以在线实时估计，可以使地磁导航具备自适应特性。

最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (2)

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)²]的条件为地磁平静期和地磁微扰期。

Images