| 主权项 |
应用于多模卫星导航系统中的快速卡尔曼滤波定位方法，其特征在于：其操作步骤为：步骤1：针对全球定位系统GPS和北斗2号BD‑2卫星导航系统，建立全球定位系统/北斗2号GPS/BD‑2组合的多模卫星导航系统的数学模型，其中选取的坐标系为世界大地坐标系WGS‑84，坐标系的三轴分别用X<sub>T</sub>，Y<sub>T</sub>和Z<sub>T</sub>表示；具体过程为：步骤1.1：建立GPS/BD‑2组合的多模卫星导航系统的状态方程；用符号X表示GPS/BD‑2组合的多模卫星导航系统状态方程的状态变量，<img file="FDA0001002079760000011.GIF" wi="1022" he="71" />其中<img file="FDA0001002079760000012.GIF" wi="121" he="55" />分别表示载体WGS‑84坐标系下X<sub>T</sub>轴方向上的位置、速度和加速度；<img file="FDA0001002079760000013.GIF" wi="126" he="63" />分别表示载体在WGS‑84坐标系下Y<sub>T</sub>轴方向上的位置、速度和加速度；<img file="FDA0001002079760000014.GIF" wi="116" he="53" />分别表示载体在Z<sub>T</sub>方向上的位置、速度、和加速度；<img file="FDA0001002079760000015.GIF" wi="230" he="71" />分别表示GPS接收机钟差和钟差漂移频率；<img file="FDA0001002079760000016.GIF" wi="205" he="70" />分别表示BD‑2接收机钟差和钟差漂移频率；建立离散状态方程如公式(1)所示；X<sub>k+1</sub>＝ΦX<sub>k</sub>+U<sub>k</sub>+w<sub>k</sub> (1)其中，X<sub>k</sub>为k时刻GPS/BD‑2组合卫星导航系统的状态；Φ为状态转移矩阵，其具体形式如公式(2)所示；U<sub>k</sub>为GPS/BD‑2组合卫星导航系统在k时刻的输入量，如公式(3)所示，U<sub>k</sub>在每一采样周期为定值；w<sub>k</sub>为系统在k时刻的过程噪声，其协方差矩阵用Q<sub>k</sub>表示，计算公式如式(4)所示；Φ＝diag(Φ<sub>x</sub>,Φ<sub>y</sub>,Φ<sub>z</sub>,Φ<sub>t,GPS</sub>,Φ<sub>t,BD</sub>) (2)其中，<img file="FDA0001002079760000017.GIF" wi="1822" he="403" />Φ<sub>x</sub>为在WGS‑84坐标系中描述载体在X<sub>T</sub>轴上的位置、速度和加速度的状态转移矩阵，Φ<sub>y</sub>为在WGS‑84坐标系中描述载体在Y<sub>T</sub>轴上的位置、速度和加速度的状态转移矩阵，Φ<sub>z</sub>为在WGS‑84坐标系中描述载体在Z<sub>T</sub>轴上的位置、速度和加速度的状态转移矩阵；Φ<sub>t,GPS</sub>为描述GPS接收机钟差和钟差漂移频率的状态转移矩 阵，Φ<sub>t,BD</sub>为描述BD‑2接收机钟差和钟差漂移频率的状态转移矩阵；λ为一阶时间相关常数的倒数，λ∈(0,1)；λ<sub>t</sub>为一阶马尔科夫时间常数倒数，λ<sub>t</sub>∈(0,1)；λ和λ<sub>t</sub>的值由人为设定，T为系统采样周期；<img file="FDA0001002079760000021.GIF" wi="1893" he="71" />其中，<img file="FDA0001002079760000022.GIF" wi="830" he="343" /><img file="FDA0001002079760000023.GIF" wi="70" he="59" />为载体在X<sub>T</sub>轴上的加速度均值，<img file="FDA0001002079760000024.GIF" wi="293" he="77" /><img file="FDA0001002079760000025.GIF" wi="75" he="63" />为载体在Y<sub>T</sub>轴上的加速度均值，<img file="FDA0001002079760000026.GIF" wi="294" he="71" /><img file="FDA0001002079760000027.GIF" wi="71" he="61" />为载体在Z<sub>T</sub>轴上的加速度均值，<img file="FDA0001002079760000028.GIF" wi="294" he="71" /><img file="FDA0001002079760000029.GIF" wi="213" he="62" />和<img file="FDA00010020797600000210.GIF" wi="67" he="62" />在每一采样周期内为常数；0<sub>1×2</sub>表示1行2列的零值矩阵；<img file="FDA00010020797600000211.GIF" wi="1890" he="71" />其中E(·)表示求期望；Q<sub>x,k</sub>为WGS‑84坐标系下k时刻载体在X<sub>T</sub>轴上运动状态的过程噪声协方差矩阵；Q<sub>y,k</sub>为WGS‑84坐标系下k时刻载体在Y<sub>T</sub>轴上运动状态的过程噪声协方差矩阵；Q<sub>z,k</sub>为WGS‑84坐标系下k时刻载体在Z<sub>T</sub>轴上运动状态的过程噪声协方差矩阵；Q<sub>t,gps,k</sub>为k时刻GPS接收机钟差和钟差漂移频率的过程噪声协方差矩阵，Q<sub>t,bd,k</sub>为k时刻BD‑2接收机钟差和钟差漂移频率的过程噪声协方差矩阵；步骤1.2，建立GPS/BD‑2组合卫星导航系统的观测方程；用符号M表示在k时刻接收机观测到的GPS卫星的个数；用符号N表示在k时刻接收机观测到的BD‑2卫星的个数，则建立k时刻的伪距观测方程如公式(5)和公式(6)所示；<img file="FDA00010020797600000212.GIF" wi="1893" he="94" /><img file="FDA00010020797600000213.GIF" wi="1893" he="86" />其中，<img file="FDA00010020797600000214.GIF" wi="85" he="75" />为k时刻观测到的第m颗GPS卫星的伪距，m＝1,2...M；<img file="FDA00010020797600000215.GIF" wi="78" he="70" />为k时刻观测到的第n颗BD‑2卫星的伪距，n＝1,2...N；<img file="FDA00010020797600000216.GIF" wi="307" he="78" />表示k时刻观测到的第m颗GPS卫星在WGS‑84坐标系下X<sub>T</sub>，Y<sub>T</sub>和Z<sub>T</sub>轴上的位置坐标；<img file="FDA0001002079760000031.GIF" wi="278" he="79" />表示k时刻观测到的第n颗BD‑2卫星在WGS‑84坐标系下X<sub>T</sub>，Y<sub>T</sub>和Z<sub>T</sub>轴上的位置坐标；[x<sub>k</sub>,y<sub>k</sub>,z<sub>k</sub>]为k时刻待求接收机在WGS‑84坐标系下X<sub>T</sub>，Y<sub>T</sub>和Z<sub>T</sub>轴上的位置坐标；<img file="FDA0001002079760000032.GIF" wi="109" he="71" />为k时刻GPS接收机钟差，<img file="FDA0001002079760000033.GIF" wi="94" he="71" />为k时刻BD‑2接收机钟差；<img file="FDA0001002079760000034.GIF" wi="78" he="71" />为k时刻第m颗GPS卫星的观测噪声，<img file="FDA0001002079760000035.GIF" wi="66" he="70" />为k时刻第n颗BD‑2卫星的观测噪声；为下文描述算法方便，将公式(5)和公式(6)所示的观测方程简写为公式(7)；y<sub>k</sub>＝h(X<sub>k</sub>)+ε<sub>k</sub> (7)其中y<sub>k</sub>为系统的观测量，<img file="FDA0001002079760000036.GIF" wi="789" he="71" />h(·)为观测方程中的非线性函数；ε<sub>k</sub>为系统的观测误差，<img file="FDA0001002079760000037.GIF" wi="766" he="78" />ε<sub>k</sub>的协方差矩阵用符号R<sub>k</sub>表示，<img file="FDA0001002079760000038.GIF" wi="294" he="71" />公式(1)和公式(7)构成了GPS/BD‑2组合卫星导航系统的系统模型，写成公式(8)的形式：<img file="FDA0001002079760000039.GIF" wi="1889" he="143" />步骤2：将GPS/BD‑2组合卫星导航的系统方程转换为GPS/BD‑2组合卫星导航系统的误差方程；具体为：用符号<img file="FDA00010020797600000310.GIF" wi="87" he="70" />表示系统在k‑1时刻的滤波值，多模卫星导航系统在k时刻的误差方程为：<img file="FDA00010020797600000311.GIF" wi="1893" he="158" />其中，ΔX<sub>k</sub>为GPS/BD‑2组合卫星导航系统在k时刻的状态偏差，<img file="FDA00010020797600000312.GIF" wi="524" he="72" />Δy<sub>k</sub>为GPS/BD‑2组合卫星导航系统在k时刻的测量偏差<img file="FDA00010020797600000313.GIF" wi="373" he="71" /><img file="FDA00010020797600000314.GIF" wi="174" he="70" />表示系统在k‑1时刻的雅克比矩阵，即<img file="FDA00010020797600000315.GIF" wi="450" he="119" /><img file="FDA00010020797600000316.GIF" wi="78" he="62" />表示求函数的偏导；步骤3：将GPS/BD‑2组合卫星导航系统的误差方程(8)通过张量积模型转换的方法转换为多胞型线性微分包含系统模型；具体操作为：步骤3.1：确定GPS/BD‑2组合卫星导航系统状态X中X<sub>T</sub> Y<sub>T</sub> Z<sub>T</sub>三坐标轴上位 置变量(x,y,z)的值域集合；值域集合用符号Ω<sub>p</sub>表示，即Ω<sub>p</sub>＝[c<sub>x</sub>×d<sub>x</sub>]×[c<sub>y</sub>×d<sub>y</sub>]×[c<sub>z</sub>×d<sub>z</sub>]，其中[c<sub>x</sub>,d<sub>x</sub>]为变量x的取值范围，[c<sub>y</sub>,d<sub>y</sub>]为变量y的取值范围，[c<sub>z</sub>,d<sub>z</sub>]为变量z的取值范围；Ω<sub>p</sub>包含三个维度，分别用符号p<sub>x</sub> p<sub>y</sub> p<sub>z</sub>表示；步骤3.2：将值域集合Ω<sub>p</sub>的每一个维度p<sub>n</sub>划分为L个均匀分布的网格，n＝x,y,z，L∈(0,100]，每个网格点用符号p<sub>n,l</sub>表示，l＝1,2...L，c<sub>n</sub>＜p<sub>n,1</sub>＜p<sub>n,2</sub>＜...＜p<sub>n,L</sub>＜d<sub>n</sub>；步骤3.3：计算每个网格点的雅克比矩阵H(p<sub>nl</sub>),n＝x,y,z,l＝1,2...L，并将得到的矩阵存入张量空间<img file="FDA0001002079760000041.GIF" wi="43" he="39" />中；步骤3.4：对张量空间<img file="FDA0001002079760000042.GIF" wi="43" he="39" />内的每模矩阵执行高阶奇异值分解，在每模矩阵奇异值分解时舍弃零和部分非零每模矩阵奇异值，并将每模矩阵奇异值分解得到的酉矩阵U<sub>j</sub>进行标准化转换，j＝1,2,3，转换后的矩阵用符号<img file="FDA0001002079760000043.GIF" wi="58" he="69" />表示；经过3次高阶奇异值分解，将张量空间<img file="FDA0001002079760000044.GIF" wi="44" he="47" />转换为近似张量空间<img file="FDA0001002079760000045.GIF" wi="75" he="63" /><img file="FDA0001002079760000046.GIF" wi="277" he="107" />其中<img file="FDA0001002079760000047.GIF" wi="43" he="63" />为<img file="FDA0001002079760000048.GIF" wi="43" he="47" />的近似张量空间，<img file="FDA0001002079760000049.GIF" wi="50" he="54" />为经过高阶奇异值分解后产生的子张量空间；步骤3.5：从子张量空间<img file="FDA00010020797600000410.GIF" wi="51" he="53" />中提取多胞型线性微分包含系统模型的顶点矩阵H<sub>i</sub>，i＝1,2...r，H<sub>i</sub>为常值，r为多胞型线性微分包含系统模型总的顶点个数；步骤4：利用步骤3.5得到的顶点矩阵H<sub>i</sub>将GPS/BD‑2组合卫星导航系统的误差方程转换为GPS/BD‑2组合卫星导航多胞型线性微分包含系统模型，即转换为r个顶点系统的凸组合形式，如公式(10)所示；<img file="FDA00010020797600000411.GIF" wi="1894" he="538" />步骤5：设置GPS/BD‑2组合卫星导航多胞型线性微分包含系统的在0时刻的滤波初值，其中在多胞型线性微分包含系统中状态在0时刻的初值用符号<img file="FDA00010020797600000413.GIF" wi="59" he="70" />表示，估计误差协方差矩阵的初值用符号P<sub>0</sub>表示，同时设置系统定位结束时间time；步骤6：状态一步预测；根据步骤5或步骤11得到的k‑1时刻滤波值<img file="FDA00010020797600000412.GIF" wi="90" he="71" />用 公式(11)进行状态一步预测得到系统在k时刻的一步预测值<img file="FDA0001002079760000051.GIF" wi="146" he="77" />其中当k＝1时，<img file="FDA0001002079760000052.GIF" wi="89" he="71" />通过步骤5获得，当k≥2时，<img file="FDA0001002079760000053.GIF" wi="94" he="71" />通过步骤11获得；<img file="FDA0001002079760000054.GIF" wi="1894" he="78" />步骤7：对GPS/BD‑2组合卫星导航多胞型线性微分包含系统模型的每个顶点系统进行卡尔曼滤波，获得每个顶点系统的误差估计值<img file="FDA0001002079760000055.GIF" wi="91" he="63" />和每个顶点的估计误差协方差矩阵<img file="FDA0001002079760000056.GIF" wi="75" he="63" />其中i＝1,2...r；具体操作步骤为：步骤7.1：估计误差协方差矩阵一步预测；根据步骤5或步骤10得到的k‑1时刻的估计误差协方差矩阵P<sub>k‑1</sub>通过公式(12)进行一步预测，获得系统在k时刻估计误差协方差矩阵的一步预测值P<sub>k,k‑1</sub>；其中当k＝1时，P<sub>k‑1</sub>通过步骤5获得，当k≥2时，P<sub>k‑1</sub>通过步骤10获得；P<sub>k,k‑1</sub>＝ΦP<sub>k‑1</sub>Φ<sup>T</sup>+Q<sub>k‑1</sub> (12)步骤7.2：通过公式(13)计算每个顶点系统在k时刻的滤波增益<img file="FDA0001002079760000057.GIF" wi="89" he="63" />其中i＝1,2...r；<img file="FDA0001002079760000058.GIF" wi="1894" he="79" />步骤7.3：通过公式(14)计算系统在k时刻的测量偏差Δy<sub>k</sub>；<img file="FDA0001002079760000059.GIF" wi="1888" he="79" />步骤7.4：通过公式(15)计算每个顶点在k时刻估计误差的校正量<img file="FDA00010020797600000510.GIF" wi="115" he="62" /><img file="FDA00010020797600000511.GIF" wi="1894" he="70" />步骤7.5：通过公式(16)校正每个顶点在k时刻的估计误差协方差矩阵<img file="FDA00010020797600000512.GIF" wi="75" he="67" /><img file="FDA00010020797600000513.GIF" wi="1894" he="86" />步骤8：通过公式(17)计算顶点系统的融合系数α<sub>i</sub>；<img file="FDA00010020797600000514.GIF" wi="1888" he="199" />其中<img file="FDA00010020797600000515.GIF" wi="80" he="71" />为张量空间<img file="FDA00010020797600000516.GIF" wi="40" he="39" />的3模矩阵的高阶奇异值分解的第i个奇异值，i＝1,2...r；步骤9：在公式(17)的基础上通过公式(18)将每个顶点的估计误差进行融合，得到GPS/BD‑2组合卫星导航多胞型线性微分包含系统模型误差的最优估计值<img file="FDA00010020797600000517.GIF" wi="115" he="70" /><img file="dest_path_image002.GIF" wi="195" he="70" />步骤10：在公式(17)的基础上通过公式(19)对每个顶点系统的估计误差协方差矩阵融合，获得GPS/BD‑2组合卫星导航系统在k时刻的估计误差协方差矩阵P<sub>k</sub>；<img file="FDA0001002079760000062.GIF" wi="1854" he="127" />其中，<img file="FDA0001002079760000063.GIF" wi="68" he="63" />为P<sub>k</sub>的逆矩阵；步骤11：在公式(18)的基础上通过公式(20)求取GPS/BD‑2组合卫星导航系统在k时刻状态的最优估计值<img file="FDA0001002079760000064.GIF" wi="91" he="70" /><img file="FDA0001002079760000065.GIF" wi="1850" he="77" />步骤12：判断k是否到达设定的系统结束时间time，若未到达设定时间，则返回步骤6，重复步骤6到步骤11的操作；若到达设定时间，则结束操作；经过上述步骤的操作，即可实现对运动载体的实时定位。 |
