[发明专利]一种降维度的基于Carlson滤波算法的快速组合导航方法

摘要

本发明提出了一种降维度的基于Carlson滤波算法的快速组合导航方法，该方法主要包括数据融合、位置补偿两个部分。数据融合部分采用了降维度的基于Carlson滤波算法的数据融合算法，通过数据融合算法实现对INS模块速度与姿态信息以及INS器件的误差信息的建模。位置补偿部分利用卫星接收机观测到的伪距信息解算出位置信息对INS模块解算出的位置信息进行校正。本发明通过对状态方程与量测方程进行修改，在满足对INS导航模块的准确校正的基础上，使得系统的维数由15维降低到12维。降低了系统的运算的数据量，同时采用Carlson滤波器作为数据融合滤波器，在Carlson滤波器中对均方误差阵及均方误差阵的估计进行上三角分解来保证矩阵的正定性，可以有效地避免滤波器的发散。

主权项

1.一种降维度的基于Carlson滤波算法的快速组合导航方法，其特征在于：包括以下步骤：步骤1：利用由GPS接收机得到的卫星位置信息与伪距信息解算出用户的位置(x y z)和用户的速度(v_x v_y v_z)；步骤2：利用GPS接收机得到的星历信息解算出所需卫星的位置信息和速度信息，其中第i号卫星的位置信息为(xⁱ yⁱ zⁱ)，速度信息为x.iy.iz.i;]]>由公式δρ.ij=δρ.i-δρ.j]]>=(e1i-e1j)δvx+(e2i-e2j)δvy+(e3i-e3j)δvz+ϵρ.ij]]>...]]>δρ.mj=δρ.m-δρ.j]]>=(e1m-e1j)δvx+(e2m-e2j)δvy+(e3m-e3j)δvz+ϵρ.mj]]>得到数据融合系统的观测量集合Z=δρ.ij···δρ.mj,]]>其中δv_x、δv_y、δy_z为速度误差信息，i、j、m为相应的卫星序号，为GPS接收机到的第i号卫星的方向矢量在大地坐标系上的投影；为GPS接收机相对于第i号卫星的伪距率残差信息：为第i号卫星相对GPS接收机的伪距率预测值，为第i号卫星相对于GPS接收机的伪距率量测值，为第i号卫星与第j号卫星伪距率的量测误差；步骤3：建立数据融合系统的量测方程Z＝HX+V，X为数据融合系统的状态向量，H为量测矩阵，V为量测噪声，其中X＝[δv_x δv_y δv_z δε_x δε_y δε_z d_x d_y d_z b_x b_y b_z]，δε_x、δε_y、δε_z为平台失准角信息，d_x、d_y、d_z为陀螺仪零偏信息，b_x、b_y、b_z为加速度计零偏信息；由量测方程Z＝HX+V得到数据融合系统的量测矩阵：H=e1i-e1j,e2i-e2j,e3i-e3j,01×3,01×3,01×3...e1m-e1s,e2m-e2s,e3m-e3s,01×3,01×3,01×3]]>其中s也为卫星序号；步骤4：对数据融合系统的观测量Z进行Carlson滤波，具体滤波步骤为：步骤4.1：对Carlson滤波器进行初始化；步骤4.2：对数据融合系统的观测量集合Z进行Carlson滤波，其中第k次滤波过程包括Carlson滤波的时间更新过程和Carlson滤波的量测更新过程：步骤4.2.1：对于第k次Carlson滤波的时间更新过程，记迭代结果矩阵E为E=Uk-1TΦk,k-1TΓk-1Qk-11/2l×nn×n]]>其中l为数据融合系统驱动噪声的维数，n为数据融合系统的状态向量维数，U_k-1为第k-1次滤波的协方差矩阵的上三角阵分解阵，Φ_k，k-1为数据融合系统的状态矩阵的离散化形式，Q_k-1为状态噪声，Γ_k-1为状态噪声驱动矩阵，Φ_k，k-1＝I+F·t，其中t为采样时间；F为数据融合系统的状态矩阵，由数据融合系统的状态方程：X.i(t)=δv.3×1δϵ.3×1d.3×1b.3×1=F·X=-Fiδϵ3×1i+Cbib3×1Cbid3×1-Ad3×1-Ab3×1]]>得到，其中Fⁱ为比力矩阵，为由载体坐标系到大地坐标系的方向余弦矩阵，A为一阶马尔科夫过程系数矩阵；对公式：Uk/k-1(g,g)=E(g)TE(g)Vg=1Uk/k-1(g,g)E(g)Uk/k-1(b,g)=E(b)TVg,b=1,2,···,g-1E(b)=E(b-1)-Uk/k-1(b,g)Vg,b=1,2,···,g-1]]>按照g＝n,n-1,n-2,…,2,1顺序迭代，经过n次递推后，得到第k次滤波的协方差预测矩阵的上三角阵分解阵U_k/k-1，其中U_k/k-1(g,g)为U_k/k-1中第g行第g列的元素值，E^(g)为E的第g列；步骤4.2.2：对于第k次Carlson滤波的量测更新过程，采用序贯处理的方法：对于数据融合系统的观测量集合Z中的每个观测量采用如下步骤处理：对于第r个观测量的处理过程为：对公式进行迭代运算，q从1到n，其中d_q＝d_q-1+a(q)，为第r个观测量的噪声协防差，a=Uk/k-1T(Hkr)T=a(1)a(2)···a(n)T,]]>为数据融合系统的量测矩阵的第r行，e₀＝0，式中和分别表示U_k的第q列和U_k/k-1的第q列；得到第k次滤波协方差矩阵的上三角分解阵Uk=Uk(1)Uk(2)···Uk(n);]]>对第r个观测量序贯处理的状态估计为其中为第k-1次滤波的状态向量值，为第k次滤波得到的第r个观测量的值，X^k/k-1=Φk/k-1X^k-1;]]>最终得到数据融合系统的观测量集合Z中的最后一个观测量m的状态估计为步骤5：利用步骤4得到的对INS解算模块进行校正：利用INS求解出的速度信息减去由Carlson滤波器得到的速度误差即可得到校正后的速度信息；运用公式C^Eb=1ϵz-ϵy-ϵz1ϵxϵy-ϵx1·CEb]]>得到校正后的方向余弦矩阵为由INS模块得到的大地坐标系到载体坐标系的方向余弦矩阵，将由INS器件得到的加速度与解算出的加速度计零偏信息b_x、b_y、b_z相减得到校正后的加速度，将由INS器件得到的角速度与解算出的陀螺仪零偏信息d_x、d_y、d_z减得到校正后的角速度信息。