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

摘要

本发明解析式粗对准中捷联姿态矩阵的正交化修正方法，包括：利用重力加速度和地球自转角速度确定三个三维向量；从三个三维向量确定测量得到包含不正交误差的捷联姿态矩阵，进一步将包含不正交误差的捷联姿态矩阵写成行向量的形式，进行正交化和归一化的处理，得到修正的捷联姿态矩阵，根据预设的条件判断修正的捷联姿态矩阵是否满足精度要求，满足则结束，不满足则重复迭代运算直到满足判断条件。本发明解决了捷联姿态矩阵不正交化对于解析式粗对准测量的影响，可提高载体姿态测量精度。

主权项

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_kCk=Ck(11)Ck(12)Ck(13)Ck(21)Ck(22)Ck(23)Ck(31)Ck(32)Ck(33)]]>测量得到载体的航向角、纵摇角和横摇角，完成解析式粗对准测量任务。