[发明专利]解析式粗对准中捷联姿态矩阵的正交化修正方法

权利要求书

1.一种解析式粗对准中捷联姿态矩阵的正交化修正方法，其特征是：

步骤1：解析式粗对准测量得到没有经过正交化的捷联姿态矩阵C₀，其中，捷联姿态矩阵的9个元素为

C0=C0(11)C0(12)C0(13)C0(21)C0(22)C0(23)C0(31)C0(32)C0(33)]]>

步骤2：将矩阵C₀写成行向量的形式，并用表示，即

C‾0(1)=C0(11)C0(12)C0(13)]]>

C‾0(2)=C0(21)C0(22)C0(23)]]>

C‾0(3)=C0(31)C0(32)C0(33)]]>

步骤3：利用步骤2中得到的得到中间测量值

T‾0(1)=C‾0(2)×C‾0(3)+C‾0(1)2]]>

T‾0(2)=C‾0(3)×C‾0(1)+C‾0(2)2]]>

T‾0(3)=C‾0(1)×C‾0(2)+C‾0(3)2]]>

步骤4：利用步骤3，得到的模为(i＝1，2，3)，即

|T‾0(1)|=T0(11)2+T0(12)2+T0(13)2]]>

|T‾0(2)|=T0(21)2+T0(22)2+T0(23)2]]>

|T‾0(3)|=T0(31)2+T0(32)2+T0(33)2]]>

其中T‾0(1)=T0(11)T0(12)T0(13);]]>

T‾0(2)=T0(21)T0(22)T0(23);]]>

T‾0(3)=T0(31)T0(32)T0(33)]]>

步骤5：利用步骤3和4，得到经过修正的三个行向量

C‾1(1)=T‾0(1)|T‾0(1)|]]>

C‾1(2)=T‾0(2)|T‾0(2)|]]>

C‾1(3)=T‾0(3)|T‾0(3)|]]>

步骤6：进一步得到经过修正的捷联姿态矩阵C₁

C1=C‾1(1)C‾1(2)C‾1(3)]]>

其中，捷联姿态矩阵的9个元素为

C1=C1(11)C1(12)C1(13)C1(21)C1(22)C1(23)C1(31)C1(32)C1(33)]]>

步骤7：利用步骤6中得到的第1次修正的捷联姿态矩阵C₁与没有经过正交化的捷联姿态矩阵C₀测量得到测距函数值

f1=Σi=13Σj=13(C1(ij)-C0(ij))2]]>

其中C₀(ij)为C₀的第i行、第j列元素，C₁(ij)为C₁的第i行、第j列元素；

步骤8：判断下述条件

|f₁-f₀|≤0.00000003

其中f₀为初始测距函数，并且有f₀＝0，

如果满足该条件，执行步骤9，

如果不满足该条件，用第1次修正的捷联姿态矩阵C₁替换没有经过正交化的捷联姿态矩阵C₀，再次执行步骤2至7，并执行步骤8中的判断条件，如此往复迭代测量，当进行了k次修正得到C_k，并有第k次修正后测距函数值满足|f_k-f_k-1|≤0.00000003时，执行步骤9；

步骤9：由第k次修正后的捷联姿态矩阵C_k

Ck=Ck(11)Ck(12)Ck(13)Ck(21)Ck(22)Ck(23)Ck(31)Ck(32)Ck(33)]]>

测量得到载体的航向角、纵摇角和横摇角，完成解析式粗对准测量任务。