[发明专利]一种伪卫星和惯性组合导航系统的非线性补偿方法

权利要求书

1、一种伪卫星和惯性组合导航系统的非线性补偿方法，其特征在于包括下列步骤：

(1)分别考虑惯导和伪卫星的误差状态，建立卡尔曼滤波器的状态方程；

(2)将伪卫星的伪距量测量在惯导给出的位置处进行线性化处理，建立卡尔曼滤波器的量测方程；

(3)根据建立的状态方程和量测方程，按照扩展卡尔曼滤波方法进行滤波解算，估计出状态向量；

(4)将伪卫星的伪距量测量在惯导给出的位置处进行二次泰勒展开，利用步骤(3)估计出的状态向量计算包含Hessian矩阵的二次修正项矩阵，并建立新的卡尔曼滤波器的量测方程；

(5)根据新建立的卡尔曼滤波器的量测方程再一次进行滤波解算，估计出新的状态向量；

(6)利用步骤(5)估计得到的状态向量，对惯导输出的位置、速度、姿态信息进行校正后得到组合导航系统的输出。

2、根据权利要求1所述的伪卫星和惯性组合导航系统的非线性补偿方法，其特征还在于：所述步骤(4)将伪卫星的伪距量测量在惯导给出的位置处进行二次泰勒展开，利用步骤(3)估计出的状态向量计算包含Hessian矩阵的二次修正项矩阵，建立新的卡尔曼滤波器的量测方程的方法为：

a.将伪卫星的伪距量测量在惯导给出的位置处进行二次泰勒展开，得：ρ(i)=ρ^(i)+∂ρ^(i)∂r^δr+12δrT·Hessi(i)(r^)·δr+δtu+v(i),]]>其中ρ⁽ⁱ⁾是第i颗伪卫星的伪距量测量，是根据惯导输出位置和伪卫星星历计算得到的伪距；δr表示惯导位置相对真实位置的误差；Hessi⁽ⁱ⁾为第i个伪距量测量的Hessian矩阵；δt_u＝c·t_u是接收机时钟偏差的等效距离误差，c为光速，t_u为接收机时钟与系统时的偏差；v⁽ⁱ⁾为测量噪声，将上式整理并写出矩阵形式得δρ=Gk·δrδtu+Hqua(δr)·δr+v,]]>其中δρ=ρ-ρ^,]]>是量测向量；G_k＝[e D_tu]是量测矩阵，e=[(∂ρ^(1)∂r^)T(∂ρ^(2)∂r^)T···(∂ρ^(n)∂r^)T]T,]]>D_tu＝[1 1…1]^T；Hqua(δr)=12δrT·Hessi(1)(r^)δrT·Hessi(2)(r^)...δrT·Hessi(n)(r^)]]>是包含Hessian矩阵的二次修正项矩阵，v＝[v⁽¹⁾ v⁽²⁾…v⁽ⁿ⁾]^T是测量噪声向量；

b.利用权利要求1步骤(3)估计出的状态向量计算包含Hessian矩阵的二次修正项矩阵Hqua(δr′)(Hqua(δr)=12δrT·Hessi(1)(r^)δrT·Hessi(2)(r^)...δrT·Hessi(n)(r^)),]]>其中Hessi⁽ⁱ⁾为第i个伪距量测量的Hessian矩阵；δr表示惯导位置相对真实位置的误差；

c.将步骤b的结果代入步骤a中的公式，可得δρ=Gk·δrδtu+Hqua(δr′)·δr+v=[e+Hqua(δr′)Dtu]δrδtu+v=Jk·δrδtu+v,]]>其中各个符号的含义与a中相同；由此可得新的卡尔曼滤波器的量测方程Z_k＝H′_kX_k+V_k，其中Z_k＝δρ是量测向量，X_k为系统的状态向量，H′_k＝[0_x×6[e+H_qua(δr′)]·Da 0_n×6 D_tu 0_n×1]是量测矩阵，V_k＝v是量测噪声向量。