[发明专利]一种基于多渐消因子自适应动机座旋转调制精对准方法

权利要求书

1.一种基于多渐消因子自适应动机座旋转调制精对准方法，其特征在于：包括下列步骤：

步骤1)，沿Z轴的单轴旋转调制矩阵转换模型如下：

其中，s系为MEMS单元坐标系，b为载体坐标系，Ω为调制角速度，角速度扩展量测方程推导过程如下：

其中,与φⁿ分别为由卡尔曼滤波器估计的转移矩阵及失准角向量，b为载体坐标系，n为导航坐标系，为由载体线速度运动引起的角速度向量，在系泊条件下，为等效的量测噪声向量，为载体的运动角速度向量，为陀螺理论输出，为陀螺实际输出，转换式(2)为：

其中，令

步骤2)，卡尔曼滤波模型为：

其中，X为n维状态向量，Φ，G，H是关于时间的确定性矩阵函数，W，V为零均值的高斯白噪声，及是由MEMS及GNSS计算的载体在导航坐标系n中的线速度，

步骤3)，自适应卡尔曼滤波的策略是选择合适的衰落因子进行调整以抑制发散，采用保持残差正交的方法来获得渐消因子，因此，k时刻的新息矢量为：

其中，Z_k为量测量，H_k为量测矩阵，为一次预测状态向量，k时刻新息矢量的协方差阵估计值为：

其中，P_k/k-1为一步预测协方差矩阵，R_k为量测噪声序列方差阵，C_0,k计算方法如下：

其中，b为渐消因子，其取值范围为：0.7～0.95，

步骤4)，状态估计的一步预测均方误差阵P_k/k-1的修正公式为：

其中，λ_k＝diag(s₁,s₂,…s_n)为对角矩阵，s_i≥1,i＝1,2,…n，n为状态向量的维数；为一步转移矩阵；为系统噪声序列方差阵，新息序列互不相关是线性最优卡尔曼滤波的增益矩阵取K_k最优值的必要条件，即：

上式的等价形式为：

多渐消因子自适应卡尔曼滤波的原理即通过选取合适的λ_k使式(10)近似成立，

步骤5)，定义渐消因子搜寻评价函数为：

代入式(8)到式(11)得：

寻优过程即调节渐消因子矩阵λ_k使得矩阵ξ(k)所有元素取最小值，则评价函数可定义为：

其中，n和m分别为矩阵ξ(k)得行数及列数，s_ij(k)是矩阵ξ(k)的元素，k为迭代次数，寻优过程即使f(x)取得最小值，

步骤6)，定义天牛须初始长度d₀及移动步距δ₀，天牛初始位置为：γ＝[s₁,s₂,…s_n]^T＝[1,1,…1]^T,s_i是对角阵λ_k对应的对角元素，其中，i＝1，2，…，n，其评价函数值为：f(γ)，

定义搜寻方向，通过随机函数设定搜寻方向：

其中，τ为搜寻方向，n为状态向量个数，rand为随机向量函数，||·||为求模符号，

步骤7)，计算甲虫下一步左，右移动方向，故左，右触须的位置向量分别是：

其中，γ_l为左触须位置，γ_r为右触须位置，d为甲虫触须长度，

步骤8)，分别计算左右触须的评价函数值f(γ_l)及f(γ_r)，

步骤9)，计算天牛下一步位置：

其中，δ为移动步距，sgn为符号函数，

步骤10)，计算步骤9)的评价函数

步骤11)，更新天牛新的下一步全局位置，触须长度及移动步距：

更新下一步全局位置：若

更新触须长度：d＝0.98d+0.012，

更新移动步距：δ＝0.98δ，

步骤12)，如果达到最大迭代步数或计算精度满足要求，则结束循环并输出最终渐消因子，输出参数为：λ_k(i,i)＝γ(i)，其中，i＝1，2，…，n，否则返回步骤7)并开始下一个循环。