专利名称：：硅微航姿系统惯性/地磁组合方法
技术领域：
：本发明涉及一种测量方法，特别是涉及一种捷联航姿系统的组合导航方法，尤其涉及一种硅微航姿系统惯性/地磁组合导航方法。
背景技术：
：硅微航姿系统包括由三个硅微陀螺仪、三个硅微加速度计和三轴磁强计组成的微惯性测量单元(MIMU)和导航计算机，是一种捷联式惯性组合导航系统，硅微陀螺仪和加速度计直接固连在运载体上，测量出载体的旋转运动角速率和线运动加速度信息(包括重力加速度)，然后送至导航计算机中进行实时的姿态矩阵解算，并从捷联姿态矩阵的有关元素中计算出载体的姿态角。硅微加速度计及硅微陀螺是基于MEMS技术的一种新型惯性传感器，目前其精度相对常规惯性器件要低很多，陀螺的漂移和噪声水平要高于地球的自转速率，因此目前航姿系统大多采用微惯性器件与磁强计组合使用的方案。硅微航姿系统用加速度计和磁强计测量的重力向量和地磁向量为参考信息通过Kalman组合滤波来补偿陀螺的漂移、修正陀螺测量的载体运动角速度积分后的角度漂移，确定的载体姿态信息既有长期的稳定精度又具有载体动态运动的快速实时性，明显的提高了航姿系统的精度、增强了系统的鲁棒性。因此如何构建一种更加充分利用加速度计、磁强计和陀螺测量信息的Kalman组合滤波方法对于实现硅微航姿系统具有重要的意义。在已报道和所能査阅到的国内外相关文献中，给出文献综合分析结果由于国外的硅微惯性技术发展得较为成熟，目前研究内容均集中在硅微惯性组合系统在各行业的应用研究，而国外公司的产品技术出于商业保密原因，并未见诸详细报道；而国内的相关研究则大多局限在系统的各项具体技术的理论研究上，如惯性器件的测试、误差建模与补偿、矩阵更新算法等。
发明内容本发明的目的是提供一种能够有效抑制载体机动影响和地磁干扰的硅微航姿系统惯性/<formula>formulaseeoriginaldocumentpage9</formula>重力向量fl和地磁向量m在地理坐标系"系的投影a"=<formula>formulaseeoriginaldocumentpage9</formula>地磁组合方法。为了达到上述的发明目的，本发明包括下列步骤(1)利用微惯性测量组件MIMU中的硅微传感器感应载体运动硅微陀螺敏感载体运动沿其轴向的角速度信号，硅微加速度计敏感载体运动沿其轴向的加速度及重力加速度信号；利用MIMU中的磁强计敏感地磁向量，并将信号送至导航计算机。(2)利用加速度计和磁强计对重力向量和地磁向量的观测来确定载体的初始姿态为已知常量，|"为地磁场在当地"系的投影向量，可根据经纬度和海拔高度由国际参考地磁场查得。在载体坐标系6系中的投影？和m6，可由加速度计和磁强计测得。载体坐标系可通过导航坐标系(地理坐标系"系)按航向角(y)、俯仰角(-)、横滚角(e)三次转动确定，如附图2所示。构造三个正交向量gf=<formula>formulaseeoriginaldocumentpage9</formula>则它们在w系中的投影构成矩阵[《";r";s"]，可由圹和附"求得；在6系中的投影构成矩阵[y;/;s，，可由^和i^求得。<formula>formulaseeoriginaldocumentpage9</formula>/=c、,，所以因为<formula>formulaseeoriginaldocumentpage9</formula>由此，可求出姿态矩阵《的估计^。具体地根据下面的两个公式即可求得Kalman滤波器的初值&0)。<formula>formulaseeoriginaldocumentpage10</formula>(3)利用加速度计和磁强计对重力向量和地磁向量的观测来修正陀螺给出的姿态角。具体地，把这两个参考向量在"系中的理论值通过陀螺给出的姿态角投影到6系中，与加速度计和磁强计的测量值求差作为观测量来完成对姿态的修正。本步骤包含如下步骤①建立卡尔曼滤波的状态方程<formula>formulaseeoriginaldocumentpage10</formula>式中，ATO)是f时刻系统的状态向量；F(O、g(,)分别为系统状态矩阵和噪声矩阵；『0)为系统的噪声向量。系统的状态矢量为<formula>formulaseeoriginaldocumentpage10</formula>(4)系统的状态噪声向量为<formula>formulaseeoriginaldocumentpage10</formula>其中，9e=kel&&3]T为姿态四元数向量估计误差'A^=[A^A化丫为陀螺零偏估计误差，^0)=(^为陀螺误差噪声，JT2(0=(fiVwgzf为陀螺零偏估计误差随机游走模型的零均值白噪声。{『(0}为独立零均值高斯白噪声过程，其协方差矩阵其中，,<formula>formulaseeoriginaldocumentpage10</formula>为对角阵。q的对角线元素根据陀螺噪声来确定，工程上可近似取为噪声RMS值的平方除以带宽/'系统量测向量为<formula>formulaseeoriginaldocumentpage11</formula>观测向量<formula>formulaseeoriginaldocumentpage11</formula>式中，AA为三轴加速度计输出的加速度。<formula>formulaseeoriginaldocumentpage11</formula>观湖!1向量^^=5:附"，^i^-附A—<formula>formulaseeoriginaldocumentpage11</formula>式中，m,^m,为三轴磁强计输出的磁强c观测噪声向量为<formula>formulaseeoriginaldocumentpage11</formula>{^(^为独立均值高斯白噪声过程，其协方差矩阵:<formula>formulaseeoriginaldocumentpage11</formula>込的对角线元素根据陀螺零位漂移的特点来确定<系统的噪声系数矩阵为G(0=系统的状态转移矩阵为^(0=上式中，A=[5X&为载体角速度向量在载体坐标系中的估计值问=为向量&的反对称矩阵，。②根据加速度计测量值判断系统加速运动情况和磁强计测量值判断地磁受干扰情况，如果重力向量和地磁向量没有受到干扰，转到歩骤③；如果重力向量或地磁向量受到干扰，转到步骤④；③建立卡尔曼滤波的观测方程，并转到步骤⑤<formula>formulaseeoriginaldocumentpage11</formula>(8)式中，Z(/)为/时刻系统的量测向量；W(f)为系统量测矩阵；F(/)为系统的量测噪声向及屏为对角阵，J^和及^的对角线元素分别根据加速度计和磁强计的噪声上式中，A0》(11)(12)来确定，工程上可近似取为噪声RMS值的平方&=(薦J2./3x3，4=(歸呵)2系统观测矩阵为06x3]-2[^]—构造重力向量和地磁向量的法向量/=ax附重力向量、地磁向量和两者叉乘构造的法向量如附图3所不。在"系中投影的法向量理论值为l""=fl"XW"，可以由fl"和!《"求得；在6系中法向量的测量值为/=6><16，可以由沪和m^求得；建立卡尔曼滤波的观测方程式中，Z(0为^时刻系统的量测向量；W(f)为系统量测矩阵；K(O为系统的量测噪声向(a)地磁向量受到长期干扰的情况-系统量测向量为Z=观测向量^5,=/—观测噪声向量为FO)=，)F"O为法向量的测量噪声，与K(0和^0)有关(F(^为独立均值高斯白噪声过程，其协方差矩阵及屏03x3及d3.为对角阵系统观测矩阵为0fe3]_2[,]_(b)重力向量受到载体长期机动运动干扰的情况上式中<formula>formulaseeoriginaldocumentpage12</formula>系统量测向量为Z=观测噪声向量为F0)<formula>formulaseeoriginaldocumentpage13</formula>(9)<formula>formulaseeoriginaldocumentpage13</formula>(10)(F(W为独立均值高斯白噪声过程，其协方差矩阵为对角阵上式中，<formula>formulaseeoriginaldocumentpage13</formula>(12)系统观测矩阵为<formula>formulaseeoriginaldocumentpage13</formula>根据扩展卡尔曼滤波递推方程和步骤①、③或④的结果，建立卡尔曼滤波的时间传播方程式中06=[%^A]"为当前时刻三轴陀螺的输出；为上一时刻更新后的陀螺零偏估计；Af为计算步长。<formula>formulaseeoriginaldocumentpage13</formula>式中，f5分别表示四元数向量，|705表示四元数乘法，具体为<formula>formulaseeoriginaldocumentpage13</formula>⑥根据扩展卡尔曼滤波递推方程和歩骤①、②的结果，建立卡尔曼滤波的测量修正方<formula>formulaseeoriginaldocumentpage13</formula>(14)<formula>formulaseeoriginaldocumentpage13</formula>(15)<formula>formulaseeoriginaldocumentpage14</formula>S『)经过补偿更新后需进行规一化处理。6(,+)=o/o)-》o+)(4)输出载体姿态参数由步骤(3)估计得到的姿态四元数^即可由公式2求得姿态矩阵^，姿态角可以由姿态矩阵f:的元素计算得出，具体步骤航向角主值俯仰角主值横滚角主值^主=tan画^主=一sin—1C13由上述主值按照如下公式判断真值<formula>formulaseeoriginaldocumentpage14</formula>本发明使用的硅微航姿系统中硬件包括微惯性测量单元(简称MIMU,包括三个相互垂直的硅微陀螺、三个相互垂直的硅微加速度计和三个相互垂直的磁强计)、导航计算机，具体硬件组成如附图l所示；MIMU的测量坐标系(为载体坐标系6系)与地理坐标系n系的关系及载体姿态角如附图2所示。本发明的方法具有以下优点(1)航姿系统体积小、重量轻；(2)具有完全的自主性，不需要外部输入信息；(3)受载体机动和地磁干扰的影响小，能够有效提高航姿系统的输出姿态精度；(4)航姿系统可以为其它装置提供每秒50次以上的实时姿态信号。对本发明的有益效果说明如下(1)航姿系统仿真实验航姿系统置于姿态零位，从第10秒到第20秒，在南北方向加入0.1M(M为地磁场的垂直分量)的磁场偏置；从第35秒到第45秒，在东西方向加入0.1M的磁场偏置，如附图4a所示。系统采用重力向量和地磁向量作为参考向量的姿态仿真结果如附图4c所示。由于地磁场受到干扰，利用本发明所述方法的系统采用重力向量和法向量作为参考向量的姿态仿真结果如附图4d所示。由图4b可见法向量的方向总保持水平，因此图4d中没有产生图4c的水平姿态的影响，本发明所述方法可以减小地磁场干扰对航姿系统姿态的影响。(2)航姿系统的三轴转台实验将釆用本发明研制的硅微航姿系统安装在三轴转台上进行静态和三轴摇摆试验，所用硅微航姿系统的器件精度如下硅微陀螺零位稳定性57s硅微陀螺刻度因数线性度0.1%硅微陀螺刻度因数温度系数0.1%/°C硅微陀螺噪声<0.1°/S/V^硅微加速度计零位稳定性0.02m/s2硅微加速度计刻度因数线性度0.2%硅微加速度计刻度因数稳定性0.05m/s2(-25°C+80°C)硅微加速度计噪声0.001m/s2/V^磁强计零位稳定性0.5mGauss磁强计刻度因数线性度2%磁强计刻度因数稳定性0.5mGauss磁强计噪声0.5mGauss利用发明所述方法得到三轴转台指北、水平静止状态硅微航姿系统航向角、俯仰角、横滚角曲线分别如附图5a、图5b和图5c所示；三轴摇摆状态航向角、俯仰角、横滚角曲线分别如附图6a、图6b和图6c所示。结果表明硅微航姿系统具有较好的精度。(3)航姿系统的跑车实验动态跑车实验在一段较为平直的公路上进行，车辆的姿态大致水平(其中一段较为颠簸)，航向角基本保持为315。，跑车过程包括匀速运动(时速40km/h)、加速运动、减速运动过程。以车辆上另外一套高精度光纤捷联组合惯导系统(PHINS)提供的姿态输出作为参考，得到了跑车过程中硅微航姿系统的航向姿态。颠簸路段横滚角曲线图7a所示，平坦路段横滚角曲线图7b所示，俯仰角、航向角曲线分别如图8、图9所示。动态跑车实验结果表明，航姿系统利用本发明的方法在动态跑车实验中达到了实用的水平。图1为本发明的硅微航姿系统惯性/地磁组合原理图；图2为载体坐标系、(b系)与地理坐标系(n系)的关系图3为重力向量、地磁向量与法向量的关系图4a-图4d为地磁场受干扰的航姿系统仿真结果图5a-图5c为静态状态航姿系统测试结果图6a-图6c为摇摆状态航姿系统测试结果图7a-图7b为动态跑车航姿系统横滚角测试结果图8为动态跑车航姿系统俯仰角测试结果图9为动态跑车航姿系统航向角测试结果图10为本发明的硅微航姿系统软件流程图。具体实施例方式下面举例对本发明作更详细地描述，本实施例包括以下步骤(1)利用微惯性测量组件MIMU中的硅微传感器感应载体运动硅微陀螺敏感载体运动沿其轴向的角速度信号，硅微加速度计敏感载体运动沿其轴向的加速度及重力加速度信号；利用MIMU中的磁强计敏感地磁向量，并将信号送至导航计算机。(2)利用加速度计和磁强计对重力向量和地磁向量的观测来确定载体的初始姿态<formula>formulaseeoriginaldocumentpage16</formula>重力向量fl和地磁向量m在地理坐标系w系的投影a"=<formula>formulaseeoriginaldocumentpage16</formula>m"为地磁场在当地n系的投影向量，可根据经纬度和海拔高度由国际参考地磁场查得。在载体坐标系6系中的投影Z和附6，可由加速度计和磁强计测得。载体坐标系可通过导航坐标系(地理坐标系"系)按航向角(y)、俯仰角(0)、横滚角(e)三次转动确定，如附图2所示。构造三个正交向量《=a/=ax/n贝ij它们在"系中的投影构成矩阵iy;/"";s"]，可由《"和附"求得；在6系中的投影构成矩阵[y可由fl6和挑6求得。因为<formula>formulaseeoriginaldocumentpage17</formula>所以由此，可求出姿态矩阵《的估计《,器的初值S(o)。<formula>formulaseeoriginaldocumentpage17</formula>(i)具体地根据下面的两个公式即可求得Kalman滤波<formula>formulaseeoriginaldocumentpage17</formula>(2)(3)利用加速度计和磁强计对重力向量和地磁向量的观测来修正陀螺给出的姿态角。具体地，把这两个参考向量在"系中的理论值通过陀螺给出的姿态角投影到6系中，与加速度计和磁强计的测量值求差作为观测量来完成对姿态的修正。本步骤包含如下步骤①建立卡尔曼滤波的状态方程<formula>formulaseeoriginaldocumentpage17</formula>(3)式中，X(0是^时刻系统的状态向量；F(,)、G(O分别为系统状态矩阵和噪声矩阵；W(O为系统的噪声向量。系统的状态矢量为<formula>formulaseeoriginaldocumentpage17</formula>(4)系统的状态噪声向量为<formula>formulaseeoriginaldocumentpage17</formula>(5)其中，<formula>formulaseeoriginaldocumentpage17</formula>为姿态四元数向量估计误差<formula>formulaseeoriginaldocumentpage17</formula>为陀螺零偏估计误差，为陀螺误差噪声，『2(0=(^)为陀螺零偏估计误差随机游走模型的零均值白噪声。{^^"为独立零均值!#斯白噪声过程，其协方差矩阵-<formula>formulaseeoriginaldocumentpage18</formula>其中，g(0=-03x3込似取为噪声RMS值的平方除以带宽/':a=[(iMv。)2//']./3x3fc的对角线元素根据陀螺零位漂移的特点来确定系统的噪声系数矩阵为G(0=l_为对角阵。g的对角线元素根据陀螺噪声来确定，工程上可近<formula>formulaseeoriginaldocumentpage18</formula>系统的状态转移矩阵为尸(0=上式中，&=&A了为载体角速度向量在载体坐标系中的估计值(6)<formula>formulaseeoriginaldocumentpage18</formula>为向量6的反对称矩阵，。②根据加速度计测量值判断系统加速运动情况和磁强计测量值判断地磁受干扰情况，如果重力向量和地磁向量没有受到干扰，转到步骤③；如果重力向量或地磁向量受到干扰，转到步骤④；③建立卡尔曼滤波的观测方程，并转到步骤⑤Z(0二则外)+K0)(8)式中，Z(0为/时刻系统的量测向量；开(0为系统量测矩阵；"(O为系统的量测噪声向系统量测向量为Z=(9)观测向量<formula>formulaseeoriginaldocumentpage18</formula>式中，&^",为三轴加速度计输出的加速度。<formula>formulaseeoriginaldocumentpage19</formula>式中，^为三轴磁强计输出的磁强c观测噪声向量为<formula>formulaseeoriginaldocumentpage19</formula>(F(^为独立均值高斯白噪声过程，其协方差矩阵<formula>formulaseeoriginaldocumentpage19</formula>为对角阵，J^和/^的对角线元素分别根据加速度计和磁强计的噪声<formula>formulaseeoriginaldocumentpage19</formula>来确定，工程上可近似取为噪声RMS值的平方系统观测矩阵为<formula>formulaseeoriginaldocumentpage19</formula>上式中，(11)(12)构造重力向量和地磁向量的法向量重力向量、地磁向量和两者叉乘构造的法向量如附图3所示。在"系中投影的法向量理论值为r"=fl"xi"，可以由和m"求得；在6系中法向量的测量值为/="*乂附*，可以由^和挑*求得；建立卡尔曼滤波的观测方程则=聰竭+，式中，Z()为f时刻系统的量测向量；H(/)为系统量测矩阵；F(/)为系统的量测噪声向(a)地磁向量受到长期干扰的情况:系统量测向量为Z观测向..观测噪声向量为F(O<formula>formulaseeoriginaldocumentpage19</formula>K"O为法向量的测量噪声，与^(r)和F^)有关^F(/》为独立均值高斯白噪声过程，其协方差矩阵:￡{K(0「r(r)}=id(,)^-r)义0,03x3及rf3.为对角阵上式中，^(0=系统量测向量为Z=系统观测矩阵为"(f)=[A(006x3]—2[一]—(b)重力向量受到载体长期机动运动干扰的情况观测噪声向量为H>)={^(/)}为独立均值高斯白噪声过程，其协方差矩阵为对角阵上式中，(12)(9)(10)(12)系统观测矩阵为tf(/)=[A(006x3]—2[,]—⑤根据扩展卡尔曼滤波递推方程和步骤①、③或④的结果，建立卡尔曼滤波的时间传播方程式中《/=[&^了为当前时刻三轴陀螺的输出；》(f-A0+为上一时刻更新后的陀螺零偏估计；A/为计算步长。一0丄1—1—^式中，f6分别表示四元数向量，丄f(8^表示四元数乘法，具体为"12<formula>formulaseeoriginaldocumentpage21</formula>⑥根据扩展卡尔曼滤波递推方程和步骤①、②的结果，建立卡尔曼滤波的测量修正方<formula>formulaseeoriginaldocumentpage21</formula>^f+)经过补偿更新后需进行规一化处理。S0+)=w*(r)_~+)(4)输出载体姿态参数由步骤(3)估计得到的姿态四元数^即可由公式2求得姿态矩阵^，姿态角可以由姿态矩阵f!的元素计算得出，具体步骤航向角主值俯仰角主值横滚角主值<formula>formulaseeoriginaldocumentpage21</formula>由上述主值按照如下公式判断真值:<formula>formulaseeoriginaldocumentpage21</formula><formula>formulaseeoriginaldocumentpage22</formula>如图10所示，本实施例的工作流程如下(1)系统初始化硅微航姿系统在静态条件下正常启动。首先进行系统的初始化，包括导航计算机硬件自检，各接口初始化，对MIMU信号的检测，相应状态的设置，以及卡尔曼滤波器的初始化等。(2)初始对准静基座下，由MIMU输出信号经器件误差补偿后使用公式1和公式2解算出四元数初值"0)与初始姿态矩阵^"6(0)。(3)器件误差补偿定时采集到MIMU输出信号后，根据采集的环境温度和器件的温度误差模型完成器件的温度误差补偿；补偿器件的非正交安装误差、零位误差、刻度因数误差等。(4)姿态矩阵更新由经过误差补偿的陀螺测量角速度信号求解四元数姿态方程，根据姿态算法中的四元数方法完成四元数更新与姿态矩阵更新。(5)重力、地磁干扰判断判断重力、地磁向量是否被干扰，根据重力、地磁向量被干扰情况选取卡尔曼滤波器观测向量，建立相应的观测方程。(6)姿态组合卡尔曼滤波模块根据公式3公式16，建立卡尔曼滤波器。(7)四元数与陀螺零位误差修正由卡尔曼滤波估计出的陀螺零位误差偏差和四元数误差及时修正陀螺零位误差和四元数，以便在下一循环中分别应用到流程(3)和流程(4)。(8)四元数归一化处理将上一流程(7)得到的修正后的四元数进行归一化处理。(9)航姿角计算将上一流程(8)经过归一化处理的四元数使用公式2得出姿态矩阵估计^\再使用步骤(4)可以计算出航向姿态角。(10)航姿角数据输出系统根据外部设备的需要，设置航姿信息的格式、输出频率以及校验方式等。权利要求1.硅微航姿系统惯性/地磁组合方法，其特征是(1)利用微惯性测量组件MIMU中的硅微传感器感应载体运动硅微陀螺敏感载体运动沿其轴向的角速度信号，硅微加速度计敏感载体运动沿其轴向的加速度及重力加速度信号，利用MIMU中的磁强计敏感地磁向量，并将信号送至导航计算机；(2)利用加速度计和磁强计对重力向量和地磁向量的观测来确定载体的初始姿态重力向量a和地磁向量m在地理坐标系n系的投影<mathsid="math0001"num="0001"><math><![CDATA[<mrow><msup><mi>a</mi><mi>n</mi></msup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mi>g</mi></mtd></mtr></mtable></mfenced></mrow>]]></math>id="icf0001"file="A2008100647140002C1.tif"wi="14"he="17"top="72"left="121"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>和<mathsid="math0002"num="0002"><math><![CDATA[<mrow><msup><mi>m</mi><mi>n</mi></msup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msubsup><mi>m</mi><mrow><mn>0</mn><mi>x</mi></mrow><mi>n</mi></msubsup></mtd></mtr><mtr><mtd><msubsup><mi>m</mi><mrow><mn>0</mn><mi>y</mi></mrow><mi>n</mi></msubsup></mtd></mtr><mtr><mtd><msubsup><mi>m</mi><mrow><mn>0</mn><mi>z</mi></mrow><mi>n</mi></msubsup></mtd></mtr></mtable></mfenced></mrow>]]></math>id="icf0002"file="A2008100647140002C2.tif"wi="19"he="19"top="71"left="141"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>为已知常量，mn为地磁场在当地地理坐标系n系中的投影向量，在载体坐标系b系中的投影为ab和mb，构造三个正交向量q＝ar＝a×ms＝r×a它们在n系中的投影构成矩阵[id="icf0003"file="A2008100647140002C3.tif"wi="14"he="5"top="126"left="86"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>]，由an和mn求得；在b系中的投影构成矩阵[id="icf0004"file="A2008100647140002C4.tif"wi="14"he="5"top="135"left="23"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>]，由ab和mb求得，<mathsid="math0003"num="0003"><math><![CDATA[<mrow><mfencedopen='{'close=''><mtable><mtr><mtd><msup><mi>q</mi><mi>b</mi></msup><mo>=</mo><msubsup><mi>C</mi><mi>n</mi><mi>b</mi></msubsup><mo>·</mo><msup><mi>q</mi><mi>n</mi></msup></mtd></mtr><mtr><mtd><msup><mi>r</mi><mi>b</mi></msup><mo>=</mo><msubsup><mi>C</mi><mi>n</mi><mi>b</mi></msubsup><mo>·</mo><msup><mi>r</mi><mi>n</mi></msup></mtd></mtr><mtr><mtd><msup><mi>s</mi><mi>b</mi></msup><mo>=</mo><msubsup><mi>C</mi><mi>n</mi><mi>b</mi></msubsup><mo>·</mo><msup><mi>s</mi><mi>n</mi></msup></mtd></mtr></mtable></mfenced><mo>,</mo></mrow>]]></math></maths><mathsid="math0004"num="0004"><math><![CDATA[<mrow><mrow><mo>[</mo><msup><mi>q</mi><mi>b</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>r</mi><mi>b</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>s</mi><mi>b</mi></msup><mo>]</mo></mrow><mo>=</mo><msubsup><mi>C</mi><mi>n</mi><mi>b</mi></msubsup><mo>·</mo><mo>[</mo><msup><mi>q</mi><mi>n</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>r</mi><mi>n</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>s</mi><mi>n</mi></msup><mo>]</mo></mrow>]]></math></maths><mathsid="math0005"num="0005"><math><![CDATA[<mrow><msubsup><mi>C</mi><mi>n</mi><mi>b</mi></msubsup><mo>=</mo><mo>[</mo><msup><mi>q</mi><mi>b</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>r</mi><mi>b</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>s</mi><mi>b</mi></msup><mo>]</mo><mo>·</mo><msup><mrow><mo>[</mo><msup><mi>q</mi><mi>n</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>r</mi><mi>n</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>s</mi><mi>n</mi></msup><mo>]</mo></mrow><mrow><mo>-</mo><mn>1</mn></mrow></msup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></mrow>]]></math></maths>由此，求出姿态矩阵Cnb的估计id="icf0008"file="A2008100647140002C8.tif"wi="6"he="4"top="185"left="83"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>根据下面的两个公式求得Kalman滤波器的初值id="icf0009"file="A2008100647140002C9.tif"wi="8"he="4"top="185"left="175"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/><mathsid="math0006"num="0006"><math><![CDATA[<mrow><mo>|</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>01</mn></msub><mo>|</mo><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><msqrt><mn>1</mn><mo>+</mo><msub><mi>C</mi><mn>11</mn></msub><mo>-</mo><msub><mi>C</mi><mn>22</mn></msub><mo>-</mo><msub><mi>C</mi><mn>33</mn></msub></msqrt></mrow>]]></math></maths><mathsid="math0007"num="0007"><math><![CDATA[<mrow><mi>sig</mi><mrow><mo>(</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>00</mn></msub><mo>)</mo></mrow><mo>=</mo><mn>1</mn></mrow>]]></math>id="icf0011"file="A2008100647140002C11.tif"wi="18"he="4"top="202"left="103"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths><mathsid="math0008"num="0008"><math><![CDATA[<mrow><mo>|</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>02</mn></msub><mo>|</mo><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><msqrt><mn>1</mn><mo>-</mo><msub><mi>C</mi><mn>11</mn></msub><mo>+</mo><msub><mi>C</mi><mn>22</mn></msub><mo>-</mo><msub><mi>C</mi><mn>33</mn></msub></msqrt></mrow>]]></math>id="icf0012"file="A2008100647140002C12.tif"wi="44"he="8"top="204"left="49"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths><mathsid="math0009"num="0009"><math><![CDATA[<mrow><mi>sig</mi><mrow><mo>(</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>01</mn></msub><mo>)</mo></mrow><mo>=</mo><mi>sign</mi><mrow><mo>(</mo><msub><mi>C</mi><mn>32</mn></msub><mo>-</mo><msub><mi>C</mi><mn>23</mn></msub><mo>)</mo></mrow></mrow>]]></math>id="icf0013"file="A2008100647140002C13.tif"wi="40"he="4"top="208"left="103"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths><mathsid="math0010"num="0010"><math><![CDATA[<mrow><mo>|</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>03</mn></msub><mo>|</mo><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><msqrt><mn>1</mn><mo>-</mo><msub><mi>C</mi><mn>11</mn></msub><mo>-</mo><msub><mi>C</mi><mn>22</mn></msub><mo>+</mo><msub><mi>C</mi><mn>33</mn></msub></msqrt></mrow>]]></math>id="icf0014"file="A2008100647140002C14.tif"wi="44"he="8"top="215"left="49"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths><mathsid="math0011"num="0011"><math><![CDATA[<mrow><mi>sig</mi><mrow><mo>(</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>02</mn></msub><mo>)</mo></mrow><mo>=</mo><mi>sign</mi><mrow><mo>(</mo><msub><mi>C</mi><mn>13</mn></msub><mo>-</mo><msub><mi>C</mi><mn>31</mn></msub><mo>)</mo></mrow></mrow>]]></math>id="icf0015"file="A2008100647140002C15.tif"wi="40"he="4"top="214"left="103"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths><mathsid="math0012"num="0012"><math><![CDATA[<mrow><mrow><mo>|</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>00</mn></msub><mo>|</mo></mrow><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><msqrt><mn>1</mn><mo>+</mo><msubsup><mi>q</mi><mn>01</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>02</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>03</mn><mn>2</mn></msubsup></msqrt></mrow>]]></math></maths><mathsid="math0013"num="0013"><math><![CDATA[<mrow><mi>sig</mi><mrow><mo>(</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>03</mn></msub><mo>)</mo></mrow><mo>=</mo><mi>sign</mi><mrow><mo>(</mo><msub><mi>C</mi><mn>21</mn></msub><mo>-</mo><msub><mi>C</mi><mn>12</mn></msub><mo>)</mo></mrow></mrow>]]></math>id="icf0017"file="A2008100647140002C17.tif"wi="40"he="4"top="221"left="103"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>则id="icf0018"file="A2008100647140002C18.tif"wi="110"he="18"top="237"left="36"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>(3)利用加速度计和磁强计对重力向量和地磁向量的观测来修正陀螺给出的姿态角，即把这两个参考向量在n系中的理论值通过陀螺给出的姿态角投影到b系中，与加速度计和磁强计的测量值求差作为观测量来完成对姿态的修正；本步骤包含如下步骤①建立卡尔曼滤波的状态方程<mathsid="math0014"num="0014"><math><![CDATA[<mrow><mover><mi>X</mi><mo>·</mo></mover><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mi>F</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mi>X</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>+</mo><mi>G</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mi>W</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>3</mn><mo>)</mo></mrow></mrow>]]></math></maths>式中，X(t)是t时刻系统的状态向量，F(t)、G(t)分别为系统状态矩阵和噪声矩阵，W(t)为系统的噪声向量；系统的状态矢量为<mathsid="math0015"num="0015"><math><![CDATA[<mrow><mi>X</mi><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>q</mi><mi>e</mi></msub></mtd></mtr><mtr><mtd><mi>ΔB</mi></mtd></mtr></mtable></mfenced><mo>=</mo><msup><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>q</mi><mrow><mi>e</mi><mn>1</mn></mrow></msub></mtd><mtd><msub><mi>q</mi><mrow><mi>e</mi><mn>2</mn></mrow></msub></mtd><mtd><msub><mi>q</mi><mrow><mi>e</mi><mn>3</mn></mrow></msub></mtd><mtd><msub><mi>ΔB</mi><mi>x</mi></msub></mtd><mtd><mi>Δ</mi><msub><mi>B</mi><mi>y</mi></msub></mtd><mtd><mi>Δ</mi><msub><mi>B</mi><mi>z</mi></msub></mtd></mtr></mtable></mfenced><mi>T</mi></msup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>4</mn><mo>)</mo></mrow></mrow>]]></math></maths>系统的状态噪声向量为<mathsid="math0016"num="0016"><math><![CDATA[<mrow><mi>W</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='('close=')'><mtable><mtr><mtd><msub><mi>W</mi><mn>1</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>W</mi><mn>2</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>=</mo><msup><mfencedopen='('close=')'><mtable><mtr><mtd><msub><mi>ω</mi><mi>rx</mi></msub></mtd><mtd><msub><mi>ω</mi><mi>ry</mi></msub></mtd><mtd><msub><mi>ω</mi><mi>rz</mi></msub></mtd><mtd><msub><mi>ω</mi><mi>gx</mi></msub></mtd><mtd><msub><mi>ω</mi><mi>gy</mi></msub></mtd><mtd><msub><mi>ω</mi><mi>gz</mi></msub></mtd></mtr></mtable></mfenced><mi>T</mi></msup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>5</mn><mo>)</mo></mrow></mrow>]]></math></maths>其中，qe＝[qe1qe2qe3]T为姿态四元数向量估计误差，ΔB＝[ΔBxΔByΔBz]T为陀螺零偏估计误差，W1(t)＝(ωrxωryωrz)T为陀螺误差噪声，W2(t)＝(ωgxωgyωgz)T为陀螺零偏估计误差随机游走模型的零均值白噪声；{W(t)}为独立零均值高斯白噪声过程，其协方差矩阵E{W(t)WT(τ)}＝Q(t)δ(t-τ)其中，<mathsid="math0017"num="0017"><math><![CDATA[<mrow><mi>Q</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>Q</mi><mn>1</mn></msub></mtd><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd><mtd><msub><mi>Q</mi><mn>2</mn></msub></mtd></mtr></mtable></mfenced></mrow>]]></math>id="icf0022"file="A2008100647140003C4.tif"wi="30"he="11"top="164"left="41"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>为对角阵，Q1的对角线元素根据陀螺噪声来确定，取为噪声RMS值的平方除以带宽f*Q1＝[(RMSgyro)2/f*]·I3×3Q2的对角线元素根据陀螺零位漂移的特点来确定，系统的噪声系数矩阵为<mathsid="math0018"num="0018"><math><![CDATA[<mrow><mi>G</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mo>-</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><msub><mi>I</mi><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd><mtd><msub><mi>I</mi><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>6</mn><mo>)</mo></mrow></mrow>]]></math>id="icf0023"file="A2008100647140003C5.tif"wi="72"he="16"top="202"left="75"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>系统的状态转移矩阵为id="icf0024"file="A2008100647140003C6.tif"wi="72"he="15"top="221"left="75"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>上式中，id="icf0025"file="A2008100647140003C7.tif"wi="29"he="6"top="240"left="51"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>为载体角速度向量在载体坐标系中的估计值，id="icf0026"file="A2008100647140004C1.tif"wi="41"he="18"top="29"left="22"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>为向量id="icf0027"file="A2008100647140004C2.tif"wi="2"he="3"top="36"left="76"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>的反对称矩阵；②根据加速度计测量值判断系统加速运动情况和磁强计测量值判断地磁受干扰情况，如果重力向量和地磁向量没有受到干扰，转到步骤③；如果重力向量或地磁向量受到干扰，转到步骤④③建立卡尔曼滤波的观测方程，并转到步骤⑤Z(t)＝H(t)X(t)+V(t)(8)式中，Z(t)为t时刻系统的量测向量，H(t)为系统量测矩阵，V(t)为系统的量测噪声向量；系统量测向量为id="icf0028"file="A2008100647140004C3.tif"wi="84"he="12"top="98"left="63"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>观测向量id="icf0029"file="A2008100647140004C4.tif"wi="18"he="5"top="120"left="47"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>id="icf0030"file="A2008100647140004C5.tif"wi="49"he="20"top="113"left="67"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>式中，axayaz为三轴加速度计输出的加速度；观测向量id="icf0031"file="A2008100647140004C6.tif"wi="20"he="4"top="149"left="47"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>id="icf0032"file="A2008100647140004C7.tif"wi="56"he="20"top="142"left="71"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>式中，mxmymz为三轴磁强计输出的磁强；观测噪声向量为<mathsid="math0019"num="0019"><math><![CDATA[<mrow><mi>V</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>V</mi><mn>1</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>V</mi><mn>2</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>10</mn><mo>)</mo></mrow></mrow>]]></math>id="icf0033"file="A2008100647140004C8.tif"wi="84"he="11"top="173"left="63"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>{V(t)}为独立均值高斯白噪声过程，其协方差矩阵E{V(t)VT(τ)}＝Rd(t)δ(t-τ)<mathsid="math0020"num="0020"><math><![CDATA[<mrow><msub><mi>R</mi><mi>d</mi></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>R</mi><mrow><mi>d</mi><mn>1</mn></mrow></msub></mtd><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd><mtd><msub><mi>R</mi><mrow><mi>d</mi><mn>2</mn></mrow></msub></mtd></mtr></mtable></mfenced></mrow>]]></math>id="icf0034"file="A2008100647140004C9.tif"wi="32"he="12"top="203"left="31"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>为对角阵，Rd1和Rd2的对角线元素分别根据加速度计和磁强计的噪声来确定，工程上可近似取为噪声RMS值的平方Rd1＝(RMSacc)2·I3×3，Rd2＝(RMSmag)2·I3×3系统观测矩阵为H(t)＝[H1(t)06×3](11)上式中，id="icf0035"file="A2008100647140004C10.tif"wi="101"he="14"top="240"left="47"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>④构造重力向量和地磁向量的法向量r＝a×m在n系中投影的法向量理论值为rn＝an×mn，可以由an和mn求得；在b系中法向量的测量值为rb＝ab×mb，可以由ab和mb求得；建立卡尔曼滤波的观测方程Z(t)＝H(t)X(t)+V(t)式中，Z(t)为t时刻系统的量测向量；H(t)为系统量测矩阵；V(t)为系统的量测噪声向量；(a)地磁向量受到干扰的情况系统量测向量为id="icf0036"file="A2008100647140005C1.tif"wi="89"he="12"top="92"left="61"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>观测向量id="icf0037"file="A2008100647140005C2.tif"wi="18"he="5"top="108"left="44"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>id="icf0038"file="A2008100647140005C3.tif"wi="21"he="4"top="108"left="66"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>观测噪声向量为<mathsid="math0021"num="0021"><math><![CDATA[<mrow><mi>V</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>V</mi><mn>1</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>V</mi><mn>3</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>10</mn><mo>)</mo></mrow></mrow>]]></math>id="icf0039"file="A2008100647140005C4.tif"wi="89"he="12"top="116"left="61"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>V3(t)为法向量的测量噪声，与V1(t)和V2(t)有关{V(t)}为独立均值高斯白噪声过程，其协方差矩阵E{V(t)VT(τ)}＝Rd(t)δ(t-τ)<mathsid="math0022"num="0022"><math><![CDATA[<mrow><msub><mi>R</mi><mi>d</mi></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mo></mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>R</mi><mrow><mi>d</mi><mn>1</mn></mrow></msub></mtd><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd><mtd><msub><mi>R</mi><mrow><mi>d</mi><mn>3</mn></mrow></msub></mtd></mtr></mtable></mfenced></mrow>]]></math>id="icf0040"file="A2008100647140005C5.tif"wi="33"he="11"top="157"left="27"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>为对角阵系统观测矩阵为H(t)＝[H1(t)06×3]上式中，id="icf0041"file="A2008100647140005C6.tif"wi="106"he="12"top="180"left="44"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>(b)重力向量受到干扰的情况系统量测向量为id="icf0042"file="A2008100647140005C7.tif"wi="92"he="12"top="203"left="61"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>观测噪声向量为<mathsid="math0023"num="0023"><math><![CDATA[<mrow><mi>V</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>V</mi><mn>3</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>V</mi><mn>2</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>10</mn><mo>)</mo></mrow></mrow>]]></math>id="icf0043"file="A2008100647140005C8.tif"wi="93"he="11"top="218"left="62"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>{V(t)}为独立均值高斯白噪声过程，其协方差矩阵E{V(t)VT(τ)}＝Rd(t)δ(t-τ)<mathsid="math0024"num="0024"><math><![CDATA[<mrow><msub><mi>R</mi><mi>d</mi></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mo></mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>R</mi><mrow><mi>d</mi><mn>3</mn></mrow></msub></mtd><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd><mtd><msub><mi>R</mi><mrow><mi>d</mi><mn>2</mn></mrow></msub></mtd></mtr></mtable></mfenced></mrow>]]></math>id="icf0044"file="A2008100647140005C9.tif"wi="33"he="11"top="250"left="28"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>为对角阵系统观测矩阵为H(t)＝[H1(t)06×3]上式中，id="icf0045"file="A2008100647140006C1.tif"wi="101"he="12"top="38"left="46"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>⑤根据扩展卡尔曼滤波递推方程和步骤①、③或④的结果，建立卡尔曼滤波的时间传播方程式中ωb＝[ωxωyωz]T为当前时刻三轴陀螺的输出；id="icf0047"file="A2008100647140006C3.tif"wi="16"he="4"top="74"left="127"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>为上一时刻更新后的陀螺零偏估计；Δt为计算步长；<mathsid="math0025"num="0025"><math><![CDATA[<mrow><mover><mover><mi>q</mi><mo>‾</mo></mover><mo>·</mo></mover><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><mover><mi>q</mi><mo>‾</mo></mover><mtext>⊗</mtext><mover><mi>ω</mi><mo>‾</mo></mover><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><mover><mi>q</mi><mo>‾</mo></mover><mo>⊗</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mover><mi>ω</mi><mo>‾</mo></mover><mrow><mo>(</mo><msup><mi>t</mi><mo>-</mo></msup><mo>)</mo></mrow></mtd></mtr></mtable></mfenced></mrow>]]></math></maths>式中，<overscore>q</overscore><overscore>ω</overscore>分别表示四元数向量，id="icf0049"file="A2008100647140006C5.tif"wi="12"he="8"top="106"left="91"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>表示四元数乘法，具体为P(t+Δt)-)＝Φ(t+Δt，t)P(t+)ΦT(t+Δt，t)+Qd(t)(13)式中，Φ(t+Δt，t)＝I+F(t)Δt，Qd(t)＝Q(t)Δt⑥根据扩展卡尔曼滤波递推方程和步骤①、②的结果，建立卡尔曼滤波的测量修正方程K(t)＝P(t-)HT(HP(t-)HT+Rd)-1(14)P(t+)＝(I-K(t)H)P(t-)(15)id="icf0053"file="A2008100647140006C9.tif"wi="8"he="3"top="222"left="34"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>经过补偿更新后需进行规一化处理；(4)输出载体姿态参数由步骤(3)估计得到的姿态四元数id="icf0056"file="A2008100647140006C12.tif"wi="2"he="3"top="244"left="140"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>即可由公式(2)求得姿态矩阵id="icf0057"file="A2008100647140006C13.tif"wi="5"he="4"top="251"left="34"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>由姿态矩阵id="icf0058"file="A2008100647140006C14.tif"wi="4"he="4"top="251"left="64"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>的元素计算得出姿态角，具体步骤航向角主值id="icf0059"file="A2008100647140007C1.tif"wi="24"he="9"top="29"left="65"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>俯仰角主值φ主＝-sin-1C13横滚角主值id="icf0060"file="A2008100647140007C2.tif"wi="25"he="9"top="50"left="65"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>由上述主值按照如下公式判断真值φ＝φ主2.如权利要求1所述的硅微航姿系统惯性/地磁组合方法，其特征是步骤(3)中步骤③或步骤④卡尔曼滤波器观测方程的构造，量测向量z在载体坐标b系中构造。3.如权利要求1所述的硅微航姿系统惯性/地磁组合方法，其特征是重力向量或地磁向量受到干扰时，采用步骤(3)中步骤④的卡尔曼滤波器观测方程。全文摘要本发明硅微航姿系统惯性/地磁组合方法。具体涉及利用重力向量和地磁向量的观测量确定载体的初始姿态；基于重力场、地磁场这两个姿态参考向量选取最佳观测向量的方法；组合Kalman滤波器观测方程坐标系的选取原则；惯性/地磁Kalman组合滤波器的实现方法。通过上述发明，可以用低精度硅微惯性元件与磁传感器组合实现硅微航姿系统，实时给出载体的航向姿态。本发明对低精度惯性/地磁组合方法具有通用性，能减小载体运动加速度对重力的影响和外界磁场对地磁场的影响，可广泛应用于硅微航姿系统。文档编号G01C21/08GK101290229SQ20081006471公开日2008年10月22日申请日期2008年6月13日优先权日2008年6月13日发明者许兆新,琳赵,勇郝,闫保中,黄卫权申请人:哈尔滨工程大学