[发明专利]一种降维度的基于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减得到校正后的角速度信息。