CN102538792A - 一种位置姿态系统的滤波方法 - Google Patents

Abstract

本发明涉及一种位置姿态系统的滤波方法。首先，建立位置姿态系统的高阶误差模型；其次，设计基于三参数递推的卡尔曼滤波器；然后，将捷联惯导系统(SINS)与全球导航卫星系统(GNSS)的定位结果之差作为观测量，采用SINS/GNSS分布式组合形式，通过基于三参数递推的卡尔曼滤波器对各误差状态进行最优估计；最后将位置误差、速度误差、姿态误差和惯性器件误差的最优估计值反馈回SINS，并对陀螺漂移、加速度计偏置以及载体坐标系相对于计算坐标系的转换矩阵进行校正，计算出精确的位置、速度和姿态信息。

Description

一种位置姿态系统的滤波方法

技术领域

本发明涉及一种位置姿态系统的滤波方法，可用于航空遥感成像运动补偿。

背景技术

位置姿态系统作为航空遥感成像运动补偿的一种重要手段，主要由捷联惯导系统(Strapdown Inertial Navigation System，SINS)和全球导航卫星系统(Global Navigation Satellite System，GNSS)组成，集合了SINS数据短期精度高、输出数据频率高和GNSS数据长期稳定性好、不随时间漂移的优点于一体，可提供高精度的位置、速度和姿态信息。随着遥感图像分辨率的不断提高，高精度的运动补偿日益显示出其重要性，因此对位置姿态系统的精度提出了更高的要求。

位置姿态系统的误差包括惯性测量单元(Inertial Measurement Unit，IMU)的测量误差、GNSS测量误差和组合滤波估计误差。其中，IMU器件(陀螺和加速度计)的测量误差是影响位置姿态系统精度的主要因素。传统的位置姿态系统误差模型对误差源均作了不同程度地简化处理，如忽略了IMU器件经过标定补偿后仍存在的残余误差，以及将陀螺随机漂移和加速度计随机偏置均考虑为随机常值等，这些简化处理将直接影响位置姿态系统的精度。此外，传统的卡尔曼滤波技术采用单参数递推模型，导致滤波估计收敛速度慢且滤波精度不高。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种基于高阶系统误差模型的三参数递推滤波方法，提高了位置姿态系统的精度。

本发明的技术解决方案为：一种位置姿态系统的滤波方法，其特征在于包括以下步骤：

(1)建立位置姿态系统的高阶误差模型；

(2)设计基于三参数递推的卡尔曼滤波器；

(3)基于步骤(1)中建立的位置姿态系统高阶误差模型，取SINS与GNSS的定位结果之差作为观测量，采用SINS/GNSS分布式组合形式，通过以上步骤(2)设计的基于三参数递推的卡尔曼滤波器对各误差状态进行最优滤波估计；

(4)将步骤(3)中计算得到的位置误差、速度误差、姿态误差和惯性器件误差的最优滤波估计值反馈回SINS，并对陀螺漂移、加速度计偏置以及载体坐标系相对于计算坐标系的转换矩阵进行校正，计算出更加精确的位置、速度和姿态信息。

本发明与现有技术相比的优点在于：本发明克服了现有位置姿态系统误差建模方法的不足，对传统误差模型进行改进，考虑了位置误差、速度误差、姿态误差，陀螺和加速度计的随机漂移、随机偏置、刻度因子误差和安装误差的标定残差等，建立了位置姿态系统的高阶误差模型，并克服了传统卡尔曼滤波技术由于其单参数递推而导致的滤波估计收敛速度慢且滤波精度不高的不足，建立基于三参数递推的滤波模型，提高了误差状态的估计精度和估计收敛速度，从而更快地计算出精确的位置、速度和姿态信息。

附图说明

图1为本发明位置姿态系统的滤波方法的流程图。

具体实施方式

一种位置姿态系统的滤波方法，以位置姿态系统高阶误差模型为基础建立系统状态方程，并在系统状态方程的基础上建立系统量测方程。根据建立的系统状态方程和系统量测方程采用基于三参数递推的卡尔曼滤波器为位置姿态系统误差提供最优估计，并利用这些误差的最优估计值去修正SINS，以减少位置姿态系统误差，最后得到精确的位置、速度和姿态信息。

如图1所示，本发明的具体实施步骤如下：

1、建立位置姿态系统的高阶误差模型

位置姿态系统的高阶误差模型包含位置误差、速度误差、姿态误差、陀螺随机漂移、陀螺刻度因子误差、陀螺安装误差、加速度计随机偏置、加速度计刻度因子误差和加速度计安装误差，共9类误差。

陀螺随机漂移和加速度计随机偏置采用四种分量表示：随机常值、随机游走、一阶马尔科夫过程和白噪声，数学模型描述如下所示。

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_b=0,{\stackrel{\·}{\mathrm{\ϵ}}}_r={\mathrm{\ω}}_{\mathrm{gr}},{\stackrel{\·}{\mathrm{\ϵ}}}_m=-\frac{1}{\mathrm{\α}}{\mathrm{\ϵ}}_m+{\mathrm{\ω}}_{\mathrm{gm}},E\left[{\mathrm{\ω}}_g\left(t\right){\mathrm{\ω}}_g\left(\mathrm{\τ}\right)\right]=q_g\mathrm{\δ}(t-\mathrm{\τ})\\ {\stackrel{\·}{\▿}}_b=0,{\stackrel{\·}{\▿}}_r={\mathrm{\ω}}_{\mathrm{ar}},{\stackrel{\·}{\▿}}_m=-\frac{1}{\mathrm{\β}}{\▿}_m+{\mathrm{\ω}}_{\mathrm{am}},E\left[{\mathrm{\ω}}_a\left(t\right){\mathrm{\ω}}_a\left(\mathrm{\τ}\right)\right]=q_a\mathrm{\δ}(t-\mathrm{\τ})\end{array}\right.---\left(1\right)$

其中，ε_b、

为陀螺随机常值漂移和加速度计随机常值偏置，

为其导数；ε_r、

为陀螺随机游走漂移和加速度计随机游走偏置，

为其导数；ε_m、

为陀螺一阶马尔科夫过程漂移和加速度计一阶马尔科夫过程偏置，

为其导数；ω_gr与ω_ar分别是陀螺和加速度计的随机游走驱动白噪声；ω_gm与ω_am分别是陀螺和加速度计的一阶马尔科夫过程驱动白噪声，α与β陀螺和加速度计一阶马尔科夫过程的相关时间；ω_g、ω_a为陀螺白噪声漂移和加速度计白噪声偏置，q_g与q_a分别为陀螺和加速度计的白噪声强度，δ(t-τ)为狄拉克δ函数。

陀螺和加速度计刻度因子误差以及安装误差均采用随机常值表示，数学模型描述如下所示。

$\left\{\begin{array}{cc}\mathrm{\δ}{\stackrel{\·}{K}}_g=0,& \mathrm{\δ}\stackrel{\·}{G}=0\\ \mathrm{\δ}{\stackrel{\·}{K}}_a=0,& \mathrm{\δ}\stackrel{\·}{A}=0\end{array}\right.---\left(2\right)$

其中，δK_g、δK_a为陀螺和加速度计的刻度因子误差，

为其导数；δG、δA为陀螺和加速度计的安装误差，

为其导数。

高阶误差模型的系统状态方程和量测方程如下：

a)根据上述对位置姿态系统误差源模型的分析，系统状态方程为：

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

式中， $X=\left[\begin{array}{ccccccccccccccc}\mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\δV}}_E& {\mathrm{\δV}}_N& {\mathrm{\δV}}_U& {\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& {\▿}_{\mathrm{bx}}& {\▿}_{\mathrm{by}}& {\▿}_{\mathrm{bz}}& {\▿}_{\mathrm{rx}}& {\▿}_{\mathrm{ry}}& {\▿}_{\mathrm{rz}}\end{array}\right.$ $\begin{array}{ccccccccccccccccccccc}{\▿}_{\mathrm{mx}}& {\▿}_{\mathrm{my}}& {\▿}_{\mathrm{mz}}& {\mathrm{\δK}}_{\mathrm{Ax}}& {\mathrm{\δK}}_{\mathrm{Ay}}& {\mathrm{\δK}}_{\mathrm{Az}}& {\mathrm{\δA}}_{\mathrm{yz}}& {\mathrm{\δA}}_{\mathrm{zy}}& {\mathrm{\δA}}_{\mathrm{xz}}& {\mathrm{\δA}}_{\mathrm{zx}}& {\mathrm{\δA}}_{\mathrm{xy}}& {\mathrm{\δA}}_{\mathrm{yx}}& {\mathrm{\ϵ}}_{\mathrm{bx}}& {\mathrm{\ϵ}}_{\mathrm{by}}& {\mathrm{\ϵ}}_{\mathrm{bz}}& {\mathrm{\ϵ}}_{\mathrm{rx}}& {\mathrm{\ϵ}}_{\mathrm{ry}}& {\mathrm{\ϵ}}_{\mathrm{rz}}& {\mathrm{\ϵ}}_{\mathrm{mx}}& {\mathrm{\ϵ}}_{\mathrm{my}}& {\mathrm{\ϵ}}_{\mathrm{mz}}\end{array}$ ${\left.\begin{array}{ccccccccc}{\mathrm{\δK}}_{\mathrm{Gx}}& {\mathrm{\δK}}_{\mathrm{Gy}}& {\mathrm{\δK}}_{\mathrm{Gz}}& {\mathrm{\δG}}_{\mathrm{yz}}& {\mathrm{\δG}}_{\mathrm{zy}}& {\mathrm{\δG}}_{\mathrm{xz}}& {\mathrm{\δG}}_{\mathrm{zx}}& {\mathrm{\δG}}_{\mathrm{xy}}& {\mathrm{\δG}}_{\mathrm{yx}}\end{array}\right]}^T$ 为系统状态向量；δL、δλ、δh为纬度误差、经度误差、高度误差；δV_E、δV_N、δV_U为东向速度误差、北向速度误差、天向速度误差；φ_E、φ_N、φ_U为东向水平姿态失准角、北向水平姿态失准角、航向失准角；

为加速度计三个测量轴上的随机常值偏置；

为加速度计三个测量轴上的随机游走偏置；

为加速度计三个测量轴上的一阶马尔科夫过程偏置；δK_Ax、δK_Ay、δK_Az为加速度计三个测量轴上的刻度因子误差；δA_zx、δA_yx，δA_zy、δA_xy，δA_yz、δA_xz为加速度计三个测量轴上的安装误差，每个加速度计测量轴的安装误差用两个参数表示，因此加速度计三个测量轴的安装误差共六个；；ε_bx、ε_by、ε_bz为陀螺三个测量轴上的随机常值漂移；ε_rx、ε_ry、ε_rz为陀螺三个测量轴上的随机游走漂移；ε_mx、ε_my、ε_mz为陀螺三个测量轴上的一阶马尔科夫过程漂移；δK_Gx、δK_Gy、δK_Gz为陀螺三个测量轴上的刻度因子误差；δG_zx、δG_yx，δG_zy、δG_xy，δG_yz、δG_xz为陀螺三个测量轴上的安装误差，每个陀螺测量轴的安装误差用两个参数表示，因此陀螺三个测量轴的安装误差共六个；

为系统状态向量的导数，F为系统状态转移矩阵，G为系统噪声分配矩阵，W为系统噪声向量，具体表达式如下：

$W={\left[\begin{array}{cccccccccccccccccc}{\mathrm{\ω}}_{\mathrm{ax}}& {\mathrm{\ω}}_{\mathrm{ay}}& {\mathrm{\ω}}_{\mathrm{az}}& {\mathrm{\ω}}_{\mathrm{gx}}& {\mathrm{\ω}}_{\mathrm{gy}}& {\mathrm{\ω}}_{\mathrm{gz}}& {\mathrm{\ω}}_{\mathrm{arx}}& {\mathrm{\ω}}_{\mathrm{ary}}& {\mathrm{\ω}}_{\mathrm{arz}}& {\mathrm{\ω}}_{\mathrm{amx}}& {\mathrm{\ω}}_{\mathrm{amy}}& {\mathrm{\ω}}_{\mathrm{amz}}& {\mathrm{\ω}}_{\mathrm{grx}}& {\mathrm{\ω}}_{\mathrm{gry}}& {\mathrm{\ω}}_{\mathrm{grz}}& {\mathrm{\ω}}_{\mathrm{gmx}}& {\mathrm{\ω}}_{\mathrm{gmy}}& {\mathrm{\ω}}_{\mathrm{gmz}}\end{array}\right]}_{1\×18}^T$

$G={\left[\begin{array}{cccccc}{0}_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ C_b^n& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& C_b^n& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{12\×3}& 0_{12\×3}& 0_{12\×3}& 0_{12\×3}& 0_{12\×3}& 0_{12\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}\\ 0_{9\×3}& 0_{9\×3}& 0_{9\×3}& 0_{9\×3}& 0_{9\×3}& 0_{9\×3}\end{array}\right],}_{45\×18}$ $F={\left[\begin{array}{ccccccc}{F1}_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}\\ {F2}_{3\×9}& C_b^n& C_b^n& {F3}_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}\\ {F4}_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}& C_b^n& C_b^n& {F5}_{3\×9}\\ 0_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}\\ 0_{3\×9}& 0_{3\×3}& {F6}_{3\×3}& 0_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}\\ 0_{12\×9}& 0_{12\×3}& 0_{12\×3}& 0_{12\×9}& 0_{12\×3}& 0_{12\×3}& 0_{12\×9}\\ 0_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}& 0_{3\×3}& {F7}_{3\×3}& 0_{3\×9}\\ 0_{9\×9}& 0_{9\×3}& 0_{9\×3}& 0_{9\×9}& 0_{9\×3}& 0_{9\×3}& 0_{9\×9}\end{array}\right]}_{45\×45},$

$F1={\left[\begin{array}{ccccccccc}0& 0& -\frac{V_N}{{(R_m+h)}^2}& 0& \frac{1}{R_m+h}& 0& 0& 0& 0\\ \frac{V_E\sec L\tan L}{R_n+h}& 0& -\frac{V_E\sec L}{{(R_n+h)}^2}& \frac{\sec L}{R_n+h}& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0\end{array}\right]}_{3\×9},$

$F3={\left[\begin{array}{ccccccccc}{C}_b^n\left(\mathrm{1,1}\right)f_x& C_b^n\left(\mathrm{1,2}\right)f_y& C_b^n\left(\mathrm{1,3}\right)f_z& C_b^n\left(\mathrm{1,1}\right)f_y& C_b^n\left(\mathrm{1,2}\right)f_x& C_b^n\left(\mathrm{1,1}\right)f_z& C_b^n\left(\mathrm{1,3}\right)f_x& C_b^n\left(\mathrm{1,2}\right)f_z& C_b^n\left(\mathrm{1,3}\right)f_y\\ C_b^n\left(\mathrm{2,1}\right)f_x& C_b^n\left(\mathrm{2,2}\right)f_y& C_b^n\left(\mathrm{2,3}\right)f_z& C_b^n\left(\mathrm{2,1}\right)f_y& C_b^n\left(\mathrm{2,2}\right)f_x& C_b^n\left(\mathrm{2,1}\right)f_z& C_b^n\left(\mathrm{2,3}\right)f_x& C_b^n\left(\mathrm{2,2}\right)f_z& C_b^n\left(\mathrm{2,3}\right)f_y\\ C_b^n\left(\mathrm{3,1}\right)f_x& C_b^n\left(\mathrm{3,2}\right)f_y& C_b^n\left(\mathrm{3,3}\right)f_z& C_b^n\left(\mathrm{3,1}\right)f_y& C_b^n\left(\mathrm{3,2}\right)f_x& C_b^n\left(\mathrm{3,1}\right)f_z& C_b^n\left(\mathrm{3,3}\right)f_x& C_b^n\left(\mathrm{3,2}\right)f_z& C_b^n\left(\mathrm{3,3}\right)f_y\end{array}\right]}_{3\×9},$

$F4={\left[\begin{array}{ccccccccc}& & & & & & & {\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ 0& 0& \frac{V_N}{{(R_m+h)}^2}& 0& -\frac{1}{R_m+h}& 0& 0& +\frac{V_E\tan L}{R_n+h}& -\frac{V_E}{R_n+h}\\ & & & & & & -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& & \\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& -\frac{V_E}{{(R_n+h)}^2}& \frac{1}{R_n+h}& 0& 0& -\frac{V_E}{R_n+h}& 0& \frac{V_N}{R_m+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L& & & & & & {\mathrm{\ω}}_{\mathrm{ie}}\cos L& & \\ +\frac{V_E{\sec }^{2}L}{R_n+h}& 0& -\frac{V_E\tan L}{{(R_n+h)}^2}& \frac{\tan L}{R_n+h}& 0& 0& +\frac{V_E}{R_n+h}& \frac{V_N}{R_m+h}& 0\end{array}\right]}_{3\×9},$

$F5={\left[\begin{array}{ccccccccc}{C}_b^n\left(\mathrm{1,1}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{1,2}\right){\mathrm{\ω}}_y& C_b^n\left(\mathrm{1,3}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{1,1}\right){\mathrm{\ω}}_y& C_b^n\left(\mathrm{1,2}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{1,1}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{1,3}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{1,2}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{1,3}\right){\mathrm{\ω}}_y\\ C_b^n\left(\mathrm{2,1}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{2,2}\right){\mathrm{\ω}}_y& C_b^n\left(\mathrm{2,3}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{2,1}\right){\mathrm{\ω}}_y& C_b^n\left(\mathrm{2,2}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{2,1}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{2,3}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{2,2}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{2,3}\right){\mathrm{\ω}}_y\\ C_b^n\left(\mathrm{3,1}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{3,2}\right){\mathrm{\ω}}_y& C_b^n\left(\mathrm{3,3}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{3,1}\right){\mathrm{\ω}}_y& C_b^n\left(\mathrm{3,2}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{3,1}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{3,3}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{3,2}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{3,3}\right){\mathrm{\ω}}_y\end{array}\right]}_{3\×9},$

$F6=\mathrm{diag}(-\frac{1}{\mathrm{\α}},-\frac{1}{\mathrm{\α}},-\frac{1}{\mathrm{\α}}),$ $F7=\mathrm{diag}(-\frac{1}{\mathrm{\β}},-\frac{1}{\mathrm{\β}},-\frac{1}{\mathrm{\β}});$ ω_ax、ω_ay、ω_az为加速度计三个测量轴上的白噪声偏置；ω_gx、ω_gy、ω_gz为陀螺三个测量轴上的白噪声漂移；ω_arx、ω_ary、ω_arz为加速度计三个测量轴上的随机游走偏置驱动白噪声；ω_amx、ω_amy、ω_amz为加速度计三个测量轴上的一阶马尔科夫过程偏置驱动白噪声；ω_grx、ω_gry、ω_grz为陀螺三个测量轴上的随机游走漂移驱动白噪声；ω_gmx、ω_gmy、ω_gmz为陀螺三个测量轴上的一阶马尔科夫过程漂移驱动白噪声；

为载体坐标系到地理系的转换矩阵，I为单位阵，V_E、V_N、V_U为运动载体速度在地理系下的分量，R_n与R_m分别为卯酉圈与子午圈的主曲率半径，h为运动载体的高度，L为运动载体的纬度，ω_ie为地球自转角速度，f_E、f_N、f_U为运动载体加速度在地理坐标系下的分量，f_x、f_y、f_z为运动载体加速度在载体坐标系下的分量，ω_x、ω_y、ω_z为运动载体角速度在载体坐标系下的分量，α与β为陀螺和加速度计一阶马尔科夫过程的相关时间。

b)量测方程

将SINS输出的位置和速度信息与GNSS的相应输出信息相减，形成位置姿态系统的量测信息，其量测方程如下：

Z＝H·X+v    (4)

式中，Z为量测向量，H为量测矩阵，X为系统状态向量，v为测量噪声向量，具体表达式如下：

$Z={\left[\begin{array}{c}{L}_{\mathrm{SINS}}-L_{\mathrm{GNSS}}\\ {\mathrm{\λ}}_{\mathrm{SINS}}-{\mathrm{\λ}}_{\mathrm{GNSS}}\\ h_{\mathrm{SINS}}-h_{\mathrm{GNSS}}\\ V_{\mathrm{ESINS}}-V_{\mathrm{EGNSS}}\\ V_{\mathrm{NSINS}}-V_{\mathrm{NGNSS}}\\ V_{\mathrm{USINS}}-V_{\mathrm{UGNSS}}\end{array}\right]}_{6\×1},$ $v={\left[\begin{array}{c}{v}_L\\ v_{\mathrm{\λ}}\\ v_h\\ v_{\mathrm{ve}}\\ v_{\mathrm{vn}}\\ v_{\mathrm{vu}}\end{array}\right]}_{6\×1},$ $H={\left[\begin{array}{ccccccc}{R}_m& 0& 0& 0& 0& 0& 0_{1\×39}\\ 0& R_n\cos L& 0& 0& 0& 0& 0_{1\×39}\\ 0& 0& 1& 0& 0& 0& 0_{1\×39}\\ 0& 0& 0& 1& 0& 0& 0_{1\×39}\\ 0& 0& 0& 0& 1& 0& 0_{1\×39}\\ 0& 0& 0& 0& 0& 1& 0_{1\×39}\end{array}\right]}_{6\×45};$

L_SINS-L_GNSS，λ_SINS-λ_GNSS，h_SINS-h_GNSS，V_ESINS-V_EGNSS，V_NSINS-V_NGNSS，V_USINS-V_UGNSS，为SINS与GNSS输出的纬度、经度、高度、东向速度、北向速度、天向速度之差；v_L，v_λ，v_h，v_ve，v_vn，v_vu为GNSS的纬度、经度、高度、东向速度、北向速度、天向速度的测量噪声，均为零均值随机白噪声，其方差阵为R；R_n与R_m分别为卯酉圈与子午圈的主曲率半径，L为当地纬度。

2、设计基于三参数递推的卡尔曼滤波器

对传统的单参数递推卡尔曼滤波器进行改进，设计出基于三参数递推滤波模型的卡尔曼滤波器，设计步骤如下：

a)将系统状态方程离散化，为：

$\left\{\begin{array}{c}{X}_k=F_{k/k-1}{X}_{k-1}+W_{k-1}\\ F_{k/k-1}=I+\mathrm{TF}\left(t_{k-1}\right)+\frac{T^2}{2!}{F}^2\left(t_{k-1}\right)+\frac{T^3}{3!}{F}^3\left(t_{k-1}\right)+\·\·\·\end{array}\right.---\left(5\right)$

式中，下标k-1、k分别表示t_k-1和t_k时刻，X_k、X_k-1为t_k、t_k-1时刻的系统状态向量，W_k-1为t_k-1时刻系统噪声向量阵，F_k/k-1为t_k-1时刻到t_k时刻的系统状态一步转移阵，I为单位阵，T＝t_k-t_k-1为滤波周期，F(t_k-1)为常数矩阵。

b)在离散化后的系统状态方程和量测方程引入基于三参数递推的滤波模型

$\left\{\begin{array}{c}{X}_k=\left[\begin{array}{ccc}{C}_1{F}_{k/k-1}& C_2{F}_{k/k-1}& C_3{F}_{k/k-1}\end{array}\right]\left[\begin{array}{c}{X}_{k-1}\\ X_{k-2}\\ X_{k-3}\end{array}\right]+W_{k-1}\\ \left[\begin{array}{c}{Z}_k\\ Z_{k-1}\\ Z_{k-2}\end{array}\right]=H\left[\begin{array}{c}{X}_k\\ X_{k-1}\\ X_{k-2}\end{array}\right]+\left[\begin{array}{c}{v}_k\\ v_{k-1}\\ v_{k-2}\end{array}\right]\end{array}\right.---\left(6\right)$

式中，下标k、k-1、k-2分别表示t_k、t_k-1、t_k-2时刻，X_k、X_k-1、X_k-2、X_k-3为t_k、t_k-1、t_k-2、t_k-3时刻的系统状态向量，F_k/k-1为k-1时刻到k时刻的系统状态一步转移阵，F_k/k-2为k-2时刻到k时刻的系统状态转移阵，F_k/k-3为k-3时刻到k时刻的系统状态转移阵，W_k-1为t_k-1时刻系统噪声向量阵，Z_k、Z_k-1、Z_k-2为t_k、t_k-1、t_k-2时刻的量测向量，H为量测矩阵，v_k、v_k-1、v_k-2为t_k、t_k-1、t_k-2时刻的测量噪声向量。C₁、C₂、C₃为状态转移阵F_k/k-1、F_k/k-2、F_k/k-3的加权系数。

加权系数C₁、C₂、C₃选取原则为：

①C₁+C₂+C₃＝1；

②C₁＞C₂＞C₃。

系统状态转移阵F_k/k-2、F_k/k-3的计算公式如下所示：

$\left\{\begin{array}{c}{F}_{k/k-2}=F_{k/k-1}{F}_{k-1/k-2}\\ F_{k/k-3}=F_{k/k-1}{F}_{k-1/k-2}{F}_{k-2/k-3}\end{array}\right.---\left(7\right)$

3、采用基于三参数递推的卡尔曼滤波器对误差状态进行最优滤波估计基于步骤1建立的位置姿态系统的高阶误差模型，取SINS与GNSS的定位结果之差作为观测量，采用SINS/GNSS分布式组合形式，通过步骤2设计的基于三参数递推的卡尔曼滤波器对系统误差状态进行最优滤波估计，执行步骤如下所示：

a)给定系统状态向量估计值的初值

及其均方误差阵的初值P₀

b)计算系统状态向量的一步预测

$\left\{\begin{array}{c}{\hat{X}}_{k/k-1}=A_k\left[\begin{array}{c}{\hat{X}}_{k-1}\\ {\hat{X}}_{k-2}\\ {\hat{X}}_{k-3}\end{array}\right]\\ A_k=\left[\begin{array}{ccc}{C}_1{F}_{k/k-1}& C_2{F}_{k/k-2}& C_3{F}_{k/k-3}\end{array}\right]\end{array}\right.---\left(8\right)$

式中，下标k/k-1表示从t_k-1时刻到t_k时刻的预测；A_k为基于三参数递推滤波模型的状态转移阵，

为系统状态向量在t_k-1、t_k-2、t_k-3时刻的估计值，F_k/k-1为k-1时刻到k时刻的系统状态一步转移阵，F_k/k-2为k-2时刻到k时刻的系统状态转移阵，F_k/k-3为k-3时刻到k时刻的系统状态转移阵，C₁、C₂、C₃为状态转移阵F_k/k-1、F_k/k-2、F_k/k-3的加权系数。

c)计算系统状态向量的一步预测均方误差阵P_k/k-1

$P_{k/k-1}=A_k{P}_{k-1}{A}_k^T+G_{k-1}{Q}_{k-1}{G}_{k-1}^T---\left(9\right)$

式中，P_k/k-1为t_k-1时刻到tk时刻的系统状态向量一步预测均方误差阵，P_k-1为t_k-1时刻的系统状态向量估计的均方误差阵，A_k为基于三参数递推滤波模型的状态转移阵，G_k-1为t_k-1时刻的系统噪声分配矩阵，Q_k-1为t_k-1时刻的系统噪声均方误差阵。

d)计算滤波增益阵K_k

$K_k=A_k{P}_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_K)}^{-1}---\left(10\right)$

式中，A_k为基于三参数递推滤波模型的状态转移阵，P_k/k-1为t_k-1时刻到t_k时刻的系统状态向量一步预测均方误差阵，H_k为t_k时刻的量测阵，R_k为t_k时刻的量测噪声均方误差阵。

e)计算系统状态向量的估计

${\hat{X}}_K={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(11\right)$

式中，

为系统状态向量在tk时刻的估计值，

为t_k-1时刻到t_k时刻的系统状态向量的一步预测，K_k为t_k时刻的滤波增益阵，H_k为t_k时刻的量测阵，Z_k为t_k时刻的观测向量，即SINS与GNSS的定位结果之差。

f)计算系统状态向量估计的均方误差阵P_k

$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(12\right)$

式中，I为单位阵，K_k为t_k时刻的滤波增益阵，H_k为t_k时刻的量测阵，R_k为t_k时刻的量测噪声均方误差阵。

4、用位置误差、速度误差、姿态误差的最优滤波估计值修正SINS的位置、速度、姿态信息

将步骤3中计算得到的位置误差、速度误差、姿态误差和惯性器件误差的最优滤波估计值反馈回SINS，并对陀螺漂移、加速度计偏置以及载体坐标系相对于计算坐标系的转换矩阵进行校正，计算出更加精确的位置、速度和姿态信息。修正SINS的位置、速度、姿态的公式为：

a)位置修正

${\mathrm{Lat}}^{\′}=\mathrm{Lat}-\widehat{\mathrm{\δ}}L---\left(13\right)$

${\mathrm{Lon}}^{\′}=\mathrm{Lon}-\widehat{\mathrm{\δ}}\mathrm{\λ}---\left(14\right)$

${\mathrm{Height}}^{\′}=\mathrm{Height}-\widehat{\mathrm{\δ}}h---\left(15\right)$

式中，Lat′、Lon′、Height′为修正后的纬度、经度、高度信息；Lat、Lon、Height为SINS输出的纬度、经度、高度信息；为纬度误差、经度误差、高度误差的最优滤波估计值。

b)速度修正

${\mathrm{Ve}}^{\′}=\mathrm{Ve}-\widehat{\mathrm{\δ}}\mathrm{Ve}---\left(16\right)$

${\mathrm{Vn}}^{\′}=\mathrm{Vn}-\widehat{\mathrm{\δ}}\mathrm{Vn}---\left(17\right)$

${\mathrm{Vu}}^{\′}=\mathrm{Vu}-\widehat{\mathrm{\δ}}\mathrm{Vu}---\left(18\right)$

式中，Ve′、Vn′、Vu′为修正后的东向速度、北向速度、天向速度信息；Ve、Vn、Vu为SINS输出的东向速度、北向速度、天向速度信息；

为东向速度误差、北向速度误差、天向速度误差的最优滤波估计值。

c)姿态修正

$C_n^c={\left[\begin{array}{ccc}{\cos \mathrm{\φ}}_U\×{\cos \mathrm{\φ}}_N-\sin {\mathrm{\φ}}_U\×{\sin \mathrm{\φ}}_E\×\sin {\mathrm{\φ}}_N& {\sin \mathrm{\φ}}_U\×{\cos \mathrm{\φ}}_N+{\cos \mathrm{\φ}}_U\×{\sin \mathrm{\φ}}_E\×{\sin \mathrm{\φ}}_N& -{\cos \mathrm{\φ}}_E\×{\sin \mathrm{\φ}}_N\\ -\sin {\mathrm{\φ}}_U\×{\cos \mathrm{\φ}}_E& {\cos \mathrm{\φ}}_U\×{\cos \mathrm{\φ}}_E& {\sin \mathrm{\φ}}_E\\ {\cos \mathrm{\φ}}_U\×{\sin \mathrm{\φ}}_N+{\sin \mathrm{\φ}}_U\×{\sin \mathrm{\φ}}_E\×{\cos \mathrm{\φ}}_N& {\sin \mathrm{\φ}}_U\×{\sin \mathrm{\φ}}_N-{\cos \mathrm{\φ}}_U\×{\sin \mathrm{\φ}}_E\×{\cos \mathrm{\φ}}_N& {\cos \mathrm{\φ}}_E\×{\cos \mathrm{\φ}}_N\end{array}\right]}_{3\×3}---\left(19\right)$

式中，

为地理系n到计算系c的方向余弦矩阵，即姿态误差矩阵；φ_E、φ_N、φ_U为东向水平姿态失准角、北向水平姿态失准角、航向失准角的最优滤波估计值。

$C_n^b=C_c^b\×C_n^c---\left(20\right)$

其中，

为地理系n到载体系b的方向余弦矩阵，即修正后的姿态矩阵；为计算系c到载体系b的方向余弦矩阵，即SINS输出的姿态矩阵；根据

即可计算出载体的姿态角，包括航向角、俯仰角、横滚角，计算方法如下。

将(20)式求得的

记为

$C_n^b={\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]}_{3\×3}---\left(21\right)$

式中，C_ij(i＝1，2，3，j＝1，2，3)表示

的第i行j列元素。

又因为：

式中，

θ、γ为所求的载体航向角、俯仰角、横滚角。

通过(21)式和(22)式，即可确定载体航向角

俯仰角θ、横滚角γ。

①计算航向角

的真值判断如下表：

②计算俯仰角θ

θ＝arcsin(C₂₃)    (24)

③计算横滚角γ

γ真值判断如下表：

C33	C13	γ真值	γ所在象限
				+	-	γ主	第一象限
-	-	γ主-π	第二象限
				-	+	γ主+π	第三象限
+	+	γ主	第四象限

其中，航向角定义在[0，2π]区间，横滚角γ定义在[-π，π]区间，计算和γ的反正切函数存在多值情况，必须做真值判断，以确定真值落在哪一个象限；俯仰角θ定义在

区间，计算θ的反正弦函数不存在多值情况，不需做真值判断。

Claims (3)

1.一种位置姿态系统的滤波方法，其特征在于：包括下列步骤：

(1)建立位置姿态系统的高阶误差模型；

位置姿态系统的高阶误差模型包括系统状态方程和量测方程：

a)系统状态方程

$\stackrel{\·}{X}=F\·X+G\·W$

其中， $X=\left[\begin{array}{ccccccccccccccc}\mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\δV}}_E& {\mathrm{\δV}}_N& {\mathrm{\δV}}_U& {\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& {\▿}_{\mathrm{bx}}& {\▿}_{\mathrm{by}}& {\▿}_{\mathrm{bz}}& {\▿}_{\mathrm{rx}}& {\▿}_{\mathrm{ry}}& {\▿}_{\mathrm{rz}}\end{array}\right.$ $\begin{array}{ccccccccccccccccccccc}{\▿}_{\mathrm{mx}}& {\▿}_{\mathrm{my}}& {\▿}_{\mathrm{mz}}& {\mathrm{\δK}}_{\mathrm{Ax}}& {\mathrm{\δK}}_{\mathrm{Ay}}& {\mathrm{\δK}}_{\mathrm{Az}}& {\mathrm{\δA}}_{\mathrm{yz}}& {\mathrm{\δA}}_{\mathrm{zy}}& {\mathrm{\δA}}_{\mathrm{xz}}& {\mathrm{\δA}}_{\mathrm{zx}}& {\mathrm{\δA}}_{\mathrm{xy}}& {\mathrm{\δA}}_{\mathrm{yx}}& {\mathrm{\ϵ}}_{\mathrm{bx}}& {\mathrm{\ϵ}}_{\mathrm{by}}& {\mathrm{\ϵ}}_{\mathrm{bz}}& {\mathrm{\ϵ}}_{\mathrm{rx}}& {\mathrm{\ϵ}}_{\mathrm{ry}}& {\mathrm{\ϵ}}_{\mathrm{rz}}& {\mathrm{\ϵ}}_{\mathrm{mx}}& {\mathrm{\ϵ}}_{\mathrm{my}}& {\mathrm{\ϵ}}_{\mathrm{mz}}\end{array}$ ${\left.\begin{array}{ccccccccc}{\mathrm{\δK}}_{\mathrm{Gx}}& {\mathrm{\δK}}_{\mathrm{Gy}}& {\mathrm{\δK}}_{\mathrm{Gz}}& {\mathrm{\δG}}_{\mathrm{yz}}& {\mathrm{\δG}}_{\mathrm{zy}}& {\mathrm{\δG}}_{\mathrm{xz}}& {\mathrm{\δG}}_{\mathrm{zx}}& {\mathrm{\δG}}_{\mathrm{xy}}& {\mathrm{\δG}}_{\mathrm{yx}}\end{array}\right]}_{1\×45}^T$ 为系统状态向量；δL、δλ、δh为纬度误差、经度误差、高度误差；δV_E、δV_N、δV_U为东向速度误差、北向速度误差、天向速度误差；φ_E、φ_N、φ_U为东向水平姿态失准角、北向水平姿态失准角、航向失准角；

为加速度计三个测量轴上的随机常值偏置；为加速度计三个测量轴上的随机游走偏置； 为加速度计三个测量轴上的一阶马尔科夫过程偏置；δK_Ax、δK_Ay、δK_Az为加速度计三个测量轴上的刻度因子误差；δA_zx、δA_yx，δA_zy、δA_xy，δA_yz、δA_xz为加速度计三个测量轴上的安装误差，每个加速度计测量轴的安装误差用两个参数表示，因此加速度计三个测量轴的安装误差共六个；ε_bx、ε_by、ε_bz为陀螺三个测量轴上的随机常值漂移；ε_rx、ε_ry、ε_rz为陀螺三个测量轴上的随机游走漂移；ε_mx、ε_my、ε_mz为陀螺三个测量轴上的一阶马尔科夫过程漂移；δK_Gx、δK_Gy、δK_Gz为陀螺三个测量轴上的刻度因子误差；δG_zx、δG_yx，δG_zy、δG_xy，δG_yz、δG_xz为陀螺三个测量轴上的安装误差，每个陀螺测量轴的安装误差用两个参数表示，因此陀螺三个测量轴的安装误差共六个；为系统状态向量的导数，F为系统状态转移矩阵，G为系统噪声分配矩阵，W为系统噪声向量，具体表达式如下：

$W={\left[\begin{array}{cccccccccccccccccc}{\mathrm{\ω}}_{\mathrm{ax}}& {\mathrm{\ω}}_{\mathrm{ay}}& {\mathrm{\ω}}_{\mathrm{az}}& {\mathrm{\ω}}_{\mathrm{gx}}& {\mathrm{\ω}}_{\mathrm{gy}}& {\mathrm{\ω}}_{\mathrm{gz}}& {\mathrm{\ω}}_{\mathrm{arx}}& {\mathrm{\ω}}_{\mathrm{ary}}& {\mathrm{\ω}}_{\mathrm{arz}}& {\mathrm{\ω}}_{\mathrm{amx}}& {\mathrm{\ω}}_{\mathrm{amy}}& {\mathrm{\ω}}_{\mathrm{amz}}& {\mathrm{\ω}}_{\mathrm{grx}}& {\mathrm{\ω}}_{\mathrm{gry}}& {\mathrm{\ω}}_{\mathrm{grz}}& {\mathrm{\ω}}_{\mathrm{gmx}}& {\mathrm{\ω}}_{\mathrm{gmy}}& {\mathrm{\ω}}_{\mathrm{gmz}}\end{array}\right]}_{1\×18}^T$

$G={\left[\begin{array}{cccccc}{0}_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ C_b^n& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& C_b^n& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{12\×3}& 0_{12\×3}& 0_{12\×3}& 0_{12\×3}& 0_{12\×3}& 0_{12\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}\\ 0_{9\×3}& 0_{9\×3}& 0_{9\×3}& 0_{9\×3}& 0_{9\×3}& 0_{9\×3}\end{array}\right],}_{45\×18}$ $F={\left[\begin{array}{ccccccc}{F1}_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}\\ {F2}_{3\×9}& C_b^n& C_b^n& {F3}_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}\\ {F4}_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}& C_b^n& C_b^n& {F5}_{3\×9}\\ 0_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}\\ 0_{3\×9}& 0_{3\×3}& {F6}_{3\×3}& 0_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}\\ 0_{12\×9}& 0_{12\×3}& 0_{12\×3}& 0_{12\×9}& 0_{12\×3}& 0_{12\×3}& 0_{12\×9}\\ 0_{3\×9}& 0_{3\×3}& 0_{3\×3}& 0_{3\×9}& 0_{3\×3}& {F7}_{3\×3}& 0_{3\×9}\\ 0_{9\×9}& 0_{9\×3}& 0_{9\×3}& 0_{9\×9}& 0_{9\×3}& 0_{9\×3}& 0_{9\×9}\end{array}\right]}_{45\×45},$

$F1={\left[\begin{array}{ccccccccc}0& 0& -\frac{V_N}{{(R_m+h)}^2}& 0& \frac{1}{R_m+h}& 0& 0& 0& 0\\ \frac{V_E\sec L\tan L}{R_n+h}& 0& -\frac{V_E\sec L}{{(R_n+h)}^2}& \frac{\sec L}{R_n+h}& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0\end{array}\right]}_{3\×9},$

$F3={\left[\begin{array}{ccccccccc}{C}_b^n\left(\mathrm{1,1}\right)f_x& C_b^n\left(\mathrm{1,2}\right)f_y& C_b^n\left(\mathrm{1,3}\right)f_z& C_b^n\left(\mathrm{1,1}\right)f_y& C_b^n\left(\mathrm{1,2}\right)f_x& C_b^n\left(\mathrm{1,1}\right)f_z& C_b^n\left(\mathrm{1,3}\right)f_x& C_b^n\left(\mathrm{1,2}\right)f_z& C_b^n\left(\mathrm{1,3}\right)f_y\\ C_b^n\left(\mathrm{2,1}\right)f_x& C_b^n\left(\mathrm{2,2}\right)f_y& C_b^n\left(\mathrm{2,3}\right)f_z& C_b^n\left(\mathrm{2,1}\right)f_y& C_b^n\left(\mathrm{2,2}\right)f_x& C_b^n\left(\mathrm{2,1}\right)f_z& C_b^n\left(\mathrm{2,3}\right)f_x& C_b^n\left(\mathrm{2,2}\right)f_z& C_b^n\left(\mathrm{2,3}\right)f_y\\ C_b^n\left(\mathrm{3,1}\right)f_x& C_b^n\left(\mathrm{3,2}\right)f_y& C_b^n\left(\mathrm{3,3}\right)f_z& C_b^n\left(\mathrm{3,1}\right)f_y& C_b^n\left(\mathrm{3,2}\right)f_x& C_b^n\left(\mathrm{3,1}\right)f_z& C_b^n\left(\mathrm{3,3}\right)f_x& C_b^n\left(\mathrm{3,2}\right)f_z& C_b^n\left(\mathrm{3,3}\right)f_y\end{array}\right]}_{3\×9},$

$F5={\left[\begin{array}{ccccccccc}{C}_b^n\left(\mathrm{1,1}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{1,2}\right){\mathrm{\ω}}_y& C_b^n\left(\mathrm{1,3}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{1,1}\right){\mathrm{\ω}}_y& C_b^n\left(\mathrm{1,2}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{1,1}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{1,3}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{1,2}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{1,3}\right){\mathrm{\ω}}_y\\ C_b^n\left(\mathrm{2,1}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{2,2}\right){\mathrm{\ω}}_y& C_b^n\left(\mathrm{2,3}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{2,1}\right){\mathrm{\ω}}_y& C_b^n\left(\mathrm{2,2}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{2,1}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{2,3}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{2,2}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{2,3}\right){\mathrm{\ω}}_y\\ C_b^n\left(\mathrm{3,1}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{3,2}\right){\mathrm{\ω}}_y& C_b^n\left(\mathrm{3,3}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{3,1}\right){\mathrm{\ω}}_y& C_b^n\left(\mathrm{3,2}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{3,1}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{3,3}\right){\mathrm{\ω}}_x& C_b^n\left(\mathrm{3,2}\right){\mathrm{\ω}}_z& C_b^n\left(\mathrm{3,3}\right){\mathrm{\ω}}_y\end{array}\right]}_{3\×9},$

$F6=\mathrm{diag}(-\frac{1}{\mathrm{\α}},-\frac{1}{\mathrm{\α}},-\frac{1}{\mathrm{\α}}),$ $F7=\mathrm{diag}(-\frac{1}{\mathrm{\β}},-\frac{1}{\mathrm{\β}},-\frac{1}{\mathrm{\β}});$ ω_ax、ω_ay、ω_az为加速度计三个测量轴上的白噪声偏置；ω_gx、ω_gy、ω_gz为陀螺三个测量轴上的白噪声漂移；ω_arx、ω_ary、ω_arz为加速度计三个测量轴上的随机游走偏置驱动白噪声；ω_amx、ω_amy、ω_amz为加速度计三个测量轴上的一阶马尔科夫过程偏置驱动白噪声；ω_grx、ω_gry、ω_grz为陀螺三个测量轴上的随机游走漂移驱动白噪声；ω_gmx、ω_gmy、ω_gmz为陀螺三个测量轴上的一阶马尔科夫过程漂移驱动白噪声；为载体坐标系到地理系的转换矩阵，I为单位阵，V_E、V_N、V_U为运动载体速度在地理系下的分量，R_n与R_m分别为卯酉圈与子午圈的主曲率半径，h为运动载体的高度，L为运动载体的纬度，ω_ie为地球自转角速度，f_E、f_N、f_U为运动载体加速度在地理坐标系下的分量，f_x、f_y、f_z为运动载体加速度在载体坐标系下的分量，ω_x、ω_y、ω_z为运动载体角速度在载体坐标系下的分量，α与β为陀螺和加速度计一阶马尔科夫过程的相关时间；

b)量测方程

Z＝H·X+v

其中，Z为量测向量，H为量测矩阵，X为系统状态向量，v为测量噪声向量，具体表达式如下：

$Z={\left[\begin{array}{c}{L}_{\mathrm{SINS}}-L_{\mathrm{GNSS}}\\ {\mathrm{\λ}}_{\mathrm{SINS}}-{\mathrm{\λ}}_{\mathrm{GNSS}}\\ h_{\mathrm{SINS}}-h_{\mathrm{GNSS}}\\ V_{\mathrm{ESINS}}-V_{\mathrm{EGNSS}}\\ V_{\mathrm{NSINS}}-V_{\mathrm{NGNSS}}\\ V_{\mathrm{USINS}}-V_{\mathrm{UGNSS}}\end{array}\right]}_{6\×1},$ $v={\left[\begin{array}{c}{v}_L\\ v_{\mathrm{\λ}}\\ v_h\\ v_{\mathrm{ve}}\\ v_{\mathrm{vn}}\\ v_{\mathrm{vu}}\end{array}\right]}_{6\×1},$ $H={\left[\begin{array}{ccccccc}{R}_m& 0& 0& 0& 0& 0& 0_{1\×39}\\ 0& R_n\cos L& 0& 0& 0& 0& 0_{1\×39}\\ 0& 0& 1& 0& 0& 0& 0_{1\×39}\\ 0& 0& 0& 1& 0& 0& 0_{1\×39}\\ 0& 0& 0& 0& 1& 0& 0_{1\×39}\\ 0& 0& 0& 0& 0& 1& 0_{1\×39}\end{array}\right]}_{6\×45};$

L_SINS-L_GNSS，λ_SINS-λ_GNSS，h_SINS-h_GNSS，V_ESINS-V_EGNSS，V_NSINS-V_NGNSS，V_USINS-V_UGNSS，为SINS与GNSS输出的纬度、经度、高度、东向速度、北向速度、天向速度之差；v_L，v_λ，v_h，v_ve，v_vn，v_vu为GNSS的纬度、经度、高度、东向速度、北向速度、天向速度的测量噪声，均为零均值随机白噪声，其方差阵为R；R_n与R_m分别为卯酉圈与子午圈的主曲率半径，L为当地纬度；

(2)设计基于三参数递推的卡尔曼滤波器，其设计步骤为：

a)将系统状态方程

离散化，为：

$\left\{\begin{array}{c}{X}_k=F_{k/k-1}{X}_{k-1}+W_{k-1}\\ F_{k/k-1}=I+\mathrm{TF}\left(t_{k-1}\right)+\frac{T^2}{2!}{F}^2\left(t_{k-1}\right)+\frac{T^3}{3!}{F}^3\left(t_{k-1}\right)+\·\·\·\end{array}\right.$

其中，下标k-1、k分别表示t_k-1和t_k时刻，X_k、X_k-1为t_k、t_k-1时刻的系统状态向量，W_k-1为t_k-1时刻系统噪声向量阵，F_k/k-1为t_k-1时刻到t_k时刻的系统状态一步转移阵，I为单位阵，T＝t_k-t_k-1为滤波周期，F(t_k-1)为常数矩阵；

b)在离散化后的系统状态方程和量测方程引入基于三参数递推的滤波模型：

$X_k=\left[\begin{array}{ccc}{C}_1{F}_{k/k-1}& C_2{F}_{k/k-2}& C_3{F}_{k/k-3}\end{array}\right]\left[\begin{array}{c}{X}_{k-1}\\ X_{k-2}\\ X_{k-3}\end{array}\right]+W_{k-1},$ $\left[\begin{array}{c}{Z}_k\\ Z_{k-1}\\ Z_{k-2}\end{array}\right]=H\left[\begin{array}{c}{X}_k\\ X_{k-1}\\ X_{k-2}\end{array}\right]+\left[\begin{array}{c}{v}_k\\ v_{k-1}\\ v_{k-2}\end{array}\right]$

其中，下标k、k-1、k-2分别表示t_k、t_k-1、t_k-2时刻，X_k、X_k-1、X_k-2、X_k-3为t_k、t_k-1、t_k-2、t_k-3时刻的系统状态向量，F_k/k-1为k-1时刻到k时刻的系统状态一步转移阵，F_k/k-2为k-2时刻到k时刻的系统状态转移阵，F_k/k-3为k-3时刻到k时刻的系统状态转移阵，C₁、C₂、C₃为状态转移阵F_k/k-1、F_k/k-2、F_k/k-3的加权系数，W_k-1为t_k-1时刻系统噪声向量阵，Z_k、Z_k-1、Z_k-2为t_k、t_k-1、t_k-2时刻的量测向量，H为量测矩阵，v_k、v_k-1、v_k-2为t_k、t_k-1、t_k-2时刻的测量噪声向量；

(3)基于步骤(1)中建立的位置姿态系统的高阶误差模型，取SINS与GNSS的定位结果之差作为观测量，采用SINS/GNSS分布式组合形式，通过步骤(2)设计的基于三参数递推的卡尔曼滤波器对各误差状态进行最优滤波估计；

(4)将步骤(3)中计算得到的位置误差、速度误差、姿态误差和惯性器件误差的最优滤波估计值反馈回SINS，并对陀螺漂移、加速度计偏置以及载体坐标系相对于计算坐标系的转换矩阵进行校正，计算出更加精确的位置、速度和姿态信息；

修正SINS的位置、速度、姿态的公式为：

a)位置修正

${\mathrm{Lat}}^{\′}=\mathrm{Lat}-\widehat{\mathrm{\δ}}L$

${\mathrm{Lon}}^{\′}=\mathrm{Lon}-\widehat{\mathrm{\δ}}\mathrm{\λ}$

${\mathrm{Height}}^{\′}=\mathrm{Height}-\widehat{\mathrm{\δ}}h$

其中，Lat′、Lon′、Height′为修正后的纬度、经度、高度信息；Lat、Lon、Height为SINS输出的纬度、经度、高度信息；

为纬度误差、经度误差、高度误差的最优滤波估计值；

b)速度修正

${\mathrm{Ve}}^{\′}=\mathrm{Ve}-\widehat{\mathrm{\δ}}\mathrm{Ve}$

${\mathrm{Vn}}^{\′}=\mathrm{Vn}-\widehat{\mathrm{\δ}}\mathrm{Vn}$

${\mathrm{Vu}}^{\′}=\mathrm{Vu}-\widehat{\mathrm{\δ}}\mathrm{Vu}$

其中，Ve′、Vn′、Vu′为修正后的东向速度、北向速度、天向速度信息；Ve、Vn、Vu为SINS输出的东向速度、北向速度、天向速度信息；

为东向速度误差、北向速度误差、天向速度误差的最优滤波估计值；

c)姿态修正

$C_n^c=\left[\begin{array}{ccc}{\cos \mathrm{\φ}}_U\×{\cos \mathrm{\φ}}_N-\sin {\mathrm{\φ}}_U\×{\sin \mathrm{\φ}}_E\×{\sin \mathrm{\φ}}_N& {\sin \mathrm{\φ}}_U\×{\cos \mathrm{\φ}}_N+{\cos \mathrm{\φ}}_U\×{\sin \mathrm{\φ}}_E\×{\sin \mathrm{\φ}}_N& -{\cos \mathrm{\φ}}_E\×{\sin \mathrm{\φ}}_N\\ -{\sin \mathrm{\φ}}_U\×{\cos \mathrm{\φ}}_E& {\cos \mathrm{\φ}}_U\×{\cos \mathrm{\φ}}_E& {\sin \mathrm{\φ}}_E\\ {\cos \mathrm{\φ}}_U\×{\sin \mathrm{\φ}}_N+{\sin \mathrm{\φ}}_U\×{\sin \mathrm{\φ}}_E\×{\cos \mathrm{\φ}}_N& \sin {\mathrm{\φ}}_U\×{\sin \mathrm{\φ}}_N-\cos {\mathrm{\φ}}_U\×{\sin \mathrm{\φ}}_E\×{\cos \mathrm{\φ}}_N& {\cos \mathrm{\φ}}_E\×{\cos \mathrm{\φ}}_N\end{array}\right]$

其中，

为地理系n到计算系c的方向余弦矩阵，即姿态误差矩阵；φ_E、φ_N、φ_U为东向水平姿态失准角、北向水平姿态失准角、航向失准角的最优滤波估计值：

$C_n^b=C_c^b\×C_n^c$

其中，

为地理系n到载体系b的方向余弦矩阵，即修正后的姿态矩阵；为计算系c到载体系b的方向余弦矩阵，即SINS输出的姿态矩阵；根据即可计算出载体的姿态角，包括航向角、俯仰角、横滚角。

2.根据权利要求1所述一种位置姿态系统的滤波方法，其特征在于：步骤(1)所述位置姿态系统的高阶误差模型包含的误差源为陀螺随机漂移、陀螺刻度因子误差、陀螺安装误差、加速度计随机偏置、加速度计刻度因子误差和加速度计安装误差，共9类误差。

陀螺随机漂移和加速度计随机偏置采用四种分量表示：随机常值、随机游走、一阶马尔科夫过程和白噪声，数学模型为：

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_b=0,{\stackrel{\·}{\mathrm{\ϵ}}}_r={\mathrm{\ω}}_{\mathrm{gr}},{\stackrel{\·}{\mathrm{\ϵ}}}_m=-\frac{1}{\mathrm{\α}}{\mathrm{\ϵ}}_m+{\mathrm{\ω}}_{\mathrm{gm}},E\left[{\mathrm{\ω}}_g\left(t\right){\mathrm{\ω}}_g\left(\mathrm{\τ}\right)\right]=q_g\mathrm{\δ}(t-\mathrm{\τ})\\ {\stackrel{\·}{\▿}}_b=0,{\stackrel{\·}{\▿}}_r={\mathrm{\ω}}_{\mathrm{ar}},{\stackrel{\·}{\▿}}_m=-\frac{1}{\mathrm{\β}}{\▿}_m+{\mathrm{\ω}}_{\mathrm{am}},E\left[{\mathrm{\ω}}_a\left(t\right){\mathrm{\ω}}_a\left(\mathrm{\τ}\right)\right]=q_a\mathrm{\δ}(t-\mathrm{\τ})\end{array}\right.$

其中，ε_b、

为陀螺随机常值漂移和加速度计随机常值偏置，

为其导数；ε_r、

为陀螺随机游走漂移和加速度计随机游走偏置，为其导数；ε_m、

为陀螺一阶马尔科夫过程漂移和加速度计一阶马尔科夫过程偏置，

为其导数；ω_gr与ω_ar分别是陀螺和加速度计的随机游走驱动白噪声；ω_gm与ω_am分别是陀螺和加速度计的一阶马尔科夫过程驱动白噪声，α与β陀螺和加速度计一阶马尔科夫过程的相关时间；ω_g、ω_a为陀螺白噪声漂移和加速度计白噪声偏置，q_g与q_a分别为陀螺和加速度计的白噪声强度，δ(t-τ)为狄拉克δ函数；

陀螺和加速度计刻度因子误差以及安装误差均采用随机常值表示，数学模型为：

$\left\{\begin{array}{cc}\mathrm{\δ}{\stackrel{\·}{K}}_g=0,& \mathrm{\δ}\stackrel{\·}{G}=0\\ \mathrm{\δ}{\stackrel{\·}{K}}_a=0,& \mathrm{\δ}\stackrel{\·}{A}=0\end{array}\right.$

其中，δK_g、δK_a为陀螺和加速度计的刻度因子误差，

为其导数；δG、δA为陀螺和加速度计的安装误差，

为其导数。

3.根据权利要求1所述的一种位置姿态系统的滤波方法，其特征在于：步骤(2)所述基于三参数递推的滤波模型为：

$X_k=\left[\begin{array}{ccc}{C}_1{F}_{k/k-1}& C_2{F}_{k/k-2}& C_3{F}_{k/k-3}\end{array}\right]\left[\begin{array}{c}{X}_{k-1}\\ X_{k-2}\\ X_{k-3}\end{array}\right]+W_{k-1}$

系统状态转移阵F_k/k-2、F_k/k-3的计算公式如下所示：

$\left\{\begin{array}{c}{F}_{k/k-2}=F_{k/k-1}{F}_{k-1/k-2}\\ F_{k/k-3}=F_{k/k-1}{F}_{k-1/k-2}{F}_{k-2/k-3}\end{array}\right.,$

其中F_k/k-1为k-1时刻到k时刻的系统状态一步转移阵，F_k-1/k-2为k-2时刻到k-1时刻的系统一步状态转移阵，F_k-2/k-3为k-3时刻到k-2时刻的系统一步状态转移阵。C₁、C₂、C₃为F_k/Fk-1、F_k/k-2、F_k/k-3状态转移阵的加权系数。

Images