本发明涉及一种基于MRUPF的捷联惯导系统大方位失准角初始对准方法。该方法首先建立大方位失准角条件下捷联惯导系统静基座初始对准的状态空间模型，进行滤波初始化；然后利用UPF滤波算法进行初始对准的状态估计，在某一时刻下利用多分辨方法对粒子及粒子权值进行选择，降低粒子数目，将选择的粒子集及粒子权值作为初始粒子集和权值，继续利用UPF滤波方法进行初始对准的状态估计，得到失准角估计值。本发明通过多分辨方法减少UPF滤波的粒子数目，从而降低了计算量，在保证初始对准精度的同时提高了大方位失准角下捷联惯导系统初始对准的实时性。本发明适用于捷联惯导系统初始对准。
1.一种基于MRUPF的捷联惯导系统大方位失准角初始对准方法，其特征在于包括以下步骤：(1)建立捷联惯导系统大方位失准角静基座初始对准的状态空间模型捷联惯导系统的状态变量为 以
东向和北向速度误差为观测量，即Z＝[ΔVE ΔVN] ；地理坐标系为东北天地理坐标系；
ΔVE和ΔVN分别为东向和北向速度误差；ψE、ψN、ψU分别为东向、北向和天向失准角；
和 分别为东向和北向加速度计常值偏置；εE、εN和εU分别为三个方向上的陀螺仪的常值漂移；
根据捷联惯导系统静基座下的速度、姿态角误差方程，建立捷联惯导系统在大方位失准角下的状态空间模型，则误差方程和观测方程分别为：其中，f(X)为在大方位失准角下捷联惯导系统的非线性误差方程； 为系统观测矩阵，I2×2为2×2单位阵，O2×8为2×8的零矩阵；υ和η分别为系统噪声和观测噪声，且分别服从以下非高斯概率分布：υ～pυ，η～pη；
式中，Xk-1和Xk分别为第k-1时刻和第k时刻的状态变量；F(Xk-1)是第k-1时刻离散化的系统方程非线性项；υk-1为第k-1时刻的系统噪声；Zk为第k时刻的观测量；ηk为第k时刻的观测噪声；
在初始时刻即第k＝0时刻，对步骤(1)中的状态变量进行初始化，生成N个服从p(X0)分布的粒子 i＝1，...，N，粒子权值为 i＝1，…，N；其中p(X0)为初始状态X0服从的概率分布；
(3)设M为利用多分辨方法对粒子及粒子权值进行选择的时刻，时刻k＜M且有新的观测量时，根据步骤(1)中的状态空间模型，利用UPF滤波方法进行初始对准的状态估计，得到第k时刻的初始对准状态估计值 粒子集 和粒子权值 从而得到失准角估计值，其具体步骤如下：a.采样
首先利用UKF得到粒子滤波的重要性采样函数，即利用UKF滤波算法由第k-1时刻得到的 和 求第k时刻各粒子 的均值 和方差 得到重要性采样函数：式中， 表示以 为均值，以 为方差的正态分布；X0:k-1表示从初始时刻到第k-1时刻的状态变量；Z1:k表示从第1时刻到第k时刻的观测值；
然后从重要性密度函数 中抽取粒子 i＝1，…，N，即i＝1，…，N (12)b.计算粒子权值
i＝1，…，N (13)式中， 为第k-1时刻归一化后的权值；
i＝1，...，N (14)d.计算有效粒子的尺寸Neff
如果Neff小于门限值Nth，说明粒子多样性降低，需转入步骤(e)进行重采样，否则转入步骤(f)进行初始对准状态估计；
重采样的目的是消除权值较小的粒子，增加权值较大的粒子，使重采样后的样本集的分布符合后验密度p(Xk|Zk)，重采样后每个粒子的权值 被重新设置为1/N；
第k时刻初始对准状态估计值 和估计误差方差阵Pk分别为：得到 后，其第3～5维就是初始对准失准角估计值；
(4)时刻k＝M且有新的观测量时，根据步骤(1)中的状态空间模型，基于多分辨方法对步骤(3)所得到的第k-1时刻的粒子及粒子权值进行选择，再利用UPF进行初始对准的状态估计，得到第k时刻的初始对准状态估计值 粒子集 和粒子权值 从而得到失准角估计值，其具体步骤如下：a.多分辨小波滤波器组对权值进行分解
设L为小波分解的尺度数，将第k-1时刻粒子集 及粒子权值 分为N/2 个时间块，对每个时间块的权值进行多分辨分解，将其分解为低通部分和高通部分：式中， 表示第m个时间块的粒子权值； 为第k-1时刻尺度1的低通部分；
为第k-1时刻尺度j上的高通部分，且每个时间块中尺度j上的权值分布个数为
2j-1，j＝1，…，L；T为正交的小波分解变换矩阵；
式中，j＝1，…，L；i＝1，…，2 ；Ts为设定的权值阈值，Ts∈(10 ，10 )；
则经过阈值处理后，高通和低通部分的粒子权值写成矢量形式为：c.粒子选取
对阈值处理后的粒子权值 进行T逆变换得：式中， 表示变换后的第m个时间块的粒子权值，d)将粒子集 和权值 作为第k时刻初始的粒子集和权值，重复步骤(3)中的步骤a～f得到第k时刻的初始对准状态估计值 粒子集 和粒子权值 此过程中，粒子数目由N减少为
(5)时刻k＞M且有新的观测量时，根据步骤(1)中的状态空间模型，以前一时刻得到的粒子集 权值 作为当前时刻初始的粒子集和权值，利用UPF滤波方法，重复步骤(3)中的步骤a～f进行初始对准状态估计，得到失准角估计值，在此过程中，粒子数目是
一种基于MRUPF的捷联惯导系统大方位失准角初始对准方
法
技术领域
[0001] 本发明涉及一种基于MRUPF(Multiresolution Unscented Particle Filter，多分辨Unscented粒子滤波)的捷联惯导系统大方位失准角初始对准方法，适用于大方位失准角情况下误差模型是非线性的，非高斯噪声条件下静基座捷联惯导初始对准。
背景技术
[0002] 捷联惯性导航系统(Strap-down Inertial Navigation System，SINS)利用惯性器件及初始参数来确定运载体的位置、速度和姿态等全部运动参数，它工作时不依靠任何外界信息，也不向外辐射能量，是一种完全自主的导航系统。对于捷联惯导系统，由于姿态矩阵起到“数字平台”的作用，因此导航工作一开始就需要获得捷联姿态矩阵的初始值，以便完成导航的任务。显然如何获得捷联矩阵的初值，即初始对准问题，就成了首先必须解决的问题。对准精度和对准实时性是惯导系统进行初始对准时的两项重要技术指标。初始对准精度影响SINS的性能，初始对准实时性标志着系统的快速反应能力，所以初始对准一直被看作一项导航关键技术，从而成为近年来惯导领域的研究热点之一。
[0003] SINS的初始对准过程通常采用状态估计器来估计粗对准的误差，进而精确地确定SINS的初始姿态矩阵。常用的状态估计器就是卡尔曼滤波器(Kalman Filter，KF)，但KF滤波方法只能处理线性系统。当SINS的初始航向失准角较大时，会引起系统的非线性。扩展卡尔曼滤波(Extended Kalman Filter，EKF)虽然可以处理SINS初始对准的非线性问题，但是当系统非线性较强时，例如空中大失准角对准，EKF的线性化过程将带来很大误差。Unscented卡尔曼滤波(UKF)在处理任意非线性系统时，通过计算非线性系统后验期望与方差，能够达到非线性系统二阶泰勒级数的精度，但UKF对于具有较强非线性、非高斯系统的状态估计就达不到期望的效果。
[0004] KF、EKF以及UKF等滤波方法都要求系统的噪声为高斯白噪声，由于实际SINS的系统噪声为非高斯噪声，因此会导致SINS初始对准精度的下降，将适用于非高斯系统的粒子滤波(Particle filter，PF)应用于SINS初始对准，可以解决SINS系统非高斯噪声引起的精度下降问题。但是PF滤波方法存在粒子退化问题，针对这一问题，已有学者将UPF滤波方法(Unscented Particle Filter，Unscented粒子滤波)运用到SINS非线性初始对准中。但是，UPF的缺点是计算量很大，随着选取的粒子数目的增加，计算量会急剧增加，初始对准的实时性严重降低，惯导系统的性能就会受到严重影响。
发明内容
[0005] 本发明的技术解决问题是：针对大方位失准角，非高斯噪声条件下捷联惯导静基座初始对准利用现有滤波方法计算量大而造成初始对准时间长，实时性差的不足，采用MRUPF非线性滤波方法进行大方位失准角下捷联惯导初始对准，在保证初始对准精度的同时提高初始对准实时性。
[0006] 本发明的技术解决方案是：本发明提出了一种基于MRUPF的非线性滤波方法来解决捷联惯导大方位失准角初始对准问题。MRUPF滤波方法将多分辨小波滤波与UPF相结合，在利用UPF进行初始对准的状态估计时，在某一时刻下利用多分辨方法对粒子进行选择，降低粒子数目，将选择的粒子集及粒子权值作为初始粒子集和权值，然后继续利用UPF滤波方法进行初始对准的状态估计，得到失准角估计值。具体步骤如下：
[0007] (1)建立捷联惯导大方位失准角静基座初始对准的状态空间模型
[0008] 系统的状态变量为 以东向T
和北向速度误差为观测量，即Z＝[ΔVE ΔVN]。地理坐标系为东北天地理坐标系；ΔVe和ΔVN分别为东向和北向速度误差；ψE、ψN、ψN分别为东向、北向和天向失准角； 和 分别为东向和北向加速度计常值偏置；εE、εN和εU分别为三个方向上的陀螺仪的常值漂移。
[0009] 根据捷联惯导系统静基座下的速度、姿态角误差方程，建立捷联惯导系统大方位失准角下的误差方程和观测方程：
[0010]
[0011] 其中，f(X)为捷联惯导系统大方位失准角下的非线性误差方程； 为系统观测矩阵，I2×2为2×2单位阵，O2×8为2×8的零矩阵；υ和η分别为系统噪声和观测噪声，且服从以下非高斯分布：υ～pυ，η～Pη。
[0012] 将所述的状态空间模型离散化得：
[0013]
[0014] 式中，Xk-1和Xk分别为第k-1时刻和第k时刻的状态变量；F(Xk-1)是第k-1时刻离散化的系统方程非线性项；υk-1为第k-1时刻的系统噪声；Zk为第k时刻的观测量；ηk为第k时刻的观测噪声。
[0015] (2)滤波初始化
[0016] 在初始时刻即第k＝0时刻，对步骤(1)中的状态变量进行初始化，即对初始状态X0的分布p(X0)进行采样，生成N个服从p(X0)概率分布的粒子 其均值 和方差 分别满足：
[0017]
[0018] 式中，E[·]表示数学期望。
[0019] 令各粒子的权值为
[0020] (3)设M为利用多分辨方法对粒子及粒子权值进行选择的时刻，k＜M时刻且有新的观测量时，根据步骤(1)中的状态空间模型，利用UPF滤波方法进行初始对准的状态估计，得到第k时刻的初始对准状态估计值 粒子集 和粒子权值 从而得到失准角估计值。
[0021] (4)时刻k＝M且有新的观测量时，根据步骤(1)中的状态空间模型，基于多分辨方法对步骤(3)所得到的第k-1时刻的粒子及粒子权值进行选择，进而利用UPF进行初始对准的状态估计。该步骤先利用多分辨小波滤波对第k-1时刻得到的粒子权值 进行处理，再对小波变换后权值大小的判断来选择 个粒子 及粒子权值最后将选择后的粒子集 及粒子权值 作为第k时刻初始粒子和权值，利用UPF滤波得到第k时刻的初始对准状态估计值 粒子集 和粒子权值 从而得到失
准角估计值。在这一过程中，粒子数目由N降为
[0022] (5)时刻k＞M且有新的观测量时，根据步骤(1)中的状态空间模型，以前一时刻得到的粒子集 权值 作为当前时刻初始的粒子集和权值，利用UPF滤波方法进行初始对准的状态估计，得到失准角估计值。在这一过程中，粒子数目是[0023] 本发明的原理是：捷联惯导系统在大方位失准角下静基座初始对准的系统误差方程是非线性的，而且实际SINS的系统噪声为非高斯噪声。针对这些问题，本发明提出了一种MRUPF非线性滤波方法进行捷联惯导系统大方位失准角初始对准。MRUPF滤波方法将多分辨小波滤波与UPF相结合，在利用UPF进行初始对准的状态估计时，在某一时刻下利用多分辨方法对粒子进行选择，降低粒子数目，将选择的粒子集及粒子权值作为初始粒子集和权值，然后继续利用UPF滤波方法进行初始对准的状态估计。
[0024] 本发明与现有技术相比的优点在于：本发明用一种基于MRUPF的非线性滤波方法来解决捷联惯导大方位失准角初始对准问题。MRUPF滤波方法将多分辨小波滤波与UPF相结合，利用多分辨方法对粒子进行选择，降低粒子数目。克服了非高斯噪声条件下，捷联惯导大方位失准角静基座初始对准利用现有滤波方法计算量大，初始对准实时性差的不足，在保证初始对准精度的同时提高初始对准的实时性，从而提高捷联惯导系统的性能。
附图说明
[0025] 图1为本发明一种基于MRUPF的捷联惯导系统大方位失准角初始对准方法的流程图。
具体实施方式
[0026] 如图1所示，本发明利用MRUPF非线性滤波解决大方位失准角下的捷联惯导系统静基座初始对准问题，其具体步骤如下：
[0027] 1、建立捷联惯导系统大方位失准角静基座初始对准的状态空间模型[0028] 在捷联惯导系统中，初始对准的速度、姿态角误差方程分别为：
[0029]
[0030]
[0031] 式中，i，t，p分别代表惯性、地理、平台坐标系，地理坐标系为东北天地理坐标系；为比力信息；δft为比力信息误差项； 为捷联惯导系统三个方向的
p
加速度计的常值偏置；为地理系相对于惯性系的角速度； 为角速度误差项；ε ＝[εE T
εN εU] 为捷联惯导系统三个方向的陀螺仪的常值漂移； 表示从地理坐标系到平台坐标系的姿态矩阵，在大于2°的大方位失准角情况下， 可简化为：
[0032]
[0033] 在地球表面进行静基座初始对准，有 g为重力加速度；Lat为当地纬度，R为地球半径。由惯导系统的
速度误差方程与姿态角误差得大方位失准角下捷联惯导初始对准的非线性误差方程为：
[0034]
[0035] 式中，Cij为转换矩阵 的第i行，第j列元素，i＝1，...，3；j＝1，...，3；ωie为地球自转角速度。
[0036] 系统状态变量为 以东向和北向速度误差为观测量，即Z＝[ΔVE ΔVN]T，则系统的误差方程和观测方程分别为：
[0037]
[0038] 式中，f(X)为捷联惯导系统在大方位失准角下的非线性误差方程；为系统观测矩阵，I2×2为2×2单位阵，O2×8为2×8的零矩阵；υ和η分别为系统噪声和观测噪声，且分别服从以下非高斯分布：υ～pυ，η～pη。
[0039] 将上述的状态空间模型离散化得：
[0040]
[0041] 式中，Xk-1和Xk分别为第k-1时刻和第k时刻的状态变量；F(Xk-1)是第k-1时刻离散化的系统方程非线性项；υk-1为第k-1时刻的系统噪声；Zk为第k时刻的观测量；ηk为第k时刻的观测噪声。
[0042] 状态变量的先验概率p(Xk|Xk-1)与似然概率p(zk|Xk)分别满足：
[0043] p(Xk|Xk-1)＝pυ(Xk-F(Xk-1)) (7)
[0044] p(zk|Xk)＝pη(zk-HXk) (8)
[0045] 2、滤波初始化
[0046] 在初始时刻即第k＝0时刻，对步骤(1)选取的状态变量进行初始化，即对初始状态X0的分布p(X0)进行采样，生成N个服从p(X0)概率分布的粒子 其均值和方差 分别满足：
[0047]
[0048] 式中，E[·]表示数学期望。
[0049] 令各粒子的权值为
[0050] 3、设M为利用多分辨方法对粒子及粒子权值进行选择的时刻，当k＜M且有新的观测量时，根据步骤(1)中的状态空间模型，利用UPF滤波方法，得到初始对准失准角估计值。其基本步骤如下：
[0051] a.采样
[0052] 首先利用UKF得到粒子滤波的重要性采样函数，即利用UKF滤波算法由第k-1时刻得到的 和 求第k时刻各粒子 的均值 和方差 得到重要性采样函数：
[0053]
[0054] 式中， 表示以 为均值，以 为方差的正态分布；X0：k-1表示从初始时刻到第k-1时刻的状态变量；Z1：k表示从第1时刻到第k时刻的观测值。
[0055] 然后从重要性密度函数 中抽取粒子 即
[0056]
[0057] b.计算粒子权值
[0058]
[0059] 式中， 为第k-1时刻归一化后的权值。
[0060] c.权值归一化
[0061]
[0062] d.计算有效粒子的尺寸Neff
[0063]
[0064] 如果Neff小于门限值Nth，说明粒子多样性降低，需转入步骤(e)进行重采样，否则转入步骤(f)进行初始对准状态估计。
[0065] e.重采样
[0066] 重采样的目的是消除权值较小的粒子，增加权值较大的粒子，使重采样后的样本集的分布符合后验密度p(Xk|Zk)。重采样后每个粒子的权值 被重新设置为1/N。
[0067] f.初始对准状态估计
[0068] 第k时刻初始对准状态估计值 和估计误差方差阵Pk分别为：
[0069]
[0070]
[0071] 得到 后，其第3～5维就是初始对准失准角估计值。
[0072] 4、时刻k＝M且有新的观测值时，根据步骤(1)中的状态空间模型，基于多分辨方法对步骤(3)所得到的第k-1时刻的粒子及粒子权值进行选择，再利用UPF进行状态估计，得到第k时刻的初始对准失准角。该步骤将多分辨方法应用到UPF中，先利用多分辨方法对第k-1时刻得到的粒子权值 进行处理，再选择相应的粒子，最后将选择后的粒子集 及粒子权值 作为第k时刻初始粒子和权值进行UPF滤波，从而得到失准角估计值。其基本步骤如下：
[0073] a.多分辨小波滤波器组对权值进行分解
[0074] 设L为小波分解的尺度数，将第k-1时刻粒子集 及粒子权值 分为L
N/2 个时间块，对每个时间块的权值进行多分辨分解，将其分解为低通部分和高通部分：
[0075]
[0076] 式中， 表示第m个时间块的粒子权值； 为第k-1时刻尺度1的低通部分； 为第k-1时刻尺度j上的高通部分，且每个时间块中尺度j上的权值分布j-1个数为2 ，j＝1，…，L；T为正交的小波分解变换矩阵。
[0077] b.对各尺度的高通部分进行阈值处理
[0078]
[0079] 式中，j＝1，…，L；i＝1，…，2j-1；Ts为设定的权值阈值，Ts∈(10-4，10-2)。
[0080] 则经过阈值处理后，高通和低通部分的粒子权值写成矢量形式为：
[0081]
[0082] c.粒子选取
[0083] 对阈值处理后的粒子权值Wm，lh，k-1进行T逆变换得：
[0084]
[0085] 式中， 表示变换后的第m个时间块的粒子权值。经过阈值处理，Wm，lh，k-1中的某些权值为零，由于小波分解变换矩阵T为正交阵，所以 中某些权值会相等。对于相等的权值，只选取其中一个具有代表性的权值即可，同时选取其对应的粒子。记选取后的粒子集为 权值为 为经过选择后的粒子数目。
[0086] d)将粒子集 和权值 作为第k时刻初始的粒子集和权值，重复步骤(3)中的步骤a)～f)得到第k时刻的初始对准状态估计值 粒子集 和粒子权值此过程中，粒子数目是
[0087] 5、时刻k＞M且有新的观测量时，根据步骤(1)中的状态空间模型，以前一时刻得到的粒子集 权值 作为当前时刻初始的粒子集和权值，利用UPF滤波方法，重复步骤(3)中的步骤a)～f)进行初始对准状态估计，得到失准角估计值。在此过程中，粒子数目是
[0088] 本发明利用MRUPF滤波在进行初始对准时，将多分辨小波滤波与UPF相结合。首先利用UPF进行初始对准的状态估计，在某一时刻下利用多分辨方法对粒子进行选择，降低粒子数目，将选择的粒子集及粒子权值作为初始粒子集和权值，然后继续利用UPF滤波方法进行初始对准的状态估计，得到失准角估计值。通过多分辨方法减少UPF滤波的粒子数目，从而降低了计算量，在保证初始对准精度的同时，缩短了初始对准时间，提高了大方位失准角下捷联惯导系统初始对准的实时性。
[0089] 本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
