[发明专利]一种基于强跟踪动机座旋转调制大失准角初始对准方法

权利要求书

1.一种基于强跟踪动机座旋转调制大失准角初始对准方法，其特征在于：包括下列步骤：

步骤1)，建立单轴旋转调制无迹卡尔曼滤波(UKF)模型，

步骤1.1)，建立非线性状态方程如下：

其中，s系为MEMS惯性单元坐标系，b为载体坐标系，n与n'分别为理想导航坐标系与实际数学平台坐标系，i与e分别为惯性坐标系与地心坐标系，是n系到n'系的旋转矩阵，与分别表示n系相对于i系的角速度计算值与计算误差，是b系到n'系的旋转矩阵，δvⁿ为速度误差，表示加速度测量值，表示地球自转，表示n系相对于e系的角速度计算值,I表示单位矩阵，ε^s与分别表示陀螺常值漂移与加速度计常值偏置，与分别表示陀螺与加速度计的零均值的高斯白噪声，α,C_ω及计算式如下：

α＝[α_x,α_y,α_z]^T (2)

α_x，α_y及α_z表示表示n系到n'系的三次转动角,Ω为旋转调制角速度，

步骤1.2)，依据式(1)建立卡尔曼滤波方程如下：

及是由MEMS及GNSS(全球卫星定位系统)计算的载体在n系中的线速度，f(x)与g(x)由式(1)计算得到，状态向量噪声向量H＝[0 I 0 0]^T，v为量测噪声向量，

步骤1.3)，基于离散UKF卡尔曼滤波模型如下：

其中,与分别是是k-1时刻与k时刻的状态估计值,为一步状态估计值，为一步量测预测误差,为量测预测误差，z_k为量测，P_k-1为预测均方误差阵,P_k/k-1为一步预测均方误差阵,为状态预测误差与量测预测误差之间协方差阵，为量测预测误差均方差阵，Q_k-1量测噪声分配矩阵,R_k为量测状态噪声，H_k为量测转移矩阵,K_k为增益矩阵,为由组成的L列矩阵，χ_k-1为sigma点矩阵，L，W_i^(m)与W_i^(c)为待求解相关系数，

步骤2)，建立多渐消因子强跟踪卡尔曼滤波器模型，

步骤2.1)强跟踪卡尔曼滤波器的策略是是通过渐消因子调整P_k/k-1以保持残差相互正交，抑制外部干扰，防止系统发散，k时刻的新息矢量为：

步骤2.2)k时刻新息矢量的协方差阵估计值为：

其中，C_0,k计算方法如下：

其中，b为常值系数，

步骤2.3)，建立基于多渐消因子的P_k/k-1的修正公式为：

其中，λ_k＝diag(s₁,s₂,…s_n)为渐消因子组成的对角矩阵，s_i≥1,i＝1,2,…n，n为状态向量的维数，

步骤2.4)新息序列互不相关的等价条件是：

其中，符号E表示求期望，式(11)的等价形式为：

强跟踪卡尔曼滤波的原理即通过选取合适的λ_k使式(12)近似成立，

步骤3)，建立基于卡方检验的异常检测准则如下：

其中，为检测阈值，α为置信水平，m为自由度，θ_k为滤波系统计算的卡方值，其计算方法如下：

步骤4)，定义目标矩阵如下：

代入式(10)到式(15)得：

调整渐消因子矩阵λ_k使矩阵h(k)所有元素绝对值取值最小，使等式(12)近似成立，则设计评价函数的等价形式为：

其中，n和m分别为矩阵h(k)行数及列数，s_ij是矩阵h(k)的元素，渐消因子的寻优过程即使fit取最小值，

步骤5)，建立基于改进的具有全局和局部搜索能力的粒子群搜索算法(PSO)求解多渐消因子，

步骤5.1)，随机初始化M个n维粒子的位置与移动速度：X_i及V_i,i＝1,2,…,M，

步骤5.2)，运用式(18)计算M个粒子对应的渐消因子矩阵，运用式(17)计算M个粒子对应的评价函数值fit(X_i)，记录M个粒子的全局最优位置为gbest及其评价函数值fit(gbest)，存储M个粒子的当前位置pbest_i及其评价函数值fit(pbest_i)，其中pbest_i＝X_i，i＝1,2,…,M，

λ_k(j,j)＝V_i(j),j＝1,2,…n (18)

步骤5.3)，随机生成M个粒子的左、右位置如下：

其中，LX_i为左位置，RX_i为右位置，d_t为移动步距，τ_i为n维单位向量，计算粒子左、右位置对应的评价函数值fit(LX_i)及fit(RX_i)，按如下原则更新M个粒子的新位置：

若fit(LX_i)fit(X_i),则X_i＝LX_i，fit(X_i)＝fit(LX_i)，

若fit(RX_i)fit(X_i),则X_i＝RX_i，fit(X_i)＝fit(RX_i)，

步骤5.4)，更新M个粒子的全局最优位置gbest及其评价函数值fit(gbest)；按如下准则更新M个粒子的最优历史位置pbest_i及其评价函数值fit(pbest_i)：

若fit(X_i)fit(pbest_i),则pbest_i＝X_i，fit(pbest_i)＝fit(X_i)，

步骤5.5)：更新M个粒子的移动速度如下：

其中，c₁与c₂为学习因子，为惯性系数，rand₁与rand₂为随机生成的n维单位向量，按式(21)计算M个粒子的新位置X_i，并计算其对应的评价函数值fit(X_i)：

X_i＝X_i+V_i (21)

步骤5.6)：依据步骤5.4)更新M个粒子的全局最优位置gbest及其评价函数值fit(gbest)，更新M个粒子的历史最优位置pbest_i及其对应评价函数值fit(pbest_i)，

步骤5.7)，若达到停止条件，则结束迭代循环并输出渐消因子，即λ_k(i,i)＝gbest(i),i＝1,2,…n，否则，返回步骤5.3)继续下一个循环。