[发明专利]一种基于最小偏度单形采样的自适应UKF组合导航方法

权利要求书

1.一种基于最小偏度单形采样的自适应UKF组合导航方法，其特征在于，包括以下步骤：

(1)根据SINS和GPS的误差特性，建立起SINS/GPS紧组合误差模型，即状态方程和量测方程；

(2)选取初始滤波值并设置初始比例因子α；

(3)采用最小偏度单形采样的方法计算Sigma样本点和相应的权值并更新比例因子α＝α_k|k-1；

(4)将计算得到的Sigma采样点通过状态函数求取Sigma点集的一步预测并计算系统状态量的一步预测和协方差矩阵P(k|k-1)；

(5)计算系统的状态更新和协方差更新P(k|k)；

所述的步骤(1)具体包括：

(1.1)建立状态方程：

(1.1.1)选取SINS系统的误差量为：东北天向的平台误差角速度误差δv_E,δv_N,δv_U，经度、纬度、高度的位置误差δL,δλ,δh，沿载体坐标系x,y,z轴的陀螺仪漂移ε_bx,ε_by,ε_bz、陀螺仪的一阶马氏过程ε_rx,ε_ry,ε_rz，加速度计零偏

(1.1.2)选取GPS系统的误差量为：与时钟频率误差等效的速度误差量δ_tru以及与时钟误差等效的距离误差量δ_tu；

(1.1.3)SINS系统的误差量和GPS系统的误差量为状态量列写状态方程得：

即

式中的状态向量X(t)为20维，是分别由X_I(t)的18维，和X_G(t)的2维合并成的，下标I表示SINS，下标G表示GPS，

其中，F_I(t)、F_G(t)分别为X_I(t)，X_G(t)的系统矩阵，B_I(t)、B_G(t)分别为W_I(t)，W_G(t)的噪声驱动阵，

W_G(t)＝[w_u,w_ru]^T；

其中，ω_gx,ω_gy,ω_gz,ω_rx,ω_ry,ω_rz,ω_ax,ω_ay,ω_az分别为陀螺仪漂移、陀螺仪的一阶马氏过程以及加速度计在载体坐标系下的白噪声；

(1.2)建立量测方程：

(1.2.1)紧组合系统的伪距量测方程为：

Z_ρ(t)＝H_ρ(t)X(t)+v_ρ(t)

式中，Z_p(t)为伪距量测量，Z_ρ(t)＝[δρ₁ δρ₂ … δρ_n]^T，

H_ρ(t)＝[0_n×6 H_ρ1 0_n×6 H_ρ2]，

(1.2.2)紧组合系统的伪距率量测方程为：

式中，

(1.2.3)合并伪距量测方程和伪距率量测方程并进行维数扩展，得到SINS/GPS导航系统紧组合模型的量测方程：

所述的步骤(2)具体包括：

滤波器的初值取为：

其中，x₀为状态变量的初值，为x₀的均值，P₀为状态变量的初始误差协方差矩阵；

根据量测噪声m维和系统噪声q维，对n维的系统状态矩阵和协方差矩阵进行扩维，并分别设初始状态向量和初始协方差矩阵为：

其中，Q是系统噪声的协方差阵，R是观测噪声的协方差阵；

比例因子α用来控制Sigma点到中心点x的距离，在[10^-4，1]区间上取值；

所述的步骤(3)具体包括：

(3.1)最小偏度单形采样：

Sigma点集为：

其中，设为j维空间采样点集的第i个Sigma点，不失一般性，假设P_xx＝I，选择0≤W₀≤1；

Sigma采样点一阶、二阶权系数分别为：

其中α为比例因子，参数β用来描述x的先验分布信息，β≥0是一个非负的权函数，上标m为均值，c为协方差，下标为第几个采样点，表示P_xx的平方根矩阵，可用Cholesky分解取下三角计算得到；

(3.2)更新比例因子：

具体算法为：

(3.2.1)由x_k|k和P_k|k按预先确定的最小偏度单形采样策略生成Sigma点集和权值{χ_i,W_j}；

(3.2.2)计算

(3.2.3)令

其中，d_i为比例修正后第i个Sigma点到中心点x的距离，即L为最小偏度单形采样点的个数；

所述的步骤(4)具体包括：

计算得到的Sigma采样点通过状态函数求取Sigma点集的一步预测

预测k时刻的系统状态变量及协方差：

根据一步预测值再次进行UT变换，生成新的Sigma点集，代入观测方程得：

预测系统的量测均值及协方差：

计算卡尔曼增益阵：

所述的步骤(5)具体包括：

计算系统的状态更新和协方差更新：