CN105547326B - 陀螺与磁传感器联合标定方法 - Google Patents
陀螺与磁传感器联合标定方法 Download PDFInfo
- Publication number
- CN105547326B CN105547326B CN201510901523.8A CN201510901523A CN105547326B CN 105547326 B CN105547326 B CN 105547326B CN 201510901523 A CN201510901523 A CN 201510901523A CN 105547326 B CN105547326 B CN 105547326B
- Authority
- CN
- China
- Prior art keywords
- mtd
- mrow
- msub
- mtr
- msubsup
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Abstract
本发明提供的一种陀螺与磁传感器联合标定方法,其特征在于,包括如下步骤:步骤1,获得磁传感器与陀螺的关联测量模型,在所述测量模型中包括标定参数;步骤2,采用基于所述标定参数的约束非线性最小二乘估计得到所述标定参数的确定值。与现有技术相比,本发明的有益效果如下:1、既可以标定磁传感器与陀螺之间的失准角,还可以同时标定陀螺零偏;2、过程中磁场越稳定,标定效果越好,而且不受任何加速度干扰的影响,因此实施过程中不需要设备保持静止状态;3、可用于磁传感器与惯性测量单元(内含陀螺和加速度计)的姿态配准。
Description
技术领域
本发明涉及传感器技术领域,具体而言,涉及一种陀螺与磁传感器的联合标定方法。
背景技术
陀螺和磁传感器(后者又名磁力仪、磁强计)经常用于姿态确定或科学测量领域。陀螺敏感载体的角速度,磁力仪敏感环境磁场。当磁力仪处于铁磁材料附近时,磁力仪周围的磁场受到扭曲,不能正确测量出磁场强度。磁场干扰可分为硬磁效应和软磁效应两种。硬磁效应是由永磁铁或电流产生的加性磁干扰,而软磁效应由软磁材料诱导产生,处于背景磁场中的软磁材料会诱导产生出自身的磁场,并对背景磁场的强度和方向产生扭曲。除此之外,因制造工艺不完善,磁力仪还存在标度因数、敏感轴交叉耦合和偏置等误差,因此,在使用磁力仪之前,必须对以上误差进行标校。这里所说的标定是指磁力仪内部标定。
当磁力仪与陀螺一起使用时,还必须进行磁力仪外部标定,即需要标定磁力仪与陀螺之间的坐标系失准角。软磁效应不仅会引起磁力仪内部参数的变化,也会导致磁力仪与陀螺的坐标系失准角发生改变。因此,在使用之前,需要进行磁力仪内部标定以及磁力仪与其他传感器间的坐标系失准角标定。常用的磁力仪标定方法利用了当地的磁场强度与磁力仪姿态无关这样一个事实,优点是不需要外部辅助设备,缺点是不能标定磁力仪与其他传感器的坐标系失准角。另一方面,低成本陀螺(如MEMS陀螺)的零偏误差较大,而且每次使用时都有所变化,如果不做补偿直接使用陀螺测量值,将影响磁力仪与陀螺的坐标系失准角标定效果。
发明内容
针对现有技术中的缺陷,本发明的目的是提供一种解决上述技术问题的陀螺与磁传感器联合标定方法。
为解决上述技术问题,本发明提供的一种陀螺与磁传感器联合标定方法,包括如下步骤:
步骤1,获得磁传感器与陀螺的关联测量模型,在所述关联测量模型中包括标定参数;
步骤2,采用基于所述标定参数的约束非线性最小二乘估计得到所述标定参数的确定值。
优选地,所述标定参数包括坐标系失准角和陀螺零偏。
优选地,所述关联测量模型为:
其中,mm(tk+1)表示第k+1个时刻tk+1时磁传感器坐标系m下的磁场向量,mm(tk)表示第k个时刻tk时磁传感器坐标系m下的磁场向量,k为正整数,mm(t)表示时刻t时磁传感器坐标系m下的磁场向量,表示磁传感器与陀螺坐标系失准姿态矩阵,表示时刻t时陀螺坐标系b的角速度向量,ε表示陀螺零偏向量;vec(·)表示将矩阵按照列的顺序拼接起来;
mm(t)×表示由三维向量mm(t)=[mm(t)1 mm(t)2 mm(t)3]T构成的叉乘矩阵,即其中mm(t)1表示时刻t时磁传感器坐标系m下的磁场向量的X方向上的分量;其中mm(t)2表示时刻t时磁传感器坐标系m下的磁场向量的Y方向上的分量;其中mm(t)3表示时刻t时磁传感器坐标系m下的磁场向量的Z方向上的分量。
优选地,所述约束非线性最小二乘估计由所述关联测量模型导出:
其中,S3表示模为1的四维向量;λ为拉格朗日系数;q=[q0 q1 q2 q3]T为磁传感器与陀螺坐标系失准姿态矩阵对应的四元数,q0,q1,q2,q3分别表示四元数q的四个分量;εm表示在磁传感器坐标系下的陀螺零偏,
vec(C(q))表示将矩阵C(q)按照列的顺序拼接起来;
磁传感器与陀螺坐标系失准姿态矩阵的系数矩阵Wk为:
陀螺零偏的系数矩阵Mk为:
mm(t)×表示由三维向量mm(t)=[mm(t)1 mm(t)2 mm(t)3]T构成的叉乘矩阵,即其中mm(t)1表示时刻t时磁传感器坐标系m下的磁场向量的X方向上的分量;其中mm(t)2表示时刻t时磁传感器坐标系m下的磁场向量的Y方向上的分量;其中mm(t)3表示时刻t时磁传感器坐标系m下的磁场向量的Z方向上的分量。
优选地,利用线性最小二乘方法从所述关联测量模型中得到磁传感器与陀螺坐标系失准姿态矩阵的初始值和陀螺零偏的初始值
从最小二乘解中提取出对应的和将正交化并利用姿态矩阵与四元数的变换关系得到四元数的初始值q(0),拉格朗日系数初始值λ(0)设为零。
优选地,迭代计算直至满足收敛条件:
其中,x(i+1)表示待估参数x的第i+1次迭代值,x(i)表示待估参数x的第i次迭代值,J为雅可比矩阵导数向量,H为海森矩阵,待估参数x为:
其中,所述雅可比矩阵导数向量J和所述海森矩阵H为:
其中,
Jλ=qTq-1,
αk=Wkvec(C(q))+Mkεm-(mm(tk+1)-mm(tk)),
与现有技术相比,本发明的有益效果如下:
1、既可以标定磁传感器与陀螺之间的失准角,还可以同时标定陀螺零偏;
2、过程中磁场越稳定,标定效果越好,而且不受任何加速度干扰的影响,因此实施过程中不需要设备保持静止状态;
3、可用于磁传感器与惯性测量单元(内含陀螺和加速度计)的姿态配准。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明,但不以任何形式限制本发明。应当指出的是,对本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变化和改进。这些都属于本发明的保护范围。
在稳定磁场中,三轴磁传感器测量值的变化完全是由于姿态的变化引起的。基于此事实,本发明提供了一种三轴磁力仪与三轴陀螺之间的坐标系失准角以及陀螺零偏的联合标定方法。磁力仪与陀螺固定连接,充分变化姿态并同步采集磁力仪与陀螺的测量。磁力仪数据可用于磁力仪内部标定,磁力仪和陀螺的数据共同用于本发明的磁力仪与陀螺之间的坐标系失准角以及陀螺零偏的联合标定。以下假定已经实现了磁力仪内部标定。
本发明陀螺与磁传感器联合标定方法包括:获得磁传感器与陀螺的关联测量模型,所述测量模型中包括坐标系失准角和陀螺零偏等参数;采用基于所述标定参数的约束非线性最小二乘估计得到所述标定参数确定值。
其中,关联测量模型为:
其中,mm(tk+1)表示第k+1个时刻tk+1时磁传感器坐标系m下的磁场向量,mm(tk)表示第k个时刻tk时磁传感器坐标系m下的磁场向量,k为正整数,mm(t)表示时刻t时磁传感器坐标系m下的磁场向量,表示磁传感器与陀螺坐标系失准姿态矩阵,表示时刻t时陀螺坐标系b的角速度向量,ε表示陀螺零偏向量;vec(·)表示将矩阵按照列的顺序拼接起来;
mm(t)×表示由三维向量mm(t)=[mm(t)1 mm(t)2 mm(t)3]T构成的叉乘矩阵,即其中mm(t)1表示时刻t时磁传感器坐标系m下的磁场向量的X方向上的分量;其中mm(t)2表示时刻t时磁传感器坐标系m下的磁场向量的Y方向上的分量;其中mm(t)3表示时刻t时磁传感器坐标系m下的磁场向量的Z方向上的分量。
优选地,约束非线性最小二乘估计由关联测量模型导出:
其中,S3表示模为1的四维向量;λ为拉格朗日系数;q=[q0 q1 q2 q3]T为磁传感器与陀螺坐标系失准姿态矩阵对应的四元数,q0,q1,q2,q3分别表示四元数q的四个分量,四元数q表示为q0+q1i+q2j+q3k,其中,虚数单位i,j,k满足运算规则:i0=j0=k0=1,i2=j2=k2=-1;εm表示在磁传感器坐标系下的陀螺零偏,
vec(C(q))表示将矩阵C(q)按照列的顺序拼接起来;
磁传感器与陀螺坐标系失准姿态矩阵的系数矩阵Wk为:
陀螺零偏的系数矩阵Mk为:
mm(t)×表示由三维向量mm(t)=[mm(t)1 mm(t)2 mm(t)3]T构成的叉乘矩阵,即其中mm(t)1表示时刻t时磁传感器坐标系m下的磁场向量的X方向上的分量;其中mm(t)2表示时刻t时磁传感器坐标系m下的磁场向量的Y方向上的分量;其中mm(t)3表示时刻t时磁传感器坐标系m下的磁场向量的Z方向上的分量。
优选地,利用线性最小二乘方法从关联测量模型中得到磁传感器与陀螺坐标系失准姿态矩阵的初始值和陀螺零偏的初始值
从最小二乘解中提取出对应的和将正交化并利用姿态矩阵与四元数的变换关系得到四元数的初始值q(0),拉格朗日系数初始值λ(0)设为零。
优选地,迭代计算直至满足收敛条件:
其中,x(i+1)表示待估参数x的第i+1次迭代值,x(i)表示待估参数x的第i次迭代值,J为雅可比矩阵导数向量,H为海森矩阵,待估参数x为:
其中,雅可比矩阵导数向量J和海森矩阵H为:
其中,
Jλ=qTq-1,
αk=Wkvec(C(q))+Mkεm-(mm(tk+1)-mm(tk)),
以上对本发明的具体实施例进行了描述。需要理解的是,本发明并不局限于上述特定实施方式,本领域技术人员可以在权利要求的范围内做出各种变化或修改,这并不影响本发明的实质内容。在不冲突的情况下,本申请的实施例和实施例中的特征可以任意相互组合。
Claims (5)
1.一种陀螺与磁传感器联合标定方法,其特征在于,包括如下步骤:
步骤1,获得磁传感器与陀螺的关联测量模型,在所述关联测量模型中包括标定参数;所述关联测量模型为:
<mfenced open = "" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msup>
<mi>m</mi>
<mi>m</mi>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msup>
<mi>m</mi>
<mi>k</mi>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>k</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msubsup>
<mo>&Integral;</mo>
<msub>
<mi>t</mi>
<mi>k</mi>
</msub>
<msub>
<mi>t</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
</msubsup>
<mrow>
<mo>(</mo>
<msup>
<mi>m</mi>
<mi>m</mi>
</msup>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
<mo>&times;</mo>
<mo>)</mo>
</mrow>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>m</mi>
</msubsup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>b</mi>
</mrow>
<mi>b</mi>
</msubsup>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
<mo>-</mo>
<mi>&epsiv;</mi>
<mo>)</mo>
</mrow>
<mi>d</mi>
<mi>t</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mo>&Integral;</mo>
<msub>
<mi>t</mi>
<mi>k</mi>
</msub>
<msub>
<mi>t</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
</msubsup>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>b</mi>
</mrow>
<mi>b</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>&CircleTimes;</mo>
<mrow>
<mo>(</mo>
<msup>
<mi>m</mi>
<mi>m</mi>
</msup>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
<mo>&times;</mo>
<mo>)</mo>
</mrow>
<mi>d</mi>
<mi>t</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mo>&Integral;</mo>
<msub>
<mi>t</mi>
<mi>k</mi>
</msub>
<msub>
<mi>t</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
</msubsup>
<mrow>
<mo>(</mo>
<msup>
<mi>m</mi>
<mi>m</mi>
</msup>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
<mo>&times;</mo>
<mo>)</mo>
</mrow>
<mi>d</mi>
<mi>t</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>v</mi>
<mi>e</mi>
<mi>c</mi>
<mrow>
<mo>(</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>m</mi>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>m</mi>
</msubsup>
<mi>&epsiv;</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
其中,mm(tk+1)表示第k+1个时刻tk+1时磁传感器坐标系m下的磁场向量,mm(tk)表示第k个时刻tk时磁传感器坐标系m下的磁场向量,k为正整数,mm(t)表示时刻t时磁传感器坐标系m下的磁场向量,表示磁传感器与陀螺坐标系失准姿态矩阵,表示时刻t时陀螺坐标系b的角速度向量,ε表示陀螺零偏向量;vec(·)表示将矩阵按照列的顺序拼接起来;
mm(t)×表示由三维向量mm(t)=[mm(t)1 mm(t)2 mm(t)3]T构成的叉乘矩阵,即其中mm(t)1表示时刻t时磁传感器坐标系m下的磁场向量的X方向上的分量;其中mm(t)2表示时刻t时磁传感器坐标系m下的磁场向量的Y方向上的分量;其中mm(t)3表示时刻t时磁传感器坐标系m下的磁场向量的Z方向上的分量;
步骤2,采用基于所述标定参数的约束非线性最小二乘估计得到所述标定参数的确定值。
2.根据权利要求1所述的陀螺与磁传感器联合标定方法,其特征在于,所述标定参数包括坐标系失准角和陀螺零偏。
3.根据权利要求1所述的陀螺与磁传感器联合标定方法,其特征在于,所述约束非线性最小二乘估计由所述关联测量模型导出:
<mrow>
<munder>
<mi>min</mi>
<mrow>
<mi>q</mi>
<mo>&Element;</mo>
<msup>
<mi>S</mi>
<mn>3</mn>
</msup>
<mo>,</mo>
<msub>
<mi>&epsiv;</mi>
<mi>m</mi>
</msub>
</mrow>
</munder>
<mo>{</mo>
<munder>
<mo>&Sigma;</mo>
<mi>k</mi>
</munder>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>W</mi>
<mi>k</mi>
</msub>
<mi>v</mi>
<mi>e</mi>
<mi>c</mi>
<mrow>
<mo>(</mo>
<mi>C</mi>
<mo>(</mo>
<mi>q</mi>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<msub>
<mi>&epsiv;</mi>
<mi>m</mi>
</msub>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msup>
<mi>m</mi>
<mi>m</mi>
</msup>
<mo>(</mo>
<msub>
<mi>t</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
<mo>-</mo>
<msup>
<mi>m</mi>
<mi>m</mi>
</msup>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>k</mi>
</msub>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>|</mo>
<msup>
<mo>|</mo>
<mn>2</mn>
</msup>
<mo>}</mo>
<mo>+</mo>
<mi>&lambda;</mi>
<mrow>
<mo>(</mo>
<msup>
<mi>q</mi>
<mi>T</mi>
</msup>
<mi>q</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,S3表示模为1的四维向量;λ为拉格朗日系数;q=[q0 q1 q2 q3]T为磁传感器与陀螺坐标系失准姿态矩阵对应的四元数,q0,q1,q2,q3分别表示四元数q的四个分量;εm表示在磁传感器坐标系下的陀螺零偏,
<mrow>
<mi>C</mi>
<mrow>
<mo>(</mo>
<mi>q</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>q</mi>
<mn>0</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>q</mi>
<mn>0</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>q</mi>
<mn>0</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
vec(C(q))表示将矩阵C(q)按照列的顺序拼接起来;
磁传感器与陀螺坐标系失准姿态矩阵的系数矩阵Wk为:
<mrow>
<msub>
<mi>W</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<msubsup>
<mo>&Integral;</mo>
<msub>
<mi>t</mi>
<mi>k</mi>
</msub>
<msub>
<mi>t</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
</msubsup>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>b</mi>
</mrow>
<mrow>
<mi>b</mi>
<mi>T</mi>
</mrow>
</msubsup>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>&CircleTimes;</mo>
<mrow>
<mo>(</mo>
<msup>
<mi>m</mi>
<mi>m</mi>
</msup>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
<mo>&times;</mo>
<mo>)</mo>
</mrow>
<mi>d</mi>
<mi>t</mi>
<mo>;</mo>
</mrow>
陀螺零偏的系数矩阵Mk为:
<mrow>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<mo>-</mo>
<msubsup>
<mo>&Integral;</mo>
<msub>
<mi>t</mi>
<mi>k</mi>
</msub>
<msub>
<mi>t</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
</msubsup>
<mrow>
<mo>(</mo>
<msup>
<mi>m</mi>
<mi>m</mi>
</msup>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
<mo>&times;</mo>
<mo>)</mo>
</mrow>
<mi>d</mi>
<mi>t</mi>
<mo>;</mo>
</mrow>
mm(t)×表示由三维向量mm(t)=[mm(t)1 mm(t)2 mm(t)3]T构成的叉乘矩阵,即其中mm(t)1表示时刻t时磁传感器坐标系m下的磁场向量的X方向上的分量;其中mm(t)2表示时刻t时磁传感器坐标系m下的磁场向量的Y方向上的分量;其中mm(t)3表示时刻t时磁传感器坐标系m下的磁场向量的Z方向上的分量。
4.根据权利要求3所述的陀螺与磁传感器联合标定方法,其特征在于,利用线性最小二乘方法从所述关联测量模型中得到磁传感器与陀螺坐标系失准姿态矩阵的初始值和陀螺零偏的初始值
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>v</mi>
<mi>e</mi>
<mi>c</mi>
<mrow>
<mo>(</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>m</mi>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&epsiv;</mi>
<mi>m</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<msup>
<mrow>
<mo>(</mo>
<munder>
<mo>&Sigma;</mo>
<mi>k</mi>
</munder>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>W</mi>
<mi>k</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>W</mi>
<mi>k</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>)</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
<munder>
<mo>&Sigma;</mo>
<mi>k</mi>
</munder>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>W</mi>
<mi>k</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
<mrow>
<mo>(</mo>
<msup>
<mi>m</mi>
<mi>m</mi>
</msup>
<mo>(</mo>
<msub>
<mi>t</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
<mo>-</mo>
<msup>
<mi>m</mi>
<mi>m</mi>
</msup>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>k</mi>
</msub>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
从最小二乘解中提取出对应的和将正交化并利用姿态矩阵与四元数的变换关系得到四元数的初始值q(0),拉格朗日系数初始值λ(0)设为零。
5.根据权利要求4所述的陀螺与磁传感器联合标定方法,其特征在于,迭代计算直至满足收敛条件:
<mrow>
<msup>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</msup>
<mo>=</mo>
<msup>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
</msup>
<mo>-</mo>
<msup>
<mrow>
<mo>&lsqb;</mo>
<mi>H</mi>
<msub>
<mo>|</mo>
<msup>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
</msup>
</msub>
<mo>&rsqb;</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
<mi>J</mi>
<msub>
<mo>|</mo>
<msup>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
</msup>
</msub>
<mo>,</mo>
<mi>i</mi>
<mo>=</mo>
<mn>0</mn>
<mo>,</mo>
<mn>1</mn>
<mo>,</mo>
<mn>...</mn>
<mo>;</mo>
</mrow>
其中,x(i+1)表示待估参数x的第i+1次迭代值,x(i)表示待估参数x的第i次迭代值,J为雅可比矩阵导数向量,H为海森矩阵,待估参数x为:
<mrow>
<mi>x</mi>
<mo>=</mo>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msup>
<mi>q</mi>
<mi>T</mi>
</msup>
</mtd>
<mtd>
<msubsup>
<mi>&epsiv;</mi>
<mi>m</mi>
<mi>T</mi>
</msubsup>
</mtd>
<mtd>
<mi>&lambda;</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
<mo>;</mo>
</mrow>
其中,所述雅可比矩阵导数向量J和所述海森矩阵H为:
<mrow>
<mi>J</mi>
<mo>=</mo>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msubsup>
<mi>J</mi>
<mi>q</mi>
<mi>T</mi>
</msubsup>
</mtd>
<mtd>
<msubsup>
<mi>J</mi>
<msub>
<mi>&epsiv;</mi>
<mi>m</mi>
</msub>
<mi>T</mi>
</msubsup>
</mtd>
<mtd>
<msub>
<mi>J</mi>
<mi>&lambda;</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
</mrow>
<mrow>
<mi>H</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>H</mi>
<mrow>
<mi>q</mi>
<mi>q</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>H</mi>
<mrow>
<msub>
<mi>q&epsiv;</mi>
<mi>m</mi>
</msub>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>H</mi>
<mrow>
<mi>q</mi>
<mi>&lambda;</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>H</mi>
<mrow>
<msub>
<mi>q&epsiv;</mi>
<mi>m</mi>
</msub>
</mrow>
<mi>T</mi>
</msubsup>
</mtd>
<mtd>
<msub>
<mi>H</mi>
<mrow>
<msub>
<mi>&epsiv;</mi>
<mi>m</mi>
</msub>
<msub>
<mi>&epsiv;</mi>
<mi>m</mi>
</msub>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>1</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>H</mi>
<mrow>
<mi>q</mi>
<mi>&lambda;</mi>
</mrow>
<mi>T</mi>
</msubsup>
</mtd>
<mtd>
<msubsup>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中,
<mrow>
<msub>
<mi>J</mi>
<mi>q</mi>
</msub>
<mo>=</mo>
<mn>2</mn>
<mi>&lambda;</mi>
<mi>q</mi>
<mo>+</mo>
<mn>2</mn>
<munder>
<mo>&Sigma;</mo>
<mi>k</mi>
</munder>
<msup>
<mo>&part;</mo>
<mi>T</mi>
</msup>
<mi>q</mi>
<mrow>
<mo>(</mo>
<mi>v</mi>
<mi>e</mi>
<mi>c</mi>
<mo>(</mo>
<mrow>
<mi>C</mi>
<mrow>
<mo>(</mo>
<mi>q</mi>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<msubsup>
<mi>W</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<msub>
<mi>&alpha;</mi>
<mi>k</mi>
</msub>
<mo>,</mo>
<msub>
<mi>J</mi>
<msub>
<mi>&epsiv;</mi>
<mi>m</mi>
</msub>
</msub>
<mo>=</mo>
<mn>2</mn>
<munder>
<mo>&Sigma;</mo>
<mi>k</mi>
</munder>
<msubsup>
<mi>M</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<msub>
<mi>&alpha;</mi>
<mi>k</mi>
</msub>
<mo>,</mo>
<msub>
<mi>J</mi>
<mi>&lambda;</mi>
</msub>
<mo>=</mo>
<msup>
<mi>q</mi>
<mi>T</mi>
</msup>
<mi>q</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
</mrow>
<mfenced open = "" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>H</mi>
<mrow>
<mi>q</mi>
<mi>q</mi>
</mrow>
</msub>
<mo>=</mo>
<mn>2</mn>
<msub>
<mi>&lambda;I</mi>
<mn>4</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>+</mo>
<mn>2</mn>
<munder>
<mo>&Sigma;</mo>
<mi>k</mi>
</munder>
<mo>{</mo>
<msubsup>
<mo>&part;</mo>
<mi>q</mi>
<mi>T</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>v</mi>
<mi>e</mi>
<mi>c</mi>
<mo>(</mo>
<mrow>
<mi>C</mi>
<mrow>
<mo>(</mo>
<mi>q</mi>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<msubsup>
<mi>W</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<msub>
<mi>W</mi>
<mi>k</mi>
</msub>
<msub>
<mo>&part;</mo>
<mi>q</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>v</mi>
<mi>e</mi>
<mi>c</mi>
<mo>(</mo>
<mrow>
<mi>C</mi>
<mrow>
<mo>(</mo>
<mi>q</mi>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>Q</mi>
<mn>0</mn>
<mi>T</mi>
</msubsup>
<msubsup>
<mi>W</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<msub>
<mi>&alpha;</mi>
<mi>k</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>Q</mi>
<mn>1</mn>
<mi>T</mi>
</msubsup>
<msubsup>
<mi>W</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<msub>
<mi>&alpha;</mi>
<mi>k</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>Q</mi>
<mn>2</mn>
<mi>T</mi>
</msubsup>
<msubsup>
<mi>W</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<msub>
<mi>&alpha;</mi>
<mi>k</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>Q</mi>
<mn>3</mn>
<mi>T</mi>
</msubsup>
<msubsup>
<mi>W</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<msub>
<mi>&alpha;</mi>
<mi>k</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
<mo>}</mo>
<mo>,</mo>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mrow>
<msub>
<mi>H</mi>
<mrow>
<msub>
<mi>q&epsiv;</mi>
<mi>m</mi>
</msub>
</mrow>
</msub>
<mo>=</mo>
<mn>2</mn>
<munder>
<mo>&Sigma;</mo>
<mi>k</mi>
</munder>
<msup>
<mo>&part;</mo>
<mi>T</mi>
</msup>
<mi>q</mi>
<mrow>
<mo>(</mo>
<mi>v</mi>
<mi>e</mi>
<mi>c</mi>
<mo>(</mo>
<mrow>
<mi>C</mi>
<mrow>
<mo>(</mo>
<mi>q</mi>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<msubsup>
<mi>W</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mo>,</mo>
</mrow>
<mrow>
<msub>
<mi>H</mi>
<mrow>
<mi>q</mi>
<mi>&lambda;</mi>
</mrow>
</msub>
<mo>=</mo>
<mn>2</mn>
<mi>q</mi>
<mo>,</mo>
<msub>
<mi>H</mi>
<mrow>
<msub>
<mi>&epsiv;</mi>
<mi>m</mi>
</msub>
<msub>
<mi>&epsiv;</mi>
<mi>m</mi>
</msub>
</mrow>
</msub>
<mo>=</mo>
<mn>2</mn>
<munder>
<mo>&Sigma;</mo>
<mi>k</mi>
</munder>
<msubsup>
<mi>M</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
</mrow>
αk=Wkvec(C(q))+Mkεm-(mm(tk+1)-mm(tk)),
<mrow>
<msub>
<mo>&part;</mo>
<mi>q</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>v</mi>
<mi>e</mi>
<mi>c</mi>
<mo>(</mo>
<mrow>
<mi>C</mi>
<mrow>
<mo>(</mo>
<mi>q</mi>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mn>2</mn>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
<mrow>
<msub>
<mi>Q</mi>
<mn>0</mn>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<msub>
<mi>Q</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<msub>
<mi>Q</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<msub>
<mi>Q</mi>
<mn>3</mn>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>.</mo>
</mrow>
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510901523.8A CN105547326B (zh) | 2015-12-08 | 2015-12-08 | 陀螺与磁传感器联合标定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510901523.8A CN105547326B (zh) | 2015-12-08 | 2015-12-08 | 陀螺与磁传感器联合标定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105547326A CN105547326A (zh) | 2016-05-04 |
CN105547326B true CN105547326B (zh) | 2018-04-06 |
Family
ID=55826680
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510901523.8A Active CN105547326B (zh) | 2015-12-08 | 2015-12-08 | 陀螺与磁传感器联合标定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105547326B (zh) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106197475B (zh) * | 2016-06-29 | 2019-06-21 | 上海交通大学 | 基于序贯滤波的陀螺与磁传感器联合标定方法 |
CN107037235B (zh) * | 2016-11-28 | 2023-08-01 | 东南大学 | 一种制动下滑量软测量方法及测量装置 |
CN107389092B (zh) * | 2017-06-27 | 2020-01-07 | 上海交通大学 | 一种基于磁传感器辅助的陀螺标定方法 |
US11604067B2 (en) | 2019-12-19 | 2023-03-14 | Immersion Networks, Inc. | Systems and methods for stabilizing magnetic field of inertial measurement unit |
CN111982155B (zh) * | 2020-08-27 | 2022-08-12 | 北京爱笔科技有限公司 | 磁传感器的标定方法、装置、电子设备和计算机存储介质 |
CN112945271B (zh) * | 2021-01-27 | 2022-12-06 | 苏州大学 | 磁力计信息辅助的mems陀螺仪标定方法及标定系统 |
CN115839726B (zh) * | 2023-02-23 | 2023-04-28 | 湖南二零八先进科技有限公司 | 磁传感器和角速度传感器联合标定的方法、系统及介质 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101246024A (zh) * | 2008-03-26 | 2008-08-20 | 北京航空航天大学 | 一种外场快速标定微型多传感器组合导航系统的方法 |
CN102865881A (zh) * | 2012-03-06 | 2013-01-09 | 武汉大学 | 一种惯性测量单元的快速标定方法 |
CN103221788A (zh) * | 2010-11-08 | 2013-07-24 | 阿尔派回放股份有限公司 | 陀螺仪传感器的标定设备和方法 |
CN104501814A (zh) * | 2014-12-12 | 2015-04-08 | 浙江大学 | 一种基于视觉和惯性信息的姿态与位置估计方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20150019159A1 (en) * | 2013-07-15 | 2015-01-15 | Honeywell International Inc. | System and method for magnetometer calibration and compensation |
-
2015
- 2015-12-08 CN CN201510901523.8A patent/CN105547326B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101246024A (zh) * | 2008-03-26 | 2008-08-20 | 北京航空航天大学 | 一种外场快速标定微型多传感器组合导航系统的方法 |
CN103221788A (zh) * | 2010-11-08 | 2013-07-24 | 阿尔派回放股份有限公司 | 陀螺仪传感器的标定设备和方法 |
CN102865881A (zh) * | 2012-03-06 | 2013-01-09 | 武汉大学 | 一种惯性测量单元的快速标定方法 |
CN104501814A (zh) * | 2014-12-12 | 2015-04-08 | 浙江大学 | 一种基于视觉和惯性信息的姿态与位置估计方法 |
Non-Patent Citations (2)
Title |
---|
Error Calibration of Magnetometer Using Nonlinear Integrated Filter Model With Inertial Sensors;Wonmo Koo etal.;《IEEE Transactions on Magnetics》;20090519;第45卷(第6期);第2740-2743页 * |
惯性/地磁组合导航算法;蔡洪等;《中国惯性技术学报》;20090630;第17卷(第3期);第333-337页 * |
Also Published As
Publication number | Publication date |
---|---|
CN105547326A (zh) | 2016-05-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105547326B (zh) | 陀螺与磁传感器联合标定方法 | |
JP6525336B2 (ja) | 3軸デジタルコンパス | |
Wu et al. | Dynamic magnetometer calibration and alignment to inertial sensors by Kalman filtering | |
CN104834021B (zh) | 一种地磁传感器灵敏度的计算方法 | |
CN103299247B (zh) | 用于磁近场的动态追踪及补偿的设备和方法 | |
US10685083B2 (en) | Apparatuses and methods for calibrating magnetometer attitude-independent parameters | |
CN105676302A (zh) | 一种基于改进最小二乘法的磁力仪随机噪声误差补偿方法 | |
CN107024673B (zh) | 基于陀螺仪辅助的三轴磁强计全误差标定方法 | |
CN102778965B (zh) | 3d指示装置与补偿3d指示装置的转动的方法 | |
CN107290801A (zh) | 基于函数链接型神经网络和场模平方差的捷联三轴磁强计误差一步校正方法 | |
CN109521384A (zh) | 一种基于原子磁强计的矢量磁补偿方法 | |
CN106197475B (zh) | 基于序贯滤波的陀螺与磁传感器联合标定方法 | |
CN113156355A (zh) | 一种超导全张量磁梯度测量装置的磁干扰补偿方法 | |
CN107860382A (zh) | 一种在地磁异常情况下应用ahrs测量姿态的方法 | |
CN107389092B (zh) | 一种基于磁传感器辅助的陀螺标定方法 | |
CN104765013A (zh) | 三轴磁传感器标定方法 | |
Chesneau | Magneto-Inertial Dead-Reckoning in inhomogeneous field and indoor applications | |
Cui et al. | Three-axis magnetometer calibration based on optimal ellipsoidal fitting under constraint condition for pedestrian positioning system using foot-mounted inertial sensor/magnetometer | |
Dorveaux et al. | On-the-field calibration of an array of sensors | |
Hemanth et al. | Calibration of 3-axis magnetometers | |
CN203337153U (zh) | 一种三轴数字指南针 | |
Ouyang et al. | Compensation method of cross-axis effect for AMR sensor | |
Xiaojuan et al. | Comparison of three kinds of compensation algorithms based on magnetic sensors | |
CN105258671A (zh) | 一种提高磁通门测角精度的方法 | |
Chu et al. | Magnetic orientation system based on magnetometer, accelerometer and gyroscope |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |