适于高维GNSS/INS深耦合的容积卡尔曼滤波方法与流程

本发明涉及滤波技术，尤其涉及一种适于高维GNSS/INS深耦合的容积卡尔曼滤波方法。
背景技术：
：非线性滤波是组合导航和目标跟踪领域中常用信息融合技术。不同于目标跟踪模型中显著的非线性，组合导航中的非线性误差一般较弱，然而受滤波器结构、传感器采样率、量测质量以及滤波更新率等因素影响其非线性表现出较强的时变特性。特别是当系统初始误差较大、量测噪声较大以及系统载体大机动的情况下，基于卡尔曼滤波(KF)的非线性状态估计常常难以稳定工作。扩展卡尔曼滤波器(EKF)在工程领域应用较为广泛，其基于雅克比矩阵解决系统的非线性问题，状态估计精度可达到泰勒级数展开的一阶水平，在载体静止或者低动态情况下获得了良好的效果，但是对强非线性系统估计精度较差。以无迹卡尔曼滤波(UKF)为代表的采样点逼近滤波方法一经提出就获得迅速应用，其以多点采样后雅克比矩阵的期望值代替EKF中单点的雅克比矩阵，对非线性引起的不确定性具有更好的平滑效果。容积卡尔曼滤波(CKF)较UKF在高维系统状态估计中具有更好的精度和稳定性，虽然其均值精度在非线性的高斯噪声模型可以达到泰勒级数三阶，其方差的估计精度只在泰勒级数一阶水平。UKF采用改进的UT采样方法处理高维滤波中采样点的non-localsampling问题，提高了方差估计的精度，由于CKF采用对称采样策略其无法采用中心点权值捕获方差的高阶信息。状态误差方差估计的精度问题和一致性问题在高维的组合导航系统中更为突出。比如工程上GNSS/INS松组合一般采用15维，紧组合一般采用17或者更高，而深耦合中由于需要确保INS输出量对GNSS跟踪环路的辅助效果，需要更多的惯性器件状态误差，同时为了消除辅助信息与量测值的相关性误差，需要增加码跟踪环路和载波跟踪换路的状态误差量到组合滤波器的状态量中。以常规的4颗可见卫星为例其增加的跟踪环路的状态量维数为12维，因此现有的非线性滤波方法难以适用高维GNSS/INS深耦合状态估计问题。技术实现要素：发明目的：本发明针对现有技术存在的问题，提供一种适于高维GNSS/INS深耦合的容积卡尔曼滤波方法。技术方案：本发明所述的适于高维GNSS/INS深耦合的容积卡尔曼滤波方法，包括：S1、构造高维GNSS/INS深耦合滤波模型；S2、对构造的滤波模型采用标准容积规则产生初始化容积点；S3、采用新型容积点更新规则进行CKF滤波。进一步的，所述步骤S1包括：S11、分别设置INS和GNSS子系统的INS状态量为xI、GNSS状态量为xG，其中，xI＝[δPδVψkabakωbω]xG＝[bcdcδρdllδφpllδfpll]式中，[δPδVψkabakωbω]分别依次为3维位置误差、速度误差、姿态误差、加速度计系数误差、加速度计系数零偏、陀螺仪系数误差与常值漂移；[bcdcδρdllδφpllδfpll]分别依次为接收机时钟偏置、时钟漂移、码跟踪环路的鉴相伪距误差、载波跟踪环路的相位误差和载波跟踪环路的频率误差；S12、根据状态量xI、xG建立GNSS/INS深耦合的系统模型为：x·Ix·G=FI(t)O21×2O21×4O21×8O2×21FG(t)O2×4O2×8FID(t)O4×2FD(t)O4×8FIP(t)O8×2O8×4Fp(t)xIxG+GWIWG]]>式中，FN(t)为9维惯导基本系统矩阵和Ο3×9，FM(t)＝[Ο12×12]，Οi×j为i行j列的零矩阵，Ca和Cω分别为加速度计和陀螺的系数误差矩阵，为t时刻的姿态矩阵，T1、T2为环路滤波器的参数，分别为xI和xG的微分项，G为系统噪声驱动矩阵，WI和WG为INS和GNSS以及跟踪环路的系统噪声，Tr为相关时间，Kpll为载波环增益，Ii×j为i行j列的单位阵，为卫星与接收机的径向位置矢量矩阵，W为INS解算位置误差引起的卫星与接收机径向速度误差，为导航坐标系到地心地固坐标系的转换矩阵，Kdll为码环增益。S13、根据GNSS/INS深耦合的系统模型建立GNSS/INS深耦合的量测模型为：δρ＝ρI-ρG＝δρI-(bc+νρ+δρdll)δρ·=ρ·I-ρ·G=δρ·I-(dc+vρ·+δρ·pll)]]>式中，δρ、分别为卫星通道的伪距、伪距率观测量，ρI、分别为INS预测的伪距、伪距率，ρG、分别为GNSS量测伪距、伪距率，bc、dc分别为接收机时钟偏置和漂移，νρ、分别为伪距、伪距率观测噪声，δρdll、分别为跟踪环路的伪距误差和伪距率误差，ν为量测噪声矩阵，即有fL1为GPSL1载波频率，c为光速。进一步的，所述步骤S2包括：S21、k-1时刻采用高精度非线性滤波方法计算得到k-1时刻的状态后验分布其中，表示k-1时刻状态后验分布，表示由k-1时刻的量测推测出的k-1时刻的状态分布，Pk‐1|k‐1表示的协方差，x～N(μ,σ2)表示x服从均值为μ方差为σ2的高斯分布；S22、采用标准容积规则产生系统状态方程f(x)预测过程所需要的容积点；其中，式中，x为GNSS/INS深耦合滤波模型的状态量，其维数为nx，为nx维单位方阵，表示由k-1时刻的量测推测出的k-1时刻的第i个容积点，ξi为CKF滤波中第i个标准容积点，i＝1,…,2nx，Sk-1|k-1满足Pk‐1|k‐1＝Sk-1|k-1(Sk-1|k-1)T。4、根据权利要求3所述的高维GNSS/INS深耦合的容积卡尔曼滤波方法，其特征在于：所述高精度非线性滤波方法包括：设置度非线性滤波迭代的量测方程为其中为k时刻第j次量测迭代的状态估计值，zk为k时刻的多卫星通道器鉴相器输出的量测值，h(·)为深耦合跟踪环路的量测方程，为当前迭代中后验估计值与先验估计值的误差，Kj为第j次迭代的卡尔曼增益，αj为加速收敛设置的迭代步长控制系数，满足0＜αj≤1，为Pk|k-1的逆，Pxz,k|k-1为预测过程中产生的互协方差，状态后验估计误差的协方差矩阵取最后一次迭代计算的结果。进一步的，所述步骤S3包括：S31、采用下列公式计算得到k时刻状态先验分布：xk-~N(x^k|k-1,Pk|k-1)x^k|k-1=Σi=12nwif(Xi,k-1|k-1+)Pk|k-1=Σi=12nxwi(f(Xi,k-1|k-1+)-x^k|k-1)(f(Xi,k-1|k-1+)-x^k|k-1)T+Qk-1]]>式中，表示k时刻状态先验估计，为其均值，Pk|k-1为其方差，表示由k-1时刻的量测和状态推测出的k时刻的状态估计，Pk|k-1表示的协方差，wi＝1/2nx，Qk-1为系统噪声方差阵；S32、采用下式计算得到预测过程的容积点误差矩阵并定义为先验PDF逼近过程Sigma点统计线性回归(SLR)的误差方差；X~i,k|k-1-=Xi,k|k-1-x^k|k-1,0≤i≤2nx]]>其中，为经系统方程传播后的容积点；S33、将经系统方程传播后的容积点作为量测更新过程的容积点；S34、采用CKF量测更新计算量测值的似然分布函数；其中，式中，其中表示k时刻量测似然估计，为其均值，Pzz,k|k-1为其方差，为由k-1时刻的状态预测到的k时刻的量测，h(x)为量测方程，wi＝1/2nx，Rk为量测噪声方差阵；S35、计算状态量x的后验分布函数；其中，式中，为k时刻的状态量的后验估计，其均值和方差分别为和Pk|k，Kk＝Pxz,k|k-1(Pzz,k|k-1)‐1为卡尔曼增益矩阵，Pxz,k|k-1为状态量的后验估计值与量测似然估计值的互协方差；S36、定义Sigma点逼近后验分布产生的误差为为CKF容积点SLR的权值，k时刻的先验分布的SLR应精确捕获状态的均值和协方差，且考虑系统不确定性和噪声的影响，则有X~i,k|k-1-w=0]]>X~i,k|k-1-diag(w)X~i,k|k-1-T=Pk|k-1-Qk]]>式中，为预测过程容积点误差阵，Σ＝diag(w)表示采用w对角线元素构造该矩阵Σ，类似的后验分布的SLR中，容积点至少能精确匹配到状态的均值和方差即X~i,k|k+w=0]]>X~i,k|k+diag(w)(X~i,k|k+)T=Pk|k]]>式中，为更新后的容积点；S37、设和均为对称正定矩阵，且存在则有其中B为待求解的转换矩阵，为更新后的容积点误差阵；进而有B＝Lk+1Γ(Lk)-1，其中Γ为任意正交矩阵满足取Γ为单位阵时则得到B＝Lk+1(Lk)-1；S38、基于后验的状态估计均值和更新后的容积点误差阵得到更新的容积点为进一步的，所述的GNSS码跟踪鉴相伪距误差ρdll方程为δρ·dll=-Kdllδρdll+δVaid+KdllQ]]>其中δρdll为码环环路滤波器的输出量，为ρdll的微分，Kdll为码环增益，δVaid为滤波输出校正后INS提供给码跟踪环路的速度辅助，Q为环路热噪声和干扰引起的系统噪声。进一步的，所述的载波环路误差方程为δφ·pll=2π(δfpll+δfaid)]]>δf·pll=[2πT2T1(δfpll+δfaid)+δφpllT1]·Kpll]]>其中，φpll为载波环路相位误差，fpll为载波环路频率误差，Kpll为环路增益，δfaid为滤波输出校正后INS提供给载波跟踪环路的多普勒频率误差辅助，T1、T2为环路滤波器的参数，即环路低通滤波器的传递函数为有益效果：本发明与现有技术相比，其显著优点是：(1)采用更高精度的非线性滤波方法初始化生成容积点；(2)基于矩阵变换更新容积点改善了状态估计过程中预测过程方差的一致性，提高滤波器的稳定性；(3)将跟踪环路的状态误差参数作为组合滤波器的状态参数消除滤波器量测与跟踪环路误差的相关性，提高INS辅助量校正的精度和稳定性；(4)采用两种策略生成CKF的容积点，即初始化时采用耗时的、高精度的方法生成容积点，后续的容积点更新过程基于前一时刻状态估计误差的方差实现，提高了容积点的利用率。附图说明图1是本发明的一个实施例的系统框图；图2是高维GNSS/INS深耦合滤波模型的示意图。具体实施方式如图1所示，本实施例的适于高维GNSS/INS深耦合的容积卡尔曼滤波方法包括以下步骤：S1、构造高维GNSS/INS深耦合滤波模型，如图2所示。步骤S1具体包括：S11、分别设置INS和GNSS子系统的INS状态量为xI、GNSS状态量为xG，其中，xI＝[δPδVψkabakωbω]xG＝[bcdcδρdllδφpllδfpll]式中，[δPδVψkabakωbω]分别依次为3维位置误差、速度误差、姿态误差、加速度计系数误差、加速度计系数零偏、陀螺仪系数误差与常值漂移；[bcdcδρdllδφpllδfpll]分别依次为接收机时钟偏置、时钟漂移、码跟踪环路的鉴相伪距误差、载波跟踪环路的相位误差和载波跟踪环路的频率误差。其中，所述的GNSS码跟踪鉴相伪距误差ρdll方程为δρ·dll=-Kdllδρdll+δVaid+KdllQ]]>其中δρdll为码环环路滤波器的输出量，为ρdll的微分，Kdll为码环增益，δVaid为滤波输出校正后INS提供给码跟踪环路的速度辅助，Q为环路热噪声和干扰引起的系统噪声。其中，所述的载波环路误差方程为δφ·pll=2π(δfpll+δfaid)]]>δf·pll=[2πT2T1(δfpll+δfaid)+δφpllT1]·Kpll]]>其中，φpll为载波环路相位误差，fpll为载波环路频率误差，Kpll为环路增益，δfaid为滤波输出校正后INS提供给载波跟踪环路的多普勒频率误差辅助，T1、T2为环路滤波器的参数，即环路低通滤波器的传递函数为S12、根据状态量xI、xG建立GNSS/INS深耦合的系统模型为：x·Ix·G=FI(t)O21×2O21×4O21×8O2×21FG(t)O2×4O2×8FID(t)O4×2FD(t)O4×8FIP(t)O8×2O8×4Fp(t)xIxG+GWIWG]]>式中，FN(t)为9维惯导基本系统矩阵和Ο3×9，FM(t)＝[Ο12×12]，Οi×j为i行j列的零矩阵，Ca和Cω分别为加速度计和陀螺的系数误差矩阵，为t时刻的姿态矩阵，T1、T2为环路滤波器的参数，分别为xI和xG的微分项，G为系统噪声驱动矩阵，WI和WG为INS和GNSS以及跟踪环路的系统噪声，Tr为相关时间，Kpll为载波环增益，Ii×j为i行j列的单位阵，为卫星与接收机的径向位置矢量矩阵，W为INS解算位置误差引起的卫星与接收机径向速度误差，为导航坐标系到地心地固坐标系的转换矩阵，Kdll为码环增益。S13、根据GNSS/INS深耦合的系统模型建立GNSS/INS深耦合的量测模型为：δρ＝ρI-ρG＝δρI-(bc+νρ+δρdll)δρ·=ρ·I-ρ·G=δρ·I-(dc+vρ·+δρ·pll)]]>式中，δρ、分别为卫星通道的伪距、伪距率观测量，ρI、分别为INS预测的伪距、伪距率，ρG、分别为GNSS量测伪距、伪距率，bc、dc分别为接收机时钟偏置和漂移，νρ、分别为伪距、伪距率观测噪声，δρdll、分别为跟踪环路的伪距误差和伪距率误差，ν为量测噪声矩阵，即有fL1为GPSL1载波频率，c为光速。S2、对构造的滤波模型采用标准容积规则产生初始化容积点。步骤S2具体包括：S21、k-1时刻采用高精度非线性滤波方法计算得到k-1时刻的状态后验分布其中，表示k-1时刻状态后验分布，表示由k-1时刻的量测推测出的k-1时刻的状态分布，Pk‐1|k‐1表示的协方差，x～N(μ,σ2)表示x服从均值为μ方差为σ2的高斯分布。其中，所述高精度非线性滤波方法包括：设置度非线性滤波迭代的量测方程为其中为k时刻第j次量测迭代的状态估计值，zk为k时刻的多卫星通道器鉴相器输出的量测值，h(·)为深耦合跟踪环路的量测方程，为当前迭代中后验估计值与先验估计值的误差，Kj为第j次迭代的卡尔曼增益，αj为加速收敛设置的迭代步长控制系数，满足0＜αj≤1，为Pk|k-1的逆，Pxz,k|k-1为预测过程中产生的互协方差，状态后验估计误差的协方差矩阵取最后一次迭代计算的结果。S22、采用标准容积规则产生系统状态方程f(x)预测过程所需要的容积点；其中，式中，x为GNSS/INS深耦合滤波模型的状态量，其维数为nx，为nx维单位方阵，表示由k-1时刻的量测推测出的k-1时刻的第i个容积点，ξi为CKF滤波中第i个标准容积点，i＝1,…,2nx，Sk-1|k-1满足Pk‐1|k‐1＝Sk-1|k-1(Sk-1|k-1)T。S3、采用新型容积点更新规则进行CKF滤波。步骤S3包括：S31、采用下列公式计算得到k时刻状态先验分布：xk-~N(x^k|k-1,Pk|k-1)x^k|k-1=Σi=12nwif(Xi,k-1|k-1+)Pk|k-1=Σi=12nxwi(f(Xi,k-1|k-1+)-x^k|k-1)(f(Xi,k-1|k-1+)-x^k|k-1)T+Qk-1]]>式中，表示k时刻状态先验估计，为其均值，Pk|k-1为其方差，表示由k-1时刻的量测和状态推测出的k时刻的状态估计，Pk|k-1表示的协方差，wi＝1/2nx，Qk-1为系统噪声方差阵；S32、采用下式计算得到预测过程的容积点误差矩阵并定义为先验PDF逼近过程Sigma点统计线性回归的误差方差；X~i,k|k-1-=Xi,k|k-1-x^k|k-1,0≤i≤2nx]]>其中，为经系统方程传播后的容积点；S33、将经系统方程传播后的容积点作为量测更新过程的容积点；S34、采用CKF量测更新计算量测值的似然分布函数；其中，式中，其中表示k时刻量测似然估计，为其均值，Pzz,k|k-1为其方差，为由k-1时刻的状态预测到的k时刻的量测，h(x)为量测方程，wi＝1/2nx，Rk为量测噪声方差阵；S35、计算状态量x的后验分布函数；其中，式中，为k时刻的状态量的后验估计，其均值和方差分别为和Pk|k，Kk＝Pxz,k|k-1(Pzz,k|k-1)‐1为卡尔曼增益矩阵，Pxz,k|k-1为状态量的后验估计值与量测似然估计值的互协方差；S36、定义Sigma点逼近后验分布产生的误差为为CKF容积点SLR的权值，k时刻的先验分布的SLR应精确捕获状态的均值和协方差，且考虑系统不确定性和噪声的影响，则有X~i,k|k-1-w=0]]>X~i,k|k-1-diag(w)X~i,k|k-1-T=Pk|k-1-Qk]]>式中，为预测过程容积点误差阵，Σ＝diag(w)表示采用w对角线元素构造该矩阵Σ，类似的后验分布的SLR中，容积点至少能精确匹配到状态的均值和方差即X~i,k|k+w=0]]>X~i,k|k+diag(w)(X~i,k|k+)T=Pk|k]]>式中，为更新后的容积点；S37、设和均为对称正定矩阵，且存在则有其中B为待求解的转换矩阵，为更新后的容积点误差阵；进而有B＝Lk+1Γ(Lk)-1，其中Γ为任意正交矩阵满足取Γ为单位阵时则得到B＝Lk+1(Lk)-1；S38、基于后验的状态估计均值和更新后的容积点误差阵得到更新的容积点为以上所揭露的仅为本发明一种较佳实施例而已，不能以此来限定本发明之权利范围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。当前第1页1 2 3