CN101561496A - 一种伪卫星和惯性组合导航系统的非线性补偿方法 - Google Patents

Abstract

本发明公开了一种伪卫星/惯性组合导航系统的非线性补偿方法，并提供了该方法的具体实施方式。研究表明，在线性化方法的基础上，加入二次项修正，利用二次项补偿线性化误差，一定程度上减小由于伪卫星伪距非线性引入的滤波模型误差，对提高组合导航系统的精度有明显效果。同时采用两步算法解决了二次项带来的非线性计算问题，降低了算法的复杂性，具有实用价值。

Description

一种伪卫星和惯性组合导航系统的非线性补偿方法

技术领域

本发明涉及一种伪卫星和惯性组合导航系统的非线性补偿方法，属于导航定位技术领域。

背景技术

伪卫星也被称为陆基发射机或陆基卫星，其功能和原理与导航卫星类似，主要由接收机，发射机和天线等部分组成，能发出与导航卫星相同格式的电文。与导航卫星相比，伪卫星具有系统成本低、设置灵活机动、抗干扰性能好等特点。此外，用户可以根据应用需要来设计伪卫星的信号结构和电文格式，具有完全的自主权。因此，伪卫星技术得到了广泛的关注。

与卫星/惯性组合导航系统类似，伪卫星/惯性组合导航系统能够取长补短，获得更高的性能，具有广阔的应用前景。从利用伪卫星信号的角度看，伪卫星和惯导的组合大体可分为两种方式：浅组合方式和深组合方式。浅组合使用伪卫星接收机估计的位置、速度等信息作为综合滤波器的输入测量信息，算法简单，可靠性高，但是会损失一些信息且在综合滤波器的观测量中引入了不期望的相关，在跟踪的伪卫星几何结构差或不足四颗时，系统性能将受到严重影响。深组合方式利用了伪卫星的原始测量数据，如伪距、伪距率等，克服了浅组合的缺点，能够更好地实现伪卫星和惯导的相互辅助作用，已得到广泛应用。但是对于深组合而言，滤波器的测量方程中包含了伪卫星接收机的原始测量，因而引入了非线性。通常解决的方法是在位置的估值(一般为惯导系统输出位置)处线性化采用扩展卡尔曼滤波(EKF，extended Kalman filtering)算法。但是，由于伪卫星与用户的距离较近，其伪距观测量线性化误差的影响也更加严重，将造成卡尔曼滤波器性能恶化甚至发散。

发明内容

本发明的技术问题是：针对传统线性化方法在伪卫星/惯性组合导航中存在的线性误差大、造成滤波性能差甚至发散的问题，提供一种伪卫星和惯性组合导航系统的非线性补偿方法，该方法采用二次项修正的非线性补偿方法，提高了伪卫星/惯性组合导航系统的性能。

本发明的技术解决方案：一种伪卫星和惯性组合导航系统的非线性补偿方法，其特点在于步骤如下：

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

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

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

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

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

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

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

a.将伪卫星的伪距量测量在惯导给出的位置处进行二次泰勒展开，得： ${\mathrm{\ρ}}^{\left(i\right)}={\widehat{\mathrm{\ρ}}}^{\left(i\right)}+\frac{\∂{\widehat{\mathrm{\ρ}}}^{\left(i\right)}}{\∂\hat{r}}\mathrm{\δr}+\frac{1}{2}\mathrm{\δ}{r}^T\·{\mathrm{Hessi}}^{\left(i\right)}\left(\hat{r}\right)\·\mathrm{\δr}+{\mathrm{\δt}}_u+v^{\left(i\right)},$ 其中ρ⁽ⁱ⁾是第i颗伪卫星的伪距量测量，

是根据惯导输出位置和伪卫星星历计算得到的伪距；δr表示惯导位置相对真实位置的误差；Hessi⁽ⁱ⁾为第i个伪距量测量的Hessian矩阵；δt_u＝c·t_u是接收机时钟偏差的等效距离误差，c为光速，t_u为接收机时钟与系统时的偏差；v⁽ⁱ⁾为测量噪声，将上式整理并写出矩阵形式得 $\mathrm{\δ\ρ}=G_k\·\left[\begin{array}{c}\mathrm{\δr}\\ \mathrm{\δ}{t}_u\end{array}\right]+H_{\mathrm{qua}}\left(\mathrm{\δr}\right)\·\mathrm{\δr}+v,$ 其中 $\mathrm{\δ\ρ}=\mathrm{\ρ}-\widehat{\mathrm{\ρ}},$ 是量测向量；G_k＝[e D_tu]是量测矩阵， $e={\left[\begin{array}{cccc}{\left(\frac{\∂{\widehat{\mathrm{\ρ}}}^{\left(1\right)}}{\∂\hat{r}}\right)}^T& {\left(\frac{\∂{\widehat{\mathrm{\ρ}}}^{\left(2\right)}}{\∂\hat{r}}\right)}^T& \·\·\·& {\left(\frac{\∂{\widehat{\mathrm{\ρ}}}^{\left(n\right)}}{\∂\hat{r}}\right)}^T\end{array}\right]}^T,$ $D_{\mathrm{tu}}={\left[\begin{array}{cccc}1& 1& \·\·\·& 1\end{array}\right]}^T;H_{\mathrm{qua}}\left(\mathrm{\δr}\right)=\frac{1}{2}\left[\begin{array}{c}\mathrm{\δ}{r}^T\·{\mathrm{Hessi}}^{\left(1\right)}\left(\hat{r}\right)\\ {\mathrm{\δr}}^T\·{\mathrm{Hessi}}^{\left(2\right)}\left(\hat{r}\right)\\ \·\\ \·\\ \·\\ \mathrm{\δ}{r}^T\·{\mathrm{Hessi}}^{\left(n\right)}\left(\hat{r}\right)\end{array}\right]$ 是包含Hessian矩阵的二次修正项矩阵，v＝[v⁽¹⁾v⁽²⁾...v⁽ⁿ⁾]^T是测量噪声向量；

b.利用权利要求1步骤(3)估计出的状态向量计算包含Hessian矩阵的二次修正项矩阵 $H_{\mathrm{qua}}\left(\mathrm{\δ}{r}^{\′}\right)(H_{\mathrm{qua}}\left(\mathrm{\δr}\right)=\frac{1}{2}\left[\begin{array}{c}\mathrm{\δ}{r}^T\·{\mathrm{Hessi}}^{\left(1\right)}\left(\hat{r}\right)\\ {\mathrm{\δr}}^T\·{\mathrm{Hessi}}^{\left(2\right)}\left(\hat{r}\right)\\ \·\\ \·\\ \·\\ \mathrm{\δ}{r}^T\·{\mathrm{Hessi}}^{\left(n\right)}\left(\hat{r}\right)\end{array}\right]),$ 其中Hessi⁽ⁱ⁾为第i个伪距量测量的Hessian矩阵；δr表示惯导位置相对真实位置的误差；

c.将步骤b的结果代入步骤a中的公式，可得 $\mathrm{\δ\ρ}=G_k\·\left[\begin{array}{c}\mathrm{\δr}\\ \mathrm{\δ}{t}_u\end{array}\right]+H_{\mathrm{qua}}\left(\mathrm{\δ}{r}^{\′}\right)\·\mathrm{\δr}+v=\left[\begin{array}{cc}e+H_{\mathrm{qua}}\left(\mathrm{\δ}{r}^{\′}\right)& D_{\mathrm{tu}}\end{array}\right]\left[\begin{array}{c}\mathrm{\δr}\\ \mathrm{\δ}{t}_u\end{array}\right]+v=J_k\·\left[\begin{array}{c}\mathrm{\δr}\\ \mathrm{\δ}{t}_u\end{array}\right]+v,$ 其中各个符号的含义与a中相同；由此可得新的卡尔曼滤波器的量测方程Z_k＝H′_kX_k+V_k，其中Z_k＝δρ是量测向量，X_k为系统的状态向量，H′_k＝[0_n×6[e+H_qua(δr′)]·Da 0_n×6 D_tu 0_n×1]是量测矩阵，V_k＝v是量测噪声向量。

本发明与现有技术相比的优点在于：本发明利用二次项来补偿线性化误差，一定程度上弥补了伪卫星量测的非线性问题，对提高组合导航系统的精度有明显效果；同时采用了两步算法，即第一步：利用线性化方法来构建卡尔曼滤波器的数学模型，通过卡尔曼滤波得到状态向量的估计值；第二步：利用上一步得到的状态向量估计值计算包含Hessian矩阵的二次修正项矩阵，将非线性的量测方程转化为线性方程，再一次进行滤波计算即可得到此时刻状态向量的估计值。两步算法解决了二次项带来的非线性计算问题，降低了算法的复杂性，具有实用价值。

附图说明

图1为本发明的原理示意图；

图2为本发明的方法与标准扩展卡尔曼滤波方法的姿态精度比较；

图3为本发明的方法与标准扩展卡尔曼滤波方法的位置精度比较。

具体实施方式

本发明将伪卫星的伪距量测量在惯导给出的位置处进行二次泰勒展开，在滤波器的测量方程中加入二次项修正，利用二次项来减小由于伪卫星伪距非线性引入的滤波模型误差，提高了伪卫星/惯性组合导航系统的性能。

在说明本发明的具体实施方式前，有必要介绍一下卡尔曼滤波方法。卡尔曼滤波是一种线性、无偏递推最小方差回归估计，采用动力学方程即状态方程描述被估计量的动态变化规律，能够从噪声数据中最优估计出动态系统的未知状态。虽然工程对象一般都是连续系统，但是为了便于计算机处理，一般要将连续系统离散化。

对于连续系统有如下状态方程：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(1\right)$

式中X(t)为系统状态向量，F(t)为系统矩阵，G(t)为系统噪声矩阵，W(t)为系统激励噪声向量。

将其离散化后，得到：

X_k＝Φ_k，k-1X_k-1+Γ_k-1W_k-1(2)

式中X_k为k时刻的n维状态向量，也是被估计向量；Φ_k，k-1为k-1到k时刻的系统一步转移矩阵(n×n阶)；W_k-1为k-1时刻的系统噪声(r维)；Γ_k-1为系统噪声矩阵(n×r阶)，它表征由k-1到k时刻的各个系统噪声分别影响k时刻各个状态的程度。

系统量测方程可写为：

Z_k＝H_kX_k+V_k               (3)

式中Z_k为k时刻的m维量测向量X_k为k时刻的n维状态向量H_k为k时刻的量测矩阵(m×n阶)；V_k为k时刻的m维量测噪声。

根据离散化系统模型，给出离散卡尔曼滤波方程如(4)式所示：

状态一步预测方程：

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}---\left(4a\right)$

一步预测均方误差方程：

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(4b\right)$

滤波增益方程：

$K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^{-1}---\left(4c\right)$

状态估值方程：

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})---\left(4d\right)$

估计均方误差方程：

$P_k=(I-K_k{H}_k)P_{k,k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(4e\right)$

或

P_k＝(I-K_kH_k)P_k，k-1       (4f)

卡尔曼滤波作为一种递推形式的线性最小方差估计，在许多工程领域都取得了极大的成功。但是卡尔曼滤波的前提条件是状态和量测模型必须是线性关系的，且系统噪声和量测噪声须是已知方差的高斯噪声。

对于数学模型是非线性的系统，在理论上还很难找到一种严格的递推滤波方法，标准的卡尔曼滤波不能够直接应用。目前处理这种非线性系统的主要方法是通过对系统的状态方程和量测方程进行“线性化”近似，然后按线性化方程进行估计，即扩展卡尔曼滤波算法，但其只适用于滤波误差和预测误差很小的情况。当系统具有较强的非线性或初始误差较大时，滤波精度就会明显下降，甚至会造成滤波发散。本发明方法在扩展卡尔曼滤波方法的基础上克服了非线性的影响，同时保证了算法的实时性，具有很高的应用价值。

下面结合附图来详细阐述本发明的具体实施过程。

图1给出了本发明的原理示意图。惯导系统输出载体的实时位置、速度、姿态信息，伪卫星接收机输出参与定位的各个伪卫星的星历数据及测量的伪距信息。在组合导航中使用了非线性补偿滤波器，即首先根据伪距量测量和惯导的位置信息建立线性化量测模型使用标准扩展卡尔曼滤波算法估计出状态向量，再利用此状态向量估计值计算将伪距二次泰勒展开得到的包含Hessian矩阵的二次修正项矩阵，从而建立新的包含二次项修正的量测方程，再进行一次滤波就可得到新的状态向量估计值，最后对惯导系统输出进行校正后得到高精度的组合导航输出信息。实例中采用东北天地理坐标系为导航坐标系，考虑飞行高度h，并认为地球是旋转椭球体。

第一步：分别考虑惯导和伪卫星的误差状态，建立卡尔曼滤波器的状态方程

系统状态由两部分构成：惯导系统的导航参数误差和接收机的时钟误差。惯导系统的误差状态方程为

${\stackrel{\·}{X}}_I\left(t\right)=F_I\left(t\right)X_I\left(t\right)+G_I\left(t\right)W_I\left(t\right)---\left(5\right)$

式中X_I(t)为惯导误差状态向量，F_I(t)为系统矩阵，G_I(t)为系统噪声矩阵，W_I(t)为系统激励噪声向量。

接收机时钟误差等效为距离误差δt_u，状态方程为

$\mathrm{\δ}{\stackrel{\·}{t}}_u=0---\left(6\right)$

合并上述两式，得到卡尔曼滤波器的状态方程

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(7\right)$

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

设用户真实地理位置为经度λ、纬度L和高度h，通过下式可求得其在地球坐标系中的真实位置r_u＝(x y z)^T：

$\left.\begin{array}{c}x=(R_N+h)\cos L\cos \mathrm{\λ}\\ y=(R_N+h)\cos L\sin \mathrm{\λ}\\ z=[R_N{(1-f)}^2+h]\sin L\end{array}\right\}---\left(8\right)$

式中R_N为地球卯酉圈曲率半径，f为地球椭圆度。

用户接收机相对于第i颗伪卫星测得的伪距量测量可以表示为：

${\mathrm{\ρ}}^{\left(i\right)}=\left|\right|r_s^{\left(i\right)}-r_u\left|\right|+{\mathrm{\δt}}_u+v^{\left(i\right)},(i=1,\·\·\·,n)---\left(9\right)$

式中ρ⁽ⁱ⁾是第i颗伪卫星的伪距量测量， $d_i=\left|\right|r_s^{\left(i\right)}-r_u\left|\right|$ 为用户和伪卫星之间的几何距离，r_s ⁽ⁱ⁾为第i颗伪卫星的位置，由广播的星历数据计算得到；r_u为用户的真实位置；δt_u＝c·t_u为接收机时钟偏差的等效距离误差，c为光速，t_u为接收机时钟与系统时的偏差，v⁽ⁱ⁾为测量噪声。由此看出，伪卫星的伪距观测量是用户位置的非线性函数，为了应用卡尔曼滤波算法，需要将上述伪距测量方程做线性化处理。

用r_I表示惯导系统的输出位置，相应的位置误差以δr表示，即δr＝r_l-r_u，如果将惯导的位置作为载体位置的估值，即令 $\hat{r}=r_l,$ 将式(9)按泰勒公式展开并取一阶近似，即有：

${\mathrm{\ρ}}^{\left(i\right)}={\widehat{\mathrm{\ρ}}}^{\left(i\right)}+\frac{\∂{\widehat{\mathrm{\ρ}}}^{\left(i\right)}}{\∂\hat{r}}\mathrm{\δr}+{\mathrm{\δt}}_u+v^{\left(i\right)}---\left(10\right)$

其中 ${\widehat{\mathrm{\ρ}}}^{\left(i\right)}=\left|\right|r_s^{\left(i\right)}-\hat{r}\left|\right|=\left|\right|r_s^{\left(i\right)}-r_I\left|\right|,(i=1,\·\·\·,n)$ 是由惯导位置和第i颗伪卫星星历计算得到的伪距， $\frac{\∂{\widehat{\mathrm{\ρ}}}^{\left(i\right)}}{\∂\hat{r}}=\frac{\hat{r}-r_s^{\left(i\right)}}{\left|\right|\hat{r}-r_s^{\left(i\right)}\left|\right|}.$

令 $e={\left[\begin{array}{cccc}{\left(\frac{\∂{\widehat{\mathrm{\ρ}}}^{\left(1\right)}}{\∂\hat{r}}\right)}^T& {\left(\frac{\∂{\widehat{\mathrm{\ρ}}}^{\left(2\right)}}{\∂\hat{r}}\right)}^T& \·\·\·& {\left(\frac{\∂{\widehat{\mathrm{\ρ}}}^{\left(n\right)}}{\∂\hat{r}}\right)}^T\end{array}\right]}^T,$ D′_tu＝[1 1 … 1]^T且G_k＝[e D_tu]，式(10)可写出矩阵形式：

$\mathrm{\δ\ρ}=G_k\·\left[\begin{array}{c}\mathrm{\δr}\\ \mathrm{\δ}{t}_u\end{array}\right]+v---\left(11\right)$

式中 $\mathrm{\δ\ρ}=\mathrm{\ρ}-\widehat{\mathrm{\ρ}}$ 是量测向量，ρ＝[ρ⁽¹⁾ρ⁽²⁾…ρ⁽ⁿ⁾]^T， $\widehat{\mathrm{\ρ}}={[{\widehat{\mathrm{\ρ}}}^{\left(1\right)}{\widehat{\mathrm{\ρ}}}^{\left(2\right)}\·\·\·{\widehat{\mathrm{\ρ}}}^{\left(n\right)}]}^T,$ v＝[v⁽¹⁾v⁽²⁾…v⁽ⁿ⁾]^T是量测噪声向量。

由于δr＝(δxδyδz)^T为惯导系统给出的用户在地球坐标系中的位置误差，而在惯导误差方程中的位置误差为δr＝(δLδλδh)^T，所以由式(8)求得：

$\left[\begin{array}{c}\mathrm{\δx}\\ \mathrm{\δy}\\ \mathrm{\δz}\end{array}\right]=\left[\begin{array}{ccc}-(R_N+h)\sin L\cos \mathrm{\λ}& -(R_N+h)\cos L\sin \mathrm{\λ}& \cos L\cos \mathrm{\λ}\\ -(R_N+h)\sin L\sin \mathrm{\λ}& (R_N+h)\cos L\cos \mathrm{\λ}& \cos L\sin \mathrm{\λ}\\ [R_N{(1-f)}^2+h]\cos L& 0& \sin L\end{array}\right]\left[\begin{array}{c}\mathrm{\δL}\\ \mathrm{\δ\λ}\\ \mathrm{\δh}\end{array}\right]=D_a\left[\begin{array}{c}\mathrm{\δL}\\ \mathrm{\δ\λ}\\ \mathrm{\δh}\end{array}\right]---\left(12\right)$

将上式代入(11)得：

$\mathrm{\δ\ρ}=e\·D_a\left[\begin{array}{c}\mathrm{\δL}\\ \mathrm{\δ\λ}\\ \mathrm{\δh}\end{array}\right]+D_{\mathrm{tu}}\mathrm{\δ}{t}_u+V_{\mathrm{\ρ}}---\left(13\right)$

据此可得卡尔曼滤波器的量测方程如下：

Z_k＝H_kX_k+V_k    (14)

其中Z_k＝δρ是量测向量，X_k为系统的状态向量，H_k＝[0_n×6 e·Da 0_n×6 D_tu 0_n×1]是量测矩阵，V_k＝v是量测噪声向量。

第三步：根据以上建立的状态方程和量测方程，按照扩展卡尔曼滤波方法进行滤波解算，估计出状态向量

分别采用式(7)和式(14)作为滤波器的状态和测量方程，通过卡尔曼滤波得到状态向量的估计值X′_k。

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

将式(9)在载体的位置估值(由惯导得到的位置)处展成二阶泰勒级数，可得：

${\mathrm{\ρ}}^{\left(i\right)}={\widehat{\mathrm{\ρ}}}^{\left(i\right)}+\frac{\∂{\widehat{\mathrm{\ρ}}}^{\left(i\right)}}{\∂\hat{r}}\mathrm{\δr}+\frac{1}{2}\mathrm{\δ}{r}^T\·{\mathrm{Hessi}}^{\left(i\right)}\left(\hat{r}\right)\·\mathrm{\δr}+{\mathrm{\δt}}_u+v^{\left(i\right)}---\left(15\right)$

式中Hessi⁽ⁱ⁾为第i个伪距量测量的Hessian矩阵。Hessian矩阵为对称阵的性质大大简化了式(15)中二阶项的计算。针对本发明的情况，Hessian矩阵可表示为：

${\mathrm{Hessi}}^{\left(i\right)}\left(\hat{r}\right)=\frac{1}{{\left({\widehat{\mathrm{\ρ}}}^{\left(i\right)}\right)}^3}\·\left[\begin{array}{ccc}{\left(\mathrm{\Δ}{y}^{\left(i\right)}\right)}^2+{\left({\mathrm{\Δz}}^{\left(i\right)}\right)}^2& -\left(\mathrm{\Δ}{x}^{\left(i\right)}\right)\·\left(\mathrm{\Δ}{y}^{\left(i\right)}\right)& -\left({\mathrm{\Δx}}^{\left(i\right)}\right)\·\left(\mathrm{\Δ}{z}^{\left(i\right)}\right)\\ \·& {\left(\mathrm{\Δ}{x}^{\left(i\right)}\right)}^2+{\left(\mathrm{\Δ}{z}^{\left(i\right)}\right)}^2& -\left(\mathrm{\Δ}{y}^{\left(i\right)}\right)\·\left(\mathrm{\Δ}{z}^{\left(i\right)}\right)\\ \·& \·& {\left(\mathrm{\Δ}{x}^{\left(i\right)}\right)}^2+{\left(\mathrm{\Δ}{y}^{\left(i\right)}\right)}^2\end{array}\right]$

其中 $\mathrm{\Δ}{\mathrm{\ξ}}^{\left(i\right)}=\widehat{\mathrm{\ξ}}-{\mathrm{\ξ}}_s^{\left(i\right)},$ ξ＝x，y，z。

将式(15)写成矩阵形式为：

$\mathrm{\δ\ρ}=G_k\·\left[\begin{array}{c}\mathrm{\δr}\\ \mathrm{\δ}{t}_u\end{array}\right]+H_{\mathrm{qua}}\left(\mathrm{\δr}\right)\·\mathrm{\δr}+v---\left(16\right)$

其中 $H_{\mathrm{qua}}\left(\mathrm{\δr}\right)=\frac{1}{2}\left[\begin{array}{c}\mathrm{\δ}{r}^T\·{\mathrm{Hessi}}^{\left(1\right)}\left(\hat{r}\right)\\ \mathrm{\δ}{r}^T\·{\mathrm{Hessi}}^{\left(2\right)}\left(\hat{r}\right)\\ \·\\ \·\\ \·\\ \mathrm{\δ}{r}^T\·{\mathrm{Hessi}}^{\left(n\right)}\left(\hat{r}\right)\end{array}\right].$

由于矩阵H_qua(δr)中包含了状态变量，所以式(16)仍然是状态向量的非线性方程，要直接求解仍然比较困难。为了降低算法的复杂度和提高改进算法的实用性，采用了一种两步估计算法。

第一步利用线性化方法来构建卡尔曼滤波器的数学模型，即分别采用式(7)和式(14)作为滤波器的状态和测量方程，通过卡尔曼滤波得到状态向量的估计值x′_k。第二步利用这个估值来计算矩阵H_qua(δr′)，并将其代入式(16)中得：

$\mathrm{\δ\ρ}=G_k\·\left[\begin{array}{c}\mathrm{\δr}\\ \mathrm{\δ}{t}_u\end{array}\right]+H_{\mathrm{qua}}\left(\mathrm{\δ}{r}^{\′}\right)\·\mathrm{\δr}+v=\left[\begin{array}{cc}e+H_{\mathrm{qua}}\left(\mathrm{\δ}{r}^{\′}\right)& D_{\mathrm{tu}}\end{array}\right]\left[\begin{array}{c}\mathrm{\δr}\\ {\mathrm{\δt}}_u\end{array}\right]+v=J_k\·\left[\begin{array}{c}\mathrm{\δr}\\ \mathrm{\δ}{t}_u\end{array}\right]+v---\left(17\right)$

由此可得下式：

Z_k＝H′_kX_k+V_k(18)

其中Z_k＝δρ是量测向量，X_k为系统的状态向量，H′_k＝[0_n×6[e+H_qua(δr′)]·Da 0_n×6 D_tu 0_n×1]是量测矩阵，V_k＝v是量测噪声向量。

第五步：根据新建立的卡尔曼滤波器的量测方程再一次进行滤波解算，估计出新的状态向量

以式(18)为卡尔曼滤波器的量测方程再一次进行滤波计算即可得到k时刻状态向量的估计值X_k。

第六步：利用第五步估计得到的状态向量，对惯导输出的位置、速度、姿态信息进行校正后得到组合导航系统的输出

用此估计值X_k去校正惯导系统的输出，则可得到伪卫星/惯性组合导航系统的位置、速度、姿态信息。

为验证本发明的效果，对伪卫星/惯性组合系统进行了计算机仿真。使用的飞行轨迹模拟了复杂、高动态的飞行过程，包括滑跑、起飞、爬高、巡航、转弯和俯冲等阶段，初始位置为北纬35°，东经110°，飞行时间为3654s。

假设陀螺随机常值漂移为0.1°/hr，一阶马尔可夫漂移为0.1°/hr，相关时间为3600s，白噪声为0.01°/hr；加速度计零偏为10^-3g，一阶马尔可夫过程为10^-3g，相关时间为3600s，白噪声标准差为10^-3g。伪卫星接收机的伪距测量误差为30m。卡尔曼滤波器的周期为1s。

为比较本发明和标准扩展卡尔曼滤波算法的性能，在相同的条件下分别对两种算法进行了计算机仿真。图2和图3给出了两种算法的仿真结果，实线表示本发明的结果，虚线表示标准扩展卡尔曼滤波算法的结果。从仿真结果可以看出，本发明的滤波性能要明显优于标准扩展卡尔曼滤波算法。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。最后所应说明的是：以上实施实例仅用以说明而非限制本发明的技术方案，所有的不脱离本发明的精神和范围的修改或局部替换，均应涵盖在本发明的权利要求范围当中。

Claims (2)

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

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

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

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

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

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

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

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

a.将伪卫星的伪距量测量在惯导给出的位置处进行二次泰勒展开，得： ${\mathrm{\ρ}}^{\left(i\right)}={\widehat{\mathrm{\ρ}}}^{\left(i\right)}+\frac{\∂{\widehat{\mathrm{\ρ}}}^{\left(i\right)}}{\∂\hat{r}}\mathrm{\δr}+\frac{1}{2}{\mathrm{\δr}}^T\·{\mathrm{Hessi}}^{\left(i\right)}\left(\hat{r}\right)\·\mathrm{\δr}+{\mathrm{\δt}}_u+v^{\left(i\right)},$ 其中ρ⁽ⁱ⁾是第i颗伪卫星的伪距量测量，

是根据惯导输出位置和伪卫星星历计算得到的伪距；δr表示惯导位置相对真实位置的误差；Hessi⁽ⁱ⁾为第i个伪距量测量的Hessian矩阵；δt_u＝c·t_u是接收机时钟偏差的等效距离误差，c为光速，t_u为接收机时钟与系统时的偏差；v⁽ⁱ⁾为测量噪声，将上式整理并写出矩阵形式得 $\mathrm{\δ\ρ}=G_k\·\left[\begin{array}{c}\mathrm{\δr}\\ {\mathrm{\δt}}_u\end{array}\right]+H_{\mathrm{qua}}\left(\mathrm{\δr}\right)\·\mathrm{\δr}+v,$ 其中 $\mathrm{\δ\ρ}=\mathrm{\ρ}-\widehat{\mathrm{\ρ}},$ 是量测向量；G_k＝[e D_tu]是量测矩阵， $e={[{\left(\frac{\∂{\widehat{\mathrm{\ρ}}}^{\left(1\right)}}{\∂\hat{r}}\right)}^T{\left(\frac{{\∂\widehat{\mathrm{\ρ}}}^{\left(2\right)}}{\∂\hat{r}}\right)}^T\·\·\·{\left(\frac{{\∂\widehat{\mathrm{\ρ}}}^{\left(n\right)}}{\∂\hat{r}}\right)}^T]}^T,$ D_tu＝[1 1…1]^T； $H_{\mathrm{qua}}\left(\mathrm{\δr}\right)=\frac{1}{2}\left[\begin{array}{c}{\mathrm{\δr}}^T\·{\mathrm{Hessi}}^{\left(1\right)}\left(\hat{r}\right)\\ {\mathrm{\δr}}^T\·{\mathrm{Hessi}}^{\left(2\right)}\left(\hat{r}\right)\\ .\\ .\\ .\\ {\mathrm{\δr}}^T\·{\mathrm{Hessi}}^{\left(n\right)}\left(\hat{r}\right)\end{array}\right]$ 是包含Hessian矩阵的二次修正项矩阵，v＝[v⁽¹⁾ v⁽²⁾…v⁽ⁿ⁾]^T是测量噪声向量；

b.利用权利要求1步骤(3)估计出的状态向量计算包含Hessian矩阵的二次修正项矩阵 $H_{\mathrm{qua}}\left({\mathrm{\δr}}^{\′}\right)(H_{\mathrm{qua}}\left(\mathrm{\δr}\right)=\frac{1}{2}\left[\begin{array}{c}{\mathrm{\δr}}^T\·{\mathrm{Hessi}}^{\left(1\right)}\left(\hat{r}\right)\\ {\mathrm{\δr}}^T\·{\mathrm{Hessi}}^{\left(2\right)}\left(\hat{r}\right)\\ .\\ .\\ .\\ {\mathrm{\δr}}^T\·{\mathrm{Hessi}}^{\left(n\right)}\left(\hat{r}\right)\end{array}\right]),$ 其中Hessi⁽ⁱ⁾为第i个伪距量测量的Hessian矩阵；δr表示惯导位置相对真实位置的误差；

c.将步骤b的结果代入步骤a中的公式，可得 $\mathrm{\δ\ρ}=G_k\·\left[\begin{array}{c}\mathrm{\δr}\\ {\mathrm{\δt}}_u\end{array}\right]+H_{\mathrm{qua}}\left({\mathrm{\δr}}^{\′}\right)\·\mathrm{\δr}+v=[e+H_{\mathrm{qua}}\left({\mathrm{\δr}}^{\′}\right)D_{\mathrm{tu}}]\left[\begin{array}{c}\mathrm{\δr}\\ {\mathrm{\δt}}_u\end{array}\right]+v=J_k\·\left[\begin{array}{c}\mathrm{\δr}\\ {\mathrm{\δt}}_u\end{array}\right]+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是量测噪声向量。

Images