| 主权项 |
1.一种基于室内主导方向的航向误差测量方法，其特征在于包括以下步骤：步骤一：采集固定于移动体脚面的微惯性测量单元输出的量测信息，包括陀螺输出的角速度w<sub>ib</sub>、加速度计输出的比力f和电子罗盘输出的航向<img file="FDA0000487534390000011.GIF" wi="135" he="65" />步骤二：根据步骤一中得到的角速度w<sub>ib</sub>得到移动体航向<img file="FDA0000487534390000012.GIF" wi="152" he="59" />步骤三：根据步骤一中得到的比力f得到移动体位置p；步骤四：根据步骤三中得到的移动体位置p得到移动体步幅航向θ，其量测方程为：<maths num="0001"><![CDATA[<math><mrow><mi>θ</mi><mo>=</mo><mi>arctan</mi><mrow><mo>(</mo><mfrac><mrow><msub><mi>p</mi><mrow><mi>E</mi><mo>,</mo><mi>m</mi></mrow></msub><mo>-</mo><msub><mi>p</mi><mrow><mi>E</mi><mo>,</mo><mi>m</mi><mo>-</mo><mn>1</mn></mrow></msub></mrow><mrow><msub><mi>p</mi><mrow><mi>N</mi><mo>,</mo><mi>m</mi></mrow></msub><mo>-</mo><msub><mi>p</mi><mrow><mi>N</mi><mo>,</mo><mi>m</mi><mo>-</mo><mn>1</mn></mrow></msub></mrow></mfrac><mo>)</mo></mrow></mrow></math>]]></maths>其中，arctan为反正切函数，(p<sub>E,m</sub> p<sub>N,m</sub>)为第m步单兵在东向和北向的位置坐标，(p<sub>E,m-1</sub> p<sub>N,m-1</sub>)为第m-1步移动体在东向和北向的位置坐标；步骤五：根据步骤一中得到的航向<img file="FDA0000487534390000014.GIF" wi="108" he="65" />和步骤二中得到的移动体航向<img file="FDA0000487534390000015.GIF" wi="121" he="59" />得到航向偏差<img file="FDA0000487534390000016.GIF" wi="110" he="68" />第m步的航向偏差<img file="FDA0000487534390000017.GIF" wi="148" he="69" />量测方程为：<img file="FDA0000487534390000018.GIF" wi="858" he="141" />式中，<img file="FDA0000487534390000019.GIF" wi="153" he="69" />为移动体在第m步的航向误差，<img file="FDA00004875343900000110.GIF" wi="177" he="77" />为移动体在第k步的电子罗盘输出，<img file="FDA00004875343900000111.GIF" wi="194" he="71" />为由步骤四得到的移动体在第k步的航向，ε为零均值高斯白噪声；步骤六：根据步骤四得到的步幅航向θ和步骤五得到航向偏差<img file="FDA00004875343900000112.GIF" wi="80" he="67" />得到修正的步幅航向θ<sub>1</sub>，其量测方程为：<img file="FDA00004875343900000113.GIF" wi="245" he="71" />步骤七：根据移动体轨迹确定室内主导方向<img file="FDA00004875343900000114.GIF" wi="74" he="57" />根据步骤六得到修正的步幅航向θ<sub>1</sub>和室内主导方向<img file="FDA00004875343900000115.GIF" wi="48" he="57" />得到系统状态变量Δθ，其测量方程为：<img file="FDA00004875343900000116.GIF" wi="236" he="69" />利用卡尔曼滤波器对状态变量进行估计，得到航向误差Δθ。 |
