CN1740746A - 微小型动态载体姿态测量装置及其测量方法 - Google Patents

Abstract

一种微小型动态载体姿态测量装置，属于信号测量、导航技术领域，其特点在于体积小、重量轻、适用于高动态环境，可用在飞行器、车辆、船舶或其他载体的姿态测量；该装置包括3轴速率陀螺、3轴磁场计、单轴加速度计、温度传感器、模数转换电路、微处理器和存储器、串行通讯接口，以及相应软件测量方法；3轴速率陀螺等输出连接模数转换电路的输入端，模数转换电路的控制信号、数据信号分别与微处理器和存储器对应端相连；微处理器、存储器输出端与串口通讯单元相连。将该测量装置与飞行器固连，初始化，采集传感器信号，补偿、标定、更新状态变量，计算并输出当前姿态角。该发明对传感器性能要求不高，姿态测量精度较高，特别是适用动态姿态测量。

Description

微小型动态载体姿态测量装置及其测量方法

技术领域

本发明涉及一种微小型载体姿态测量装置，特别适用于动态载体姿态测量，属于信号处理、测量、导航领域。

本发明还涉及根据此装置的一种测量方法。

背景技术

姿态信息的测量对于飞机、车辆、船舶等运动载体具有十分重要的意义。传统方法中，人们用陀螺仪测量运动载体的姿态角。好的陀螺仪具有相当高的精度，但同时它也存在价格昂贵、结构复杂、体积大、质量重等缺点，这使它的应用受到了很大限制。速率陀螺价格较低、体积小、质量轻，但它的精度较低，漂移严重，因此不能直接用来测量姿态角。

利用加速度传感器和地磁传感器测量重力加速度和地磁场的3轴分量也可以解算出载体的姿态角(如美国专利20020188416)。这类传感器的体积能够做到很小，但由于加速度传感器同时也敏感运动加速度，因此容易受到运动加速度干扰，不能用在非平衡状态下。

另一种方法是在加速度和地磁传感器的基础上，加入速率陀螺，构造基于四元数的卡尔曼滤波器，对姿态角进行实时估计(如已授权的美国专利6647352)。此方法利用3轴速率陀螺输出更新状态量，采用3轴加速度和3轴磁场计的测量值作为观测值，在最小二乘意义下得到状态变量的最优估计，进而得到姿态角的估计值。此方法的优点在于能够较好的滤除传感器信号的随机干扰，补偿陀螺的漂移，有效地提高了精度。但它要求在长时间内重力加速度的测量值是无偏的，也就是说，当载体处于长时间处于加速(或者减速、转弯)状态时，此方法的精度将严重降低，甚至不可用。为了解决这一问题，人们提出了根据不同状态自动调整滤波器噪声的方法。这种方法在长时间处于加、减速或者转弯状态时，增加对陀螺信号的依赖程度。其前提是陀螺信号在这一段时间内漂移没有明显变化，这对陀螺的性能提出了较高的要求，但使用高精度陀螺，会增加***的价格和重量，而且随着时间的增长，误差会不断累积，因此仍然不能解决这一问题。

此外，利用差分GPS或者GPS阵列的方法测量载体姿态角也是一种常用方法，这种方法容易受到卫星信号丢失的影响，在较多遮挡的情况下效果不好，GPS接收天线的位置和距离也会影响姿态测量的精度。

发明内容

本发明介绍了一种微小型动态载体姿态测量装置。该装置包括3轴速率陀螺(101)、3轴磁场计(102)、单轴加速度计(103)、温度传感器(104)、单轴速度传感器(105)、模数转换电路(106)、微处理器和存储器(107)、串行通讯接口(108)，以及相应软件测量方法。其中单轴速度传感器(105)为可选器件，没有它***仍能正常工作，但加入单轴速度传感器(105)有助于提高测量精度。

3轴速率陀螺(101)到单轴速度传感器(105)5种传感器的输出电压信号分别连接到模数转换电路(106)的各路输入端。模数转换电路(106)的控制信号和数据信号分别与微处理器和存储器(107)的对应端口相连接。微处理器和存储器(107)的输出端口与串口通讯单元(108)相连接。微处理器和存储器(107)按照预设的采样率控制模数转换电路(106)对各路传感器信号进行采样，并读取相应数据，进行处理，计算出姿态角，存储或输出到串口通讯单元(108)。

利用上述测量装置测量载体姿态信息，步骤如下：

第一步，将测量装置与被测载体固连。定义地理坐标系和载体坐标系如图2所示。图2(a)为地理坐标系，N、E、D三个正交坐标轴分别指向北、东、地3个方向。图2(b)为载体坐标系，x、y、z三个正交轴分别与N、E、D三轴对齐时，各姿态角定义为零，方向以右手定则为准。姿态角的符号表示与范围可规定为：俯仰角(θ)范围-90°～90°，滚转角(φ)范围-180°～180°，航向角(ψ)范围0°～360°。

按照航向→俯仰→滚转的顺序，由惯性坐标到载体坐标的方向余弦矩阵R可以表示为：

$R=\left[\begin{array}{ccc}\mathrm{c\ψ}\·\mathrm{c\θ}& \mathrm{s\ψ}\·\mathrm{c\θ}& -\mathrm{s\θ}\\ \mathrm{c\ψ}\·\mathrm{s\θ}\·\mathrm{s\φ}-\mathrm{s\ψ}\·\mathrm{c\φ}& \mathrm{s\ψ}\·\mathrm{s\θ}\·\mathrm{s\φ}+\mathrm{c\ψ}\·\mathrm{c\φ}& \mathrm{c\θ}\·\mathrm{s\φ}\\ \mathrm{c\ψ}\·\mathrm{s\θ}\·\mathrm{c\φ}+\mathrm{s\ψ}\·\mathrm{s\φ}& \mathrm{s\ψ}\·\mathrm{s\θ}\·\mathrm{c\φ}-\mathrm{c\ψ}\·\mathrm{s\φ}& \mathrm{c\θ}\·\mathrm{c\φ}\end{array}\right]$

其中s和c分别为函数sin和cos的简写。

本发明用方向余弦矩阵R的第一列和第三列构造一个向量v，

v＝[r₁₁ r₂₁ r₃₁ r₁₃ r₂₃ r₃₃]

其中r_ij表示在R中的第i行j列个元素。

第二步，***初始化。设定采样周期，设定状态变量的初始值，状态估计误差的协方差矩阵的初始值，测量噪声，过程噪声。将测量装置静态放置，设定3轴速率陀螺的零点。

状态估计误差的协方差矩阵的初始值可任意给定，不影响***的收敛性。测量噪声和过程噪声的大小则要根据具体传感器和应用环境决定。状态变量的初始值需要通过初始对准来确定，比如将载体放置于各姿态角均为零的位置，然后将状态变量的初值可以设置为v₀＝[1 0 0 0 0 1]。

第三步，采集传感器信号。由微处理器控制模数转换电路按照采样周期采集各路传感器数据，读取模数转换的结果到微处理器中。

第四步，对传感器数据进行补偿和标定。按照传感器温度特性曲线对传感器采样结果进行温度补偿。由于安装引起的误差也需要进行标定。

传感器3轴速率陀螺(101)、3轴磁场计(102)、单轴加速度计(103)、单轴速度传感器(105)的温度特性曲线要经过预先测定，并保存在存储器中。根据采样时的温度值，微处理器从存储器中读取温度特性曲线对应值，对上述传感器数据进行补偿。如果不能保证加速度计(103)安装在载体的转动中心，则根据当前转动角速度与到转动中心的距离计算出由于转动引起的向心加速度，进行补偿。

$a_x=a_x^{\′}-({\mathrm{\ω}}_y^2+{\mathrm{\ω}}_z^2)X_{\mathrm{ax}}---\left(1\right)$

其中X_ax为x轴加速度计到转动中心的距离，a_x′为加速度计的测量值，a_x为补偿由于转动和偏心引起的加速度后的加速度值。

第五步，利用陀螺数据更新状态变量。利用3轴速率陀螺测量值和前一时刻的状态变量的估计值可以计算出当前时刻状态变量的一步预报估计值。

更新公式如下：

$\hat{v}(\mathrm{kh}+h|\mathrm{kh})=F\·\hat{v}\left(\mathrm{kh}\right)---\left(2\right)$

其中F为陀螺当前角速率的函数， 为kh时刻状态变量v的估计值，

为状态变量的一步预报估计。

第六步，根据观测值和约束条件修正状态变量。将3轴磁场计、单轴加速度计、以及两个状态变量的约束条件作为测量值，对状态变量的一步预报值进行修正。

根据测量值和约束条件，对一步预报得到的状态量

进行修正：

P(kh+h|kh)＝F·P(kh)·F^T+Φ_ev

K(kh+h)＝P(kh+h|kh)·C^T[C·P(kh+h|kh)·C^T+Φ_ey]^-1

$\hat{v}(\mathrm{kh}+h)=\hat{v}(\mathrm{kh}+h|\mathrm{kh})+K(\mathrm{kh}+h)\·[y(\mathrm{kh}+h)-C\·\hat{v}(\mathrm{kh}+h|\mathrm{kh})]$

P(kh+h)＝[I-K(kh+h)·C]·P(kh+h|kh)。                  (3)

第七步，计算并输出当前姿态角。根据状态变量计算当前姿态角信息，并根据需要存储或者输出。

通过状态变量v解算出姿态角，具体公式如下：

俯仰角：

θ＝-arcsin(r₁₃)                                 (4)

滚转角：

航向角：

$\mathrm{\&psi;}=\left\{\begin{array}{cc}\arccos (r_{11}/\cos \left(\mathrm{\&theta;}\right)),& r_{31}\&times;r_{23}-r_{33}\&times;r_{21}\&GreaterEqual;0\\ 2\mathrm{\&pi;}-\arccos (r_{11}/\cos \left(\mathrm{\&theta;}\right)),& r_{31}\&times;r_{32}-r_{33}\&times;r_{21}<0\end{array}\right.---\left(6\right)$

第八步，跳转到第三步或者退出。跳转到第三步继续测量，或者停止退出。

第五步和第六步基于以下离散状态空间模型：

v(kh+h)＝F·v(kh)+e_v(kh)                             (7)

y(kh+h)＝C·v(kh+h)+e_y(kh+h)                         (8)

其中

$F=I+h\&times;\left[\begin{array}{cc}\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)& 0\\ 0& \mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)\end{array}\right],$

y(kh+h)＝[m_x(kh+h) m_y(kh+h) m_z(kh+h) a_x(kh+h) 1 1]^T

$C=\left[\begin{array}{cccccc}\cos \mathrm{\&beta;}& 0& 0& \sin \mathrm{\&beta;}& 0& 0\\ 0& \cos \mathrm{\&beta;}& 0& 0& \sin \mathrm{\&beta;}& 0\\ 0& 0& \cos \mathrm{\&beta;}& 0& 0& \sin \mathrm{\&beta;}\\ 0& 0& 0& g& 0& 0\\ r_{11\mathrm{kh}}& r_{21\mathrm{kh}}& r_{31\mathrm{kh}}& 0& 0& 0\\ 0& 0& 0& r_{13\mathrm{kh}}& r_{23\mathrm{kh}}& r_{33\mathrm{kh}}\end{array}\right]$

e_v和e_y是功率谱密度为Ф_ev和Ф_ey的零均值白噪声。

以状态方程(7)，测量方程(8)构造离散卡尔曼滤波器，其中测量矩阵C由上一时刻状态变量构成。因为方程(8)中的测量值y仅含有a_x，所以不受y、z轴方向运动加速度项的影响。如前所述，如果利用

代替a_x将会消除x轴方向运动加速度的影响，这样在高动态环境下仍能保证高精度测量。

离散状态空间方程由下面的连续方程按照采样周期h离散化得到。

对方向余弦矩阵R取微分，得到方程如下：

$\frac{\mathrm{dR}}{\mathrm{dt}}=\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)R---\left(9\right)$

其中 $\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_z& {-\mathrm{\&omega;}}_y\\ {-\mathrm{\&omega;}}_z& 0& {\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_x& 0\end{array}\right]$

因为v由R的第一列和第三列组成，所以由方程(9)可以得出：

$\frac{d}{\mathrm{dt}}v=\left[\begin{array}{cc}\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)& 0\\ 0& \mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)\end{array}\right]v+e_v---\left(10\right)$

其中0为3×3的零矩阵，e_v近似为零均值白噪声，其功率谱密度近似为Ф_ev。

重力加速度矢量在惯性坐标系中的坐标表示为a₀＝[0 0 g]^T。在载体坐标系中测量值为a＝[a_x a_y a_z]^T。重力加速度矢量由惯性坐标系转换到载体坐标系：

a＝Ra₀+e_a                            (11)

其中e_a为测量噪声。当载体处于静态或者低动态环境时，e_a可以近似为功率谱密度为Ф_a的零均值白噪声，但当载体处于高动态环境时，加速度测量值与重力加速度分量相差很大，特别是存在长周期低频运动加速度时，如长时间盘旋，e_a不能认为是零均值白噪声，但此加速度对于x轴加速度分量的影响相对较小。根据方程(6)，加速度矢量在载体坐标系x轴方向的分量为：

a_x＝r₁₃g+e_ax                             (12)

其中e_ax可近似为功率谱密度为Ф_ax的零均值白噪声。如果x轴方向加入速度传感器，用代替上式中的a_x将会有助于进一步提高精度。

地磁矢量在惯性坐标系中的坐标表示为m₀＝[M cosβ 0 M sinβ]^T，其中M为地磁矢量的模(后面表述中将M作为单位长度量省略)，β为当地地磁倾角，忽略地磁偏角或解算出航向角后进行修正。在载体坐标系中测量值为m＝[m_x m_y m_z]^T。地磁矢量由惯性坐标系转换到载体坐标系：

m＝Rm₀+e_m                      (13)

其中e_m为测量噪声，可近似为独立的零均值白噪声。

注意到向量v的六个元素并不是相互独立的，存在如下两个约束条件：

$\begin{array}{cc}{r}_{11}^2+r_{21}^2+r_{31}^2=1,& r_{13}^2+r_{23}^2+r_{33}^2=1\end{array}---\left(14\right)$

将约束条件(14)作为向量v的“伪测量方程”，与方程(12)、(13)一起构成向量v的测量方程。方程(14)中含有非线性项，因此需要做线性化处理。将(14)中二次项中的一项写到输出方程中，即将方程(12)、(13)、(14)写成如下形式：

$\left[\begin{array}{c}{m}_x\\ m_y\\ m_z\\ a_x\\ 1\\ 1\end{array}\right]=\left[\begin{array}{cccccc}\cos \mathrm{\&beta;}& 0& 0& \sin \mathrm{\&beta;}& 0& 0\\ 0& \cos \mathrm{\&beta;}& 0& 0& \sin \mathrm{\&beta;}& 0\\ 0& 0& \cos \mathrm{\&beta;}& 0& 0& \sin \mathrm{\&beta;}\\ 0& 0& 0& g& 0& 0\\ r_{11}& r_{21}& r_{31}& 0& 0& 0\\ 0& 0& 0& r_{13}& r_{23}& r_{33}\end{array}\right]\&times;\left[\begin{array}{c}{r}_{11}\\ r_{21}\\ r_{31}\\ r_{13}\\ r_{23}\\ r_{33}\end{array}\right]+e_y---\left(15\right)$

其中e_y＝[e_mx e_my e_mz e_ax e_c1 e_c2]^T，e_c1、e_c2为约束条件的计算误差，可近似为白噪声。

将方程(10)和方程(15)按采样周期h离散化就会得到由方程(7)和方程(8)构成的离散***方程。

附图说明

图1是该姿态测量装置的硬件组成框图。

图2是描述惯性坐标系与载体坐标系，其中(a)惯性坐标系(NED)，(b)载体坐标系(xyz)。

图3是测量方法流程图。

图4是飞行器坐标系。

具体实施方式

下面以测量飞行器姿态为例，介绍该姿态测量装置和方法的具体实时过程。

图1为该姿态测量装置的硬件组成框图。3轴速率陀螺(101)、3轴磁场计(102)、单轴加速度计(103)、温度传感器(104)、单轴速度传感器(105)、模数转换电路(106)、微处理器和存储器(107)、串行通讯接口(108)，其中速度传感器(105)为可选器件。3轴速率陀螺(101)、3轴磁场计(102)、单轴加速度计(103)等器件选用基于MEMS技术的传感器，体积小、重量轻。

3轴速率陀螺(101)、3轴磁场计(102)、单轴加速度计(103)、温度传感器(104)、单轴速度传感器(105)5种传感器的输出电压信号分别连接到模数转换电路(106)的各路输入端。模数转换电路(106)的控制信号和数据信号分别与微处理器和存储器(107)的对应端口相连接。微处理器和存储器(107)的输出端口与串口通讯单元(108)相连接。

图2是描述惯性坐标系与载体坐标系，其中(a)惯性坐标系(NED)，(b)载体坐标系(xyz)。

图3是测量方法流程图，说明了利用上述装置测量飞行器姿态的方法和步骤。

(301)将该测量装置与飞行器固连，图4说明了坐标轴x、y、z分别与飞行器各对应轴向对齐。x轴与飞行器机头方向对其，y轴与飞行器机翼方向对其，z轴与飞行器机翼方向垂直。

(302)***初始化。设定采样周期为25Hz；将载体放置于各姿态角均为零的位置，然后将状态变量的初值设置为；状态估计误差的协方差矩阵的初始值任意设定为单位矩阵；设定测量噪声，过程噪声的值。将测量装置静态放置，设定3轴速率陀螺的测量值为零点。

(303)采集传感器信号。由微处理器控制模数转换电路按照采样周期采集各路传感器数据，读取模数转换的结果到微处理器中。

(304)温度补偿和标定，根据温度传感器测量结果，从存储器中读出预先标定好的修正值，对传感器数据进行温度补偿。下面所用到的传感器信号均为经过温度补偿后的数据。

测量出x轴加速度计到转动中心的距离X_ax，根据当前转动角速度与X_ax，计算出由于转动引起的向心加速度，进行补偿。

$a_x=a_x^{\&prime;}-({\mathrm{\&omega;}}_y^2+{\mathrm{\&omega;}}_z^2)X_{\mathrm{ax}}$

其中a_x′为加速度计的测量值，a_x为补偿由于转动和偏心引起的加速度后的加速度值。

(305)利用陀螺数据更新状态变量。利用3轴速率陀螺测量值和前一时刻的状态变量的估计值可以计算出当前时刻状态变量的一步预报估计值。

更新公式如下：

$\hat{v}(\mathrm{kh}+h|\mathrm{kh})=F\&CenterDot;\hat{v}\left(\mathrm{kh}\right)$

其中F为陀螺当前角速率的函数，

为kh时刻状态变量v的估计值，

为状态变量的一步预报估计。

(306)根据观测值和约束条件修正状态变量。将3轴磁场计、单轴加速度计、以及两个状态变量的约束条件作为测量值，对状态变量的一步预报值进行修正。

根据测量值和约束条件，对一步预报得到的状态量 进行修正：

P(kh+h|kh)＝F·P(kh)·F^T+Φ_ev

K(kh+h)＝P(kh+h|kh)·C^T[C·P(kh+h|kh)·C^T+Φ_ey]^-1

$\hat{v}(\mathrm{kh}+h)=\hat{v}(\mathrm{kh}+h|\mathrm{kh})+K(\mathrm{kh}+h)\&CenterDot;[y(\mathrm{kh}+h)-C\&CenterDot;\hat{v}(\mathrm{kh}+h|\mathrm{kh})]$

P(kh+h)＝[I-K(kh+h)·C]·P(kh+h|kh)；

(307)计算并输出当前姿态角。根据状态变量计算当前姿态角信息，并根据需要存储或者输出。

通过状态变量v解算出姿态角，具体公式如下：

俯仰角：

θ＝-arcsin(r₁₃)

滚转角：

航向角：

$\mathrm{\&psi;}=\left\{\begin{array}{cc}\arccos (r_{11}/\cos \left(\mathrm{\&theta;}\right)),& r_{31}\&times;r_{23}-r_{33}\&times;r_{21}\&GreaterEqual;0\\ 2\mathrm{\&pi;}-\arccos (r_{11}/\cos \left(\mathrm{\&theta;}\right)),& r_{31}\&times;r_{32}-r_{33}\&times;r_{21}<0\end{array}\right.$

(308)跳转到第三步或者退出。跳转到第三步继续测量，或者停止退出。

该装置体积小、重量轻，与传统惯性导航装置相比，对传感器性能要求不高，可以选用体积很小的MEMS传感器，且通过特定算法，有效地消除了运动加速度的干扰，姿态测量精度能做到较高，因此特别适用于高动态环境中对飞行器、车辆、船舶或其他载体的动态姿态进行测量。

Claims (6)

1、一种微小型动态载体姿态测量装置，其特征在于，该装置由3轴速率陀螺(101)、3轴磁场计(102)、单轴加速度计(103)、温度传感器(104)、模数转换电路(106)、微处理器和存储器(107)、串行通讯接口(108)组成；

3轴速率陀螺(101)到温度传感器(104)4种传感器的输出电压信号分别连接到模数转换电路(106)的各路输入端；

模数转换电路(106)的控制信号和数据信号分别与微处理器和存储器(107)的对应端口相连接；

微处理器和存储器(107)的输出端口与串口通讯单元(108)相连接。

2、根据权利要求1所述的一种微小型动态载体姿态测量装置，其特征在于，该装置还有一个单轴速度传感器(105)；单轴速度传感器(105)的输出电压信号连接到模数转换电路(106)的输入端。

3、根据权利要求1或2所述的一种微小型动态载体姿态测量装置，其特征在于，3轴器件分别与机体坐标系中各对应坐标轴对齐。

4、根据权利要求1或2所述的一种微小型动态载体姿态测量装置，其特征在于，单轴加速度计安装方向为运动加速度干扰较小的一轴。

5、根据权利要求2所述的一种微小型动态载体姿态测量装置，其特征在于，速度传感器与加速度计同向安装。

6、根据权利要求1所述的一种微小型动态载体姿态测量装置进行载体姿态信息测量的一种测量方法，其特征在于，该测量方法包括如下步骤：

第一步：将测量装置与被测载体固连；定义地理坐标系和载体坐标系；

按照航向→俯仰→滚转的顺序，构造由惯性坐标到载体坐标的方向余弦矩阵R：

其中，s和c分别为函数sin和cos的简写；

用方向余弦矩阵R的第一列和第三列构造一个向量v，

         v＝[r₁₁ r₂₁ r₃₁ r₁₃ r₂₃ r₃₃]

其中r_ij表示在R中的第i行j列个元素；

第二步，***初始化

设定采样周期，设定状态变量的初始值，状态估计误差的协方差矩阵的初始值，测量噪声，过程噪声；

将测量装置静态放置，设定3轴速率陀螺的零点；

第三步，采集传感器信号

微处理器控制模数转换电路按照采样周期采集各路传感器数据，读取模数转换的结果到微处理器中；

第四步，对传感器数据进行补偿和标定

按照传感器温度特性曲线对传感器采样结果进行温度补偿；

由于安装引起的误差也需要进行标定；

第五步，利用陀螺数据更新状态变量

利用3轴速率陀螺测量值和前一时刻的状态变量的估计值计算出当前时刻状态变量的一步预报估计值；更新公式为：

$\hat{v}(\mathrm{kh}+h|\mathrm{kh})=F\&CenterDot;\hat{v}\left(\mathrm{kh}\right)$

其中F为陀螺当前角速率的函数，

为kh时刻状态变量v的估计值， 为状态变量的一步预报估计；

第六步，根据观测值和约束条件修正状态变量

将3轴磁场计、单轴加速度计、以及两个状态变量的约束条件作为测量值，对状态变量的一步预报值进行修正；

根据测量值和约束条件，对一步预报得到的状态量 进行修正：

P(kh+h|kh)＝F·P(kh)·F^T+Φ_ev

k(kh+h)＝P(kh+h|kh)·C^T[C·P(kh+h|kh)·C^T+Ф_ey]^-1

$\hat{v}(\mathrm{kh}+h)=\hat{v}(\mathrm{kh}+h|\mathrm{kh})+K(\mathrm{kh}+h)\&CenterDot;[y(\mathrm{kh}+h)-C\&CenterDot;\hat{v}(\mathrm{kh}+h|\mathrm{kh})]$

P(kh+h)＝[I-K(kh+h)·C]·P(kh+h|kh)

第七步，计算并输出当前姿态角

根据状态变量计算当前姿态角信息，并根据需要存储或者输出；

通过状态变量v解算出姿态角，具体公式如下：

俯仰角：

θ＝-arcsin(r₁₃)

滚转角：

航向角：

$\mathrm{\&psi;}=\left\{\begin{array}{cc}\arccos (r_{11}/\cos \left(\mathrm{\&theta;}\right)),& r_{31}\&times;r_{23}-r_{33}\&times;r_{21}\&GreaterEqual;0\\ 2\mathrm{\&pi;}-\arccos (r_{11}/\cos \left(\mathrm{\&theta;}\right)),& r_{31}\&times;r_{23}-r_{33}\&times;r_{21}<0\end{array}\right.$

第八步，跳转到第三步或者退出；

跳转到第三步继续测量，或者停止，退出，完成测量。

Images