[发明专利]一种惯导系统单位置初始对准最优方法

权利要求书

1.一种惯导系统单位置初始对准最优方法，其特征在于，包括下述步骤：

步骤一：

外部输入和和为重力矢量在体坐标系和参考坐标系下的测量值，

步骤二：

取α_i为权重系数，且共n个系数，对应n组重力矢量测量值，α_i的值由外部输入

步骤三：

通过以下公式，计算各中间变量，

S＝B+B^T (4)

其中tr表示矩阵的迹，×表示反对称阵，即把一个向量变成3乘3的矩阵；

步骤四：

根据式(1)至(4)，计算如下损失函数构造矩阵

I为单位对角阵；

步骤五：

计算K的特征值λ_i和特征向量q_i，其中i＝1,2,3,4，

步骤六：

K的所有特征值中最大的一个λ_max，λ_max理论值为1，因此可使用1作为初值，进行迭代计算求取λ_max，将λ_max,0＝1带入式(6)进行迭代，得：

λ_max,i+1＝λ_max,i-p(λ_i)/p′(λ_i),i＝0,1,...(6)

其中

步骤七：

为保证迭代收敛性，令：

[(λ+σ)I-S]^-1＝γ^-1(αI+βS+S²) (8)

可得α＝λ²-σ²+κ，β＝λ-σ，γ＝α(λ+σ)-Δ；其中κ＝tr(adj(S))，Δ＝|S|，tr表示矩阵的迹，adj表示伴随矩阵

将α、β和γ代入式(12)可得：

p(λ)＝λ⁴-(a+b)λ²-cλ+(ab+cσ-d)＝0 (9)

其中a＝σ²-κ，

由此可得：

由此可得姿态最优解为：

其中：

2.如权利要求1所述的一种惯导系统单位置初始对准最优方法，其特征在于：还包括下述步骤，

步骤八：

姿态余弦矩阵A与姿态四元数字的转换关系为

其中

为实现姿态对最优估计，定义如下损失函数：

则有：

g(A)＝1-L(A)＝tr[AB^T] (15)

为实现损失函数最小，只需满足g(A)最大即可，

将(12)带入(15)可得：

四元数的元素具有唯一约束

为求取式(16)在式(17)约束条件下对最大值，重新构建方程：

令可得

由上式分析可知，λ是K的一个特征根，则是对应的特征向量，因此(11)的结果是一种最优估计。