CN105004351A - 基于自适应upf的sins大方位失准角初始对准方法 - Google Patents

Abstract

本发明公开一种基于自适应UPF的SINS大方位失准角初始对准方法，包括以下步骤：步骤1：建立SINS非线性误差模型：使用欧拉平台误差角来表示理想导航坐标系与计算导航坐标系之间的失准角，且陔组误差角需要考虑转动的先后顺序，建立相应的SINS非线性误差模型；本发明解决了在一定程度上降低了由于系统简化、噪声统计特性不确定对系统造成的影响，同时较好的克服了UPF中粒子退化的现象，提高了捷联惯性导航系统初始对准精度且减少了对准时间。本发明为软件方法，不需要对系统硬件做任何修改，故实际实施方便、可行。

Description

基于自适应UPF的SINS大方位失准角初始对准方法

技术领域

本发明涉及一种基于自适应UPF的SINS大方位失准角初始对准方法，属于导航技术领域。

背景技术

随着导航系统应用领域的不断拓展，大多数应用环境不能满足初始失准角为大角度和噪声为高斯白噪声的条件，此时继续使用传统导航系统线性化模型和KF(Kalman Filter，卡尔曼滤波)将会产生较大的模型误差和估计误差，使得导航参数不可信。针对这种情况，国内外的研究主要分为两个方面：一是惯性导航系统的非线性模型的研究，二是非线性滤波器的研究。常用的非线性滤波方法有EKF(Extended Kalman Filter，扩展卡尔曼滤波)、UKF(Unscented Kalman Filter，无迹卡尔曼滤波)、PF(Particle Filter，粒子滤波)、EKF-PF、UKF-PF(UPF)，在一定程度上UPF的使用限制更少、滤波效果要优于其他几种。

发明内容

发明目的：本发明的目的在于针对大多数应用环境不能满足初始失准角为大角度和噪声为高斯白噪声的条件以及UPF的不足进行改进，本发明提出一种基于强跟踪滤波器技术的自适应UPF的大方位失准角初始对准的方法。本发明将UPF和自适应UPF两种非线性滤波器在初始对准中的滤波效果进行了比较，结果表明自适应UPF比UPF具有更快的收敛速度并且在一定程度上提高了对准精度。本发明的方法是通过引入衰减记忆因子有效增强当前信息残差对系统修正作用，在一定程度上降低了由于系统简化、噪声统计特性不确定对系统造成的影响，同时较好的克服了UPF中粒子退化的现象。

技术方案：本发明所述的基于自适应UPF的SINS大方位失准角初始对准方法，包括以下步骤：

步骤1：建立SINS非线性误差模型：

使用欧拉平台误差角来表示理想导航坐标系与计算导航坐标系之间的失准角，且该组误差角需要考虑转动的先后顺序，建立相应的SINS非线性误差模型；

本发明坐标系选取如下：

i系——地心惯性坐标系，原点位于地心0，x_i轴指向春分点，z_i轴沿地球 自转轴，y_i轴与x_i、z_i构成右手系；

e系——地球坐标系，原点位于地心，x_e轴穿越本初子午线与赤道的交点，z_e轴穿越地球北极点，y_e轴穿越东经90°子午线与赤道的交点；

n系——导航坐标系，这里选取“东-北-天(E-N-U)”地理坐标系为导航坐标系；

b系——“右-前-上”坐标系为载体坐标系(b系)；

n系先后依次经过三次欧拉角转动至b系，这三个欧拉角分别记为航向角ψ∈(-π π]、俯仰角θ∈[-π/2 π/2]和横滚角γ∈(-π π]，n系与b系之间的旋转变换关系可用姿态矩阵描述；

步骤2：SINS大方位失准角初始对准误差模型：

现假设两水平失准角都为小角度；假设陀螺测量误差主要为常值漂移误差ε^b和零均值高斯白噪声加速度计测量误差主要为常值偏置误差和零均值高斯白噪声忽略重力误差项δgⁿ，静基座下成立，则初始对准滤波模型的状态方程为：

令状态向量噪声向量 建立滤波状态模型，并直接以SINS速度误差z＝δvⁿ作为观测量建立观测方程如下：

f(x)和g(x)的具体表达式参考上式的初始对准滤波模型的状态方程，H＝[0 I 0 0]，v为量测噪声；

步骤3：标准UPF算法：

假设初始状态变量x₀～p(x₀)，方差阵为P₀，对观测方程进行无迹粒子滤波，算法如下：

步骤3.1：初始化：k＝0；从初始的先验概率分布p(x₀)中进行粒子 i＝1，2，3…，N的采样，为简化计算，令其中 

步骤3.2：加权粒子的预测、采样：k＝1，2，...；利用无迹卡尔曼滤波对粒子进行预测更新，并计算σ样本点：

式中，λ＝α²(n+κ)-n，10^-4≤α≤1，κ＝3-n，为估计均方误差，n为状态维数；

时间更新为： 

其中

λ＝α²(n+κ)-n，10^-4≤α≤1，κ＝3-n，对于正态分布而言，为预测均方差，W为权值，Z为量测量，K为滤波增益；

量测更新为： 

以推荐密度函数生成粒子作为二次采样的原始粒子；

步骤3.3：根据权值更新公式对N个粒子相应的权值进行计算并做归一化处理；其中， 

步骤3.4：通过重采样算法对原始粒子(i＝1，2，…，N)作二次采样生成二次采样粒子(j＝1，2，…，N)，并计算每个二次采样粒子的权重系数；

步骤3.5：根据计算状态变量的最优估计及每个粒子对应的方差矩阵；

步骤3.6：将步骤3.4重采样后的粒子及步骤3.5计算的代入第2步进行迭代运算；

步骤4：自适应UPF算法：

基于强跟踪滤波的思想，采用时变渐消因子削弱陈旧数据对当前滤波值的影 响，实时调整状态预测误差的协方差以及相应的增益矩阵来达到。这在一定程度上能够减缓UPF粒子退化的现象，并且加快粒子滤波的收敛速度。

本发明采取的自适应措施是对滤波器的协方差进行判断，具体判断公式如下：

其中，S为设定的调整系数，一般取S＞1，此处取S＝1.5；为系统的残差序列， 

当上式的判断公式不成立时，需要对进行有效修正，本发明采用的方式是引入哀减记忆因子的自适应加权系数λ_k，其具体的定义如下：

修正公式为： 

式中，

式中，ρ一般取0＜ρ≤1，其主要作用是增强滤波器对系统状态的快速跟踪能力，其越大，则当前信息分配的权值也越大，当前信息的残差对系统估计的影响也更突出。为了保证系统对缓慢变化情况和突变状态的强跟踪能力，本发明取ρ＝0.95；

进一步地，步骤1具体包括： 

步骤1.1：建立姿态误差方程

实际工作的导航系统由于存在各种干扰和量测误差，SINS的计算的导航坐 标系(n′系)通常与理想的导航坐标系(n系)之间存在转动误差，n系需按照一定的顺序依次转过三个角度才能与n′系重合，现假设这三次转动依次绕z轴、x轴、y轴旋转，且转过的角度分别记为其矢量表示形式为 三次旋转对应的姿态变换阵依次为故n系至n′系的变换矩阵为：

其中，

若设n′系相对于n系的角速度为则与欧拉平台误差角的关系可表示如下：

则由上式可得欧拉平台误差角微分方程：

其中，

在导航坐标系中，SINS的姿态矩阵微分方程表示如下：

式中，运算符(·×)表示由向量·构成的反对称矩阵，符号表示坐标系c相对于坐标系b的运动向量在坐标系d中的投影，符号表示b系至c系的变换矩阵， 而实际上用于姿态更新的微分方程是带有误差的，其形式可以表示为：

式中，符号“～”表示测量值， 为的测量值，为的计算误差，为陀螺测量误差；令理论姿态矩阵与实际姿态矩阵的解算误差为：

式中，I表示单位矩阵，对两边同时求导，并将SINS的姿态矩阵微分方程和实际用于姿态更新的微分方程带入其中得：

同时，对两边同时求导，并将实际用于姿态更新的微分方程带入其中得：

令上两式的右端相等，则

即

再把矩阵微分方程带入上式，并在上式两边同时左乘和右乘 得

由反对称矩阵与矢量之间的关系以及反对称矩阵相似变换，上式可以等价成如下的矢量形式

式中，将上两式带入其中得

将其带入欧拉平台误差角微分方程可以得到SINS姿态误差方程为：

步骤1.2：速度误差方程的获取

在导航坐标系中，SINS速度微分方程为：

式中，为加速度计测量值，为地球自转角速率在n系上投影，是n系相对于e系的角速度在n系上的投影，vⁿ为SINS的速度在n系上的投影，为SINS的速度在n系上的投影的导数，gⁿ为当地重力加速度在n系上的投影；

但在实际系统中该速度微分方程含有误差，此时SINS的速度微分方程应为：

式中， 其中为加速度计测量误差。直接将减去式得到SINS的速度误差方程为：

在SINS姿态误差方程和速度误差方程中的计算参数误差和具体表示为：

式中，L和λ分别为所处位置所对应的纬度和经度，其中

进一步地，本发明自适应UPF的实现步骤如下：

步骤4.1：初始化：k＝0；从初始的先验概率分布p(x₀)中进行粒子 i＝1，2，3…，N的采样，为简化计算，令其中 

步骤4.2：预测更新 

根据时间更新、量测更新求取和具体的协方差如下：

步骤4.3：判断滤波器的协方差是否满足判断公式的定义，如果满足则跳至步骤4.5，否则按照上式的修正公式对进行修正；

步骤4.4：量测更新 

步骤4.5：根据权值更新公式对N个粒子相应的权值进行计算并做归一化处理；

步骤4.6：通过重采样算法对原始粒子(i＝1，2，…，N)作二次采样生成二次采样粒子(j＝1，2，…，N)，并计算每个二次采样粒子的权重系数；

步骤4.7：根据计算状态变量的最优估计及每个粒子对应的方差矩阵；

步骤4.8：将步骤4.6重采样后的粒子及步骤4.7计算的代入步骤4.2进行迭代运算；

本发明与现有技术相比，其有益效果是：1)解决了在一定程度上降低了由于系统简化、噪声统计特性不确定对系统造成的影响，同时较好的克服了UPF中粒子退化的现象，提高了捷联惯性导航系统初始对准精度且减少了对准时间。本发明为软件方法，不需要对系统硬件做任何修改，故实际实施方便、可行；2) 自适应UPF算法有效降低了运算量，提高了算法的实时处理能力，在一定程度上降低了由于系统简化、噪声统计特性不确定对系统造成的影响，同时较好的克服了UPF中粒子退化的现象；3)通过仿真比较了UPF和自适应UPF在捷联惯导系统大方位失准情况下噪声统计特性固定及噪声统计特性不固定的初始对准，在噪声统计特性确定时使用自适应UPF比普通UPF具有更快的对准速度，对准精度优势不明显。当系统噪声的统计特性不确定时，采用自适应UPF与普通UPF相比，对准速度和对准精度具有明显的优势。另外，通过选取不同的重采样算法发现，三种重采样算法中残差重采样算法造成的系统误差大于系统重采样和多项式重采样对系统造成的误差，这在高精度导航系统中是不可以忽视的。本发明的自适应UPF在工程中的运用具有一定的参考价值。

附图说明

图1为本发明自适应UPF实现的步骤；

图2为本发明基于自适应UPF与UPF初始对准的x方向的对准误差；

图3为本发明基于自适应UPF与UPF初始对准的y方向的对准误差；

图4为本发明基于自适应UPF与UPF初始对准的z方向的对准误差；

具体实施方式

下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。

实施例1：

本实施例的基于自适应UPF的SINS大方位失准角初始对准方法，具体步骤为：

步骤1：建立SINS非线性误差模型：

使用欧拉平台误差角来表示理想导航坐标系与计算导航坐标系之间的失准角，且该组误差角需要考虑转动的先后顺序，建立相应的SINS非线性误差模型；

本发明坐标系选取如下：

i系——地心惯性坐标系，原点位于地心0，x_i轴指向春分点，z_i轴沿地球自转轴，y_i轴与x_i、z_i构成右手系； 

e系——地球坐标系，原点位于地心，x_e轴穿越本初子午线与赤道的交点， z_e轴穿越地球北极点，y_e轴穿越东经90°子午线与赤道的交点；

n系——导航坐标系，这里选取“东-北-天(E-N-U)”地理坐标系为导航坐标系；

b系——“右-前-上”坐标系为载体坐标系(b系)；

n系先后依次经过三次欧拉角转动至b系，这三个欧拉角分别记为航向角ψ∈(-π π]、俯仰角θ∈[-π/2 π/2]和横滚角γ∈(-π π]，n系与b系之间的旋转变换关系可用姿态矩阵描述；

步骤2：SINS大方位失准角初始对准误差模型：

现假设两水平失准角都为小角度；假设陀螺测量误差主要为常值漂移误差ε^b和零均值高斯白噪声加速度计测量误差主要为常值偏置误差和零均值高斯白噪声忽略重力误差项δgⁿ，静基座下成立，则初始对准滤波模型的状态方程为：

令状态向量噪声向量 建立滤波状态模型，并直接以SINS速度误差z＝δvⁿ作为观测量建立现测方程如下：

f(x)和g(x)的具体表达式参考上式的初始对准滤波模型的状态方程，H＝[0 I 0 0]，v为量测噪声；

步骤3：标准UPF算法：

假设初始状态变量x₀～p(x₀)，方差阵为P₀，对观测方程进行无迹粒子滤波，算法如下：

步骤3.1：初始化：k＝0；从初始的先验概率分布p(x₀)中进行粒子 i＝1，2，3…，N的采样，为简化计算，令其中 

步骤3.2：加权粒子的预测、采样：k＝1，2，...；利用无迹卡尔曼滤波对粒子进行预测更新，并计算σ样本点：

式中，λ＝α²(n+κ)-n，10^-4≤α≤1，κ＝3-n，为估计均方误差，n为状态维数；

时间更新为： 

其中

λ＝α²(n+κ)-n，10^-4≤α≤1，κ＝3-n，对于正态分布而言，β＝2， 为预测均方差，W为权值，Z为量测量，K为滤波增益；

量测更新为： 

以推荐密度函数生成粒子作为二次采样的原始粒子；

步骤3.3：根据权值更新公式对N个粒子相应的权值进行计算并做归一化处理；其中， 

步骤3.4：通过重采样算法对原始粒子(i＝1，2，…，N)作二次采样生成二次采样粒子(j＝1，2，…，N)，并计算每个二次采样粒子的权重系数；

步骤3.5：根据计算状态变量的最优估计及每个粒子对应的方差矩阵；

步骤3.6：将步骤3.4重采样后的粒子及步骤3.5计算的代入第2步进行迭代运算；

步骤4：自适应UPF算法：

基于强跟踪滤波的思想，采用时变渐消因子削弱陈旧数据对当前滤波值的影响，实时调整状态预测误差的协方差以及相应的增益矩阵来达到。这在一定程度上能够减缓UPF粒子退化的现象，并且加快粒子滤波的收敛速度。

本发明采取的自适应措施是对滤波器的协方差进行判断，具体判断公式如下：

其中，S为设定的调整系数，一般取S＞1，此处取S＝1.5；为系统的残差序列， 

当上式的判断公式不成立时，需要对进行有效修正，本发明采用的方式是引入哀减记忆因子的自适应加权系数λ_k，其具体的定义如下：

修正公式为： 

式中，

式中，ρ一般取0＜ρ≤1，其主要作用是增强滤波器对系统状态的快速跟踪能力，其越大，则当前信息分配的权值也越大，当前信息的残差对系统估计的影响也更突出。为了保证系统对缓慢变化情况和突变状态的强跟踪能力，本发明取ρ＝0.95；

进一步地，步骤1具体包括： 

步骤1.1：建立姿态误差方程

实际工作的导航系统由于存在各种干扰和量测误差，SINS的计算的导航坐标系(n′系)通常与理想的导航坐标系(n系)之间存在转动误差，n系需按照一定的顺序依次转过三个角度才能与n′系重合，现假设这三次转动依次绕z轴、x轴、 y轴旋转，且转过的角度分别记为其矢量表示形式为 三次旋转对应的姿态变换阵依次为故n系至n′系的变换矩阵为：

其中，

若设n′系相对于n系的角速度为则与欧拉平台误差角的关系可表示如下：

则由上式可得欧拉平台误差角微分方程：

其中，

在导航坐标系中，SINS的姿态矩阵微分方程表示如下：

式中，运算符(·×)表示由向量·构成的反对称矩阵，符号表示坐标系c相对于坐标系b的运动向量在坐标系d中的投影，符号表示b系至c系的变换矩阵，而实际上用于姿态更新的微分方程是带有误差的，其形式可以表示为：

式中，符号“～”表示测量值， 为的测量值，为的计算误差，为陀螺测量误差；令理论姿态矩阵与实际姿态矩阵的解算误差为：

式中，I表示单位矩阵，对两边同时求导，并将SINS的姿态矩阵微分方程和实际用于姿态更新的微分方程带入其中得：

同时，对两边同时求导，并将实际用于姿态更新的微分方程带入其中得：

令上两式的右端相等，则

即

再把矩阵微分方程带入上式，并在上式两边同时左乘和右乘 得

由反对称矩阵与矢量之间的关系以及反对称矩阵相似变换，上式可以等价成如下的矢量形式

式中，

将上两式带入其中得

将其带入欧拉平台误差角微分方程可以得到SINS姿态误差方程为：

步骤1.2：速度误差方程的获取

在导航坐标系中，SINS速度微分方程为：

式中，为加速度计测量值，为地球自转角速率在n系上投影，是n系相对于e系的角速度在n系上的投影，vⁿ为SINS的速度在n系上的投影，为SINS的速度在n系上的投影的导数，gⁿ为当地重力加速度在n系上的投影；

但在实际系统中该速度微分方程含有误差，此时SINS的速度微分方程应为：

式中， 其中为加速度计测量误差。直接将减去式得到SINS的速度误差方程为：

在SINS姿态误差方程和速度误差方程中的计算参数误差和具体表示为：

式中，L和λ分别为所处位置所对应的纬度和经度，其中

进一步地，本发明自适应UPF的实现步骤如下：

步骤4.1：初始化：k＝0；从初始的先验概率分布p(x₀)中进行粒子 i＝1，2，3…，N的采样，为简化计算，令其中 

步骤4.2：预测更新 

根据时间更新、量测更新求取和具体的协方差如下：

步骤4.3：判断滤波器的协方差是否满足判断公式的定义，如果满足则跳至步骤4.5，否则按照上式的修正公式对进行修正；

步骤4.4：量测更新 

步骤4.5：根据权值更新公式对N个粒子相应的权值进行计算并做归一化处理；

步骤4.6：通过重采样算法对原始粒子(i＝1，2，…，N)作二次采样生成二次采样粒子(j＝1，2，…，N)，并计算每个二次采样粒子的权重系数；

步骤4.7：根据计算状态变量的最优估计及每个粒子对应的方差矩阵；

步骤4.8：将步骤4.6重采样后的粒子及步骤4.7计算的代入步骤4.2进行迭代运算；

实施例：

在静基座条件下，陀螺仪的常值漂移为0.01°/h，随机漂移为0.001°/h；加速度计零偏为100μg(g＝9.8m²/s)，随机偏差为50μg；当地地理纬度为32.37°，经度为118.22°。仿真时间为2000s。

按照建立的大方位失准角误差模型，在噪声统计特性确定的情况下分别采用两种滤波算法进行了仿真实验。现选取初始失准角仿真过 程中不进行反馈修正，对准误差的仿真结果如图2，图3及图4所示。

表1：确定噪声统计特性是对准统计结果

通过图2，图3及图4可以看出，基于大方位失准角的SINS误差模型，当初始失准角为时，采用自适应UPF，水平对准所需时间小于200s，方位对准所需时间小于450s；采用UPF，水平对准所需时间约为400s，方位对准约为600s。自适应UPF的对准时间明显优于UPF。但两者的对准精度相当。通过表1的统计结果可以看出在一定程度上采用自适应UPF比UPF的对准精度更高。

如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims (3)

1.一种基于自适应UPF的SINS大方位失准角初始对准方法，其特征为：

步骤1：建立SINS非线性误差模型：

使用欧拉平台误差角来表示理想导航坐标系与计算导航坐标系之间的失准角，且该组误差角需要考虑转动的先后顺序，建立相应的SINS非线性误差模型；

本发明坐标系选取如下：

i系——地心惯性坐标系，原点位于地心0，x_i轴指向春分点，z_i轴沿地球自转轴，y_i轴与x_i、z_i构成右手系；

e系——地球坐标系，原点位于地心，x_e轴穿越本初子午线与赤道的交点，z_e轴穿越地球北极点，y_e轴穿越东经90°子午线与赤道的交点；

n系——导航坐标系，这里选取“东-北-天(E-N-U)”地理坐标系为导航坐标系；

b系——“右-前-上”坐标系为载体坐标系(b系)；

n系先后依次经过三次欧拉角转动至b系，这三个欧拉角分别记为航向角ψ∈(-π π]、俯仰角θ∈[-π/2 π/2]和横滚角γ∈(-π π]，n系与b系之间的旋转变换关系可用姿态矩阵描述；

步骤2：SINS大方位失准角初始对准误差模型：

现假设两水平失准角都为小角度；假设陀螺测量误差主要为常值漂移误差ε^b和零均值高斯白噪声加速度计测量误差主要为常值偏置误差和零均值高斯白噪声忽略重力误差项δgⁿ，静基座下成立，则初始对准滤波模型的状态方程为：

式中， 和分别为SINS的理想的导航坐标系(n系)绕x轴、y轴和z轴转过的角度，为向量的导数，为SINS的理想的导 航坐标系(n系)至计算的导航坐标系(n′)的变换矩阵，为陀螺的常值漂移误差ε^b的导数，为加速计的常值偏置的导数，误差令状态向量 噪声向量建立滤波状态模型，并直接以SINS速度误差z＝δvⁿ作为观测量建立观测方程如下：

f(x)和g(x)的具体表达式参考上式的初始对准滤波模型的状态方程，H＝[0 I 0 0]，v为量测噪声；

步骤3：标准UPF算法：

假设初始状态变量x₀～p(x₀)，方差阵为P₀，对观测方程进行无迹粒子滤波，算法如下：

步骤3.1：初始化：k＝0；从初始的先验概率分布p(x₀)中进行粒子 i＝1，2，3…，N的采样，为简化计算，令其中 

步骤3.2：加权粒子的预测、采样：k＝1，2，...；利用无迹卡尔曼滤波对粒子进行预测更新，并计算σ样本点：

式中，λ＝α²(n+κ)-n，10^-4≤α≤1，κ＝3-n，为估计均方误差，n为状态维数；

时间更新为：

其中

λ＝α²(n+κ)-n，10^-4≤α≤1，κ＝3-n，对于正态分布而言，β＝2，为预测均方差，W为权值，Z为量测量，K为滤波增益；

量测更新为：

以推荐密度函数生成粒子作为二次采样的原始粒子；

步骤3.3：根据权值更新公式对N个粒子相应的权值进行计算并做归一化处理；其中，

步骤3.4：通过重采样算法对原始粒子作二次采样生成二次采样粒子并计算每个二次采样粒子的权重系数；

步骤3.5：根据计算状态变量的最优估计及每个粒子对应的方差矩阵；

步骤3.6：将步骤3.4重采样后的粒子及步骤3.5计算的代入第2步进行迭代运算；

步骤4：自适应UPF算法：

基于强跟踪滤波的思想，采用时变渐消因子削弱陈旧数据对当前滤波值的影响，实时调整状态预测误差的协方差以及相应的增益矩阵来达到；这在一定程度上能够减缓UPF粒子退化的现象，并且加快粒子滤波的收敛速度；

以上采取的自适应措施是对滤波器的协方差进行判断，具体判断公式如下：

其中，S为设定的调整系数，一般取S＞1，此处取S＝1.5；为系统的残差序列， 

当上式的判断公式不成立时，需要对进行有效修正，在此采用的方式是引入衰减记忆因子的自适应加权系数λ_k，其具体的定义如下：

修正公式为：

式中，

Q为系统噪声方差，

式中，ρ一般取0＜ρ≤1，其主要作用是增强滤波器对系统状态的快速跟踪能力，其越大，则当前信息分配的权值也越大，当前信息的残差对系统估计的影响也更突出。为了保证系统对缓慢变化情况和突变状态的强跟踪能力，在此取ρ＝0.95。

2.根据权利要求1所述的基于自适应UPF的SINS大方位失准角初始对准方法，其特征在于：

步骤1具体包括：

步骤1.1：建立姿态误差方程

实际工作的导航系统由于存在各种干扰和量测误差，SINS的计算的导航坐标系(n′系)通常与理想的导航坐标系(n系)之间存在转动误差，n系需按照一定的顺序依次转过三个角度才能与n′系重合，现假设这三次转动依次绕z轴、x轴、y轴旋转，且转过的角度分别记为其矢量表示形式为三次旋转对应的姿态变换阵依次为故n系至n′系的变换矩阵为：

其中，

若设n′系相对于n系的角速度为则与欧拉平台误差角的关系可表示如下：

则由上式可得欧拉平台误差角微分方程：

其中，

在导航坐标系中，SINS的姿态矩阵微分方程表示如下：

式中，运算符(·×)表示由向量·构成的反对称矩阵，符号表示坐标系c相对于坐标系b的运动向量在坐标系d中的投影，符号表示b系至c系的变换矩阵，而实际上用于姿态更新的微分方程是带有误差的，其形式可以表示为：

式中，符号“～”表示测量值， 为的测量值，为的计算误差，为陀螺测量误差；令理论姿态矩阵与实际姿态矩阵的解算误差为：

式中，I表示单位矩阵，对两边同时求导，并将SINS的姿态矩阵微分方程和实际用于姿态更新的微分方程带入其中得：

同时，对两边同时求导，并将实际用于姿态更新的微分方程带入其中得：

令上两式的右端相等，则

即

再把矩阵微分方程带入上式，并在上式两边同时左乘和右乘得

由反对称矩阵与矢量之间的关系以及反对称矩阵相似变换，上式可以等价成如下的矢量形式

式中，

将上两式带入其中得

将其带入欧拉平台误差角微分方程可以得到SINS姿态误差方程为：

步骤1.2：速度误差方程的获取

在导航坐标系中，SINS速度微分方程为：

式中，为加速度计测量值，为地球自转角速率在n系上投影，是n系相对于e系的角速度在n系上的投影，vⁿ为SINS的速度在n系上的投影，为SINS的速度在n系上的投影的导数，gⁿ为当地重力加速度在n系上的投影；

但在实际系统中该速度微分方程含有误差，此时SINS的速度微分方程应为：

式中， 其中为加速度计测量误差。直接将减去式得到SINS的 速度误差方程为：

在SINS姿态误差方程和速度误差方程中的计算参数误差和具体表示为：

式中，L和λ分别为所处位置所对应的纬度和经度，其中。

3.根据权利要求1所述的基于自适应UPF的SINS大方位失准角初始对准方法，其特征在于：

所述自适应UPF实现步骤如下：

步骤4.1：初始化：k＝0；从初始的先验概率分布p(x₀)中进行粒子 i＝1，2，3…，N的采样，为简化计算，令其中 

步骤4.2：预测更新

根据时间更新、量测更新求取和具体的协方差如下：

步骤4.3：判断滤波器的协方差是否满足判断公式的定义，如果满足则跳至步骤4.5，否则按照上式的修正公式对进行修正；

步骤4.4：量测更新

步骤4.5：根据权值更新公式对N个粒子相应的权值进行计算并做归一化处理；

步骤4.6：通过重采样算法对原始粒子作二次采样生成二次采样粒子并计算每个二次采样粒子的权重系数；

步骤4.7：根据计算状态变量的最优估计及每个粒子对应的方差矩阵；

步骤4.8：将步骤4.6重采样后的粒子及步骤4.7计算的代入步骤4.2进行迭代运算。