CN103033197A - 一种mems陀螺零位飘移的校正方法 - Google Patents

Abstract

本发明涉及一种MEMS陀螺零位飘移的校正方法，通过时间t时刻和时间t+Δt时刻的框架坐标系中方位和俯仰的角度，通过空间坐标变换将其投影到到惯性坐标系：然后计算得到每隔△t时间的飘移角速度，然后将此飘移角速度与陀螺测得框架在惯性坐标系中角速度求和，得到校正后的框架角速度。有益效果：利用系统内稳定，精度高的传感器去校正MEMS陀螺零位飘移，校正效果明显。此方法应用场合主要为移动载体领域。本方法主要由软件完成，硬件成本低廉。

Description

一种MEMS陀螺零位飘移的校正方法

技术领域

本发明属于惯导技术领域，涉及一种如何更加有效的使用MEMS陀螺的速度信息的方法，具体涉及一种MEMS陀螺零位飘移的校正方法。 

背景技术

MEMS陀螺作为角速率传感器，在消费电子，军工，航空领域得到了广泛的应用，但是其技术特点决定其固有缺点，即零位飘移大，且随温度变化明显，以往大多数应用在消费电子领域，但现在随着对其研究的进一步深入，对其零位漂移消除有了一些解决途径，比如滤波，温度补偿等方法，经过这些方法的处理之后，MEMS陀螺也可用在某些要求较高的领域，比如军工上机载，弹载等领域。 

现在MEMS陀螺以其出色的抗震性和低成本在在越来越多的领域得到应用，但其零位漂移对其应用有很大的制约，所以针对消除其零漂，提高精度的研究也越来越多地开展。 

发明内容

要解决的技术问题 

为了避免现有技术的不足之处，本发明提出一种MEMS陀螺零位飘移的校正方法，将某飞行器中测角信息和姿态信息利用起来，进行空间坐标系变换，来对MEMS陀螺零位飘移补偿的方法。 

技术方案 

一种MEMS陀螺零位飘移的校正方法，其特征在于步骤如下： 

步骤1：测出时间t时刻的框架坐标系方位和俯仰的角度，通过空间坐标变换将其投影到到惯性坐标系： 

$\left\{\begin{array}{c}\left|\begin{array}{c}{x}_{\mathrm{ft}}\\ y_{\mathrm{ft}}\\ z_{\mathrm{ft}}\end{array}\right|=\left|A\left(\begin{array}{ccc}{\mathrm{\υ}}_{\mathrm{pt}}& {\mathrm{\υ}}_{\mathrm{Tt}}& {\mathrm{\υ}}_{\mathrm{kt}}\end{array}\right)\right|\·\left|B\left(\begin{array}{ccc}{\mathrm{\β}}_t& {\mathrm{\ϵ}}_t& 0\end{array}\right)\right|\·\left|\begin{array}{c}1\\ 0\\ 0\end{array}\right|\\ {\mathrm{\ϵ}}_{\mathrm{ft}}=\arcsin y_{\mathrm{ft}}\\ {\mathrm{\β}}_{\mathrm{ft}}=-\mathrm{arctg}\frac{z_{\mathrm{ft}}}{x_{\mathrm{ft}}}\\ \end{array}\right.$

其中：x_ft，y_ft，z_ft为俯仰和方位的角度在惯性坐标系下投影的直角坐标值；ε_ft，β_ft为俯仰和方位的角度在惯性坐标系下投影的极坐标值；A(υ_ptυ_Ttυ_kt)为弹体坐标系向惯性坐标系转换所乘的方向余弦矩阵，其中υ_pt，υ_Tt，υ_kt为t时刻弹体姿态信息；B(β_tε_t0)为框架坐标系向弹体坐标系转换所乘的方向余弦矩阵，其中ε_t，β_t为t时刻位标器俯仰和方位测角数据； 

所述框架坐标系：原点都取在活动的旋转中心上，当框架的方位与俯仰处于零位时，其三个轴与载体坐标系重合； 

所述惯性坐标系：坐标系的原点取在地心，各坐标轴相对于恒星的指向不变化； 

所述弹体坐标系：坐标系的原点与导弹的质心重合，X轴沿导弹纵轴指向前方，Z轴沿导弹横轴指向右方，Y轴沿导弹竖轴指向天空并与X轴、Z轴构成右手直角坐标系；导弹相对于惯性坐标系下的俯仰角、滚转角和偏航角统称为姿态角； 

步骤2：测出时间t+Δt时刻的框架坐标系中方位和俯仰的角度，通过空间坐标变换将其投影到到惯性坐标系： 

$\left[\begin{array}{c}\left|\begin{array}{c}{x}_{f(t+\mathrm{\Δt})}\\ y_{f(t+\mathrm{\Δt})}\\ z_{f(t+\mathrm{\Δt})}\end{array}\right|=\left|A\left(\begin{array}{ccc}{\mathrm{\υ}}_{p(t+\mathrm{\Δt})}& {\mathrm{\υ}}_{T(t+\mathrm{\Δt})}& {\mathrm{\υ}}_{k(t+\mathrm{\Δt})}\end{array}\right)\right|\·\left|B\left(\begin{array}{ccc}{\mathrm{\β}}_{(t+\mathrm{\Δt})}& {\mathrm{\ϵ}}_{(t+\mathrm{\Δt})}& 0\end{array}\right)\right|\·\left|\begin{array}{c}1\\ 0\\ 0\end{array}\right|\\ {\mathrm{\ϵ}}_{f(t+\mathrm{\Δt})}=\arcsin y_{f(t+\mathrm{\Δt})}\\ {\mathrm{\β}}_{f(t+\mathrm{\Δt})}=-\mathrm{arctg}\frac{z_{f(t+\mathrm{\Δt})}}{x_{f(t+\mathrm{\Δt})}}\\ \end{array}\right]$

步骤3：将t时刻和t+△t时刻角度在惯性坐标系的投影求差： 

Δε_ft(Δt)＝ε_f(t+Δt)-ε_ft

Δβ_ft(Δt)＝β_f(t+Δt)-β_ft

步骤4：利用陀螺测得框架在惯性坐标系中角速度ω_y,ω_z，根据欧拉运动方程得到

和

角的变化速度： 

${\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{ft}}=\frac{{\mathrm{\ω}}_z}{\cos \mathrm{\β}},$ ${\stackrel{\·}{\mathrm{\β}}}_{\mathrm{ft}}={\mathrm{\ω}}_y;$

步骤5：对

和

进行积分： 

${\mathrm{\Δ\ϵ}}_{\mathrm{fti}}\left(\mathrm{\Δt}\right)=\mathrm{\Δt}\underset{j=t}{\overset{t+\mathrm{\Δt}}{\mathrm{\Σ}}}{\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{ft}}$

${\mathrm{\Δ\β}}_{\mathrm{fti}}\left(\mathrm{\Δt}\right)=\mathrm{\Δt}\underset{j=t}{\overset{t+\mathrm{\Δt}}{\mathrm{\Σ}}}{\stackrel{\·}{\mathrm{\β}}}_{\mathrm{ft}};$

步骤6：计算在惯性坐标系中△t时间内天线固有漂移速度为： 

${\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{f\Δt}}=\frac{{\mathrm{\Δ\ϵ}}_{\mathrm{fti}}\left(\mathrm{\Δt}\right)-{\mathrm{\Δ\ϵ}}_{\mathrm{ft}}\left(\mathrm{\Δt}\right)}{\mathrm{\Δt}}$

${\stackrel{\·}{\mathrm{\β}}}_{\mathrm{f\Δt}}=\frac{{\mathrm{\Δ\β}}_{\mathrm{fti}}\left(\mathrm{\Δt}\right)-{\mathrm{\Δ\β}}_{\mathrm{ft}}\left(\mathrm{\Δt}\right)}{\mathrm{\Δt}}$

步骤7：漂移角速度在惯性坐标系坐标轴上的投影为： 

$\left\{\begin{array}{c}{\mathrm{\Δ\ω}}_{\mathrm{x\Δt}}={\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{f\Δt}}\sin {\mathrm{\β}}_{\mathrm{ft}}\\ {\mathrm{\Δ\ω}}_{\mathrm{y\Δt}}={\stackrel{\·}{\mathrm{\β}}}_{\mathrm{f\Δt}}\\ {\mathrm{\Δ\ω}}_{\mathrm{z\Δt}}={\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{f\Δt}}\cos {\mathrm{\β}}_{\mathrm{ft}}\end{array}\right.;$

步骤8：每隔△t时间计算一次上述步骤的飘移角速度，然后将此飘移角速度与陀螺测得框架在惯性坐标系中角速度ω_y,ω_z求和，即得到校正后的框架角速度 ω_yt，ω_zt如下： 

ω_ft＝ω_y+Δω_yΔt

ω_zt＝ω_z+Δω_zΔt。 

有益效果 

本发明提出的一种MEMS陀螺零位飘移的校正方法，通过时间t时刻和时间t+Δt时刻的框架坐标系中方位和俯仰的角度，通过空间坐标变换将其投影到到惯性坐标系：然后计算得到每隔△t时间的飘移角速度，然后将此飘移角速度与陀螺测得框架在惯性坐标系中角速度求和，得到校正后的框架角速度。有益效果：利用系统内稳定，精度高的传感器去校正MEMS陀螺零位飘移，校正效果明显。此方法应用场合主要为移动载体领域。本方法主要由软件完成，硬件成本低廉。 

具体实施方式

现结合实施例对本发明作进一步描述： 

本实施例要解决的技术问题：MEMS陀螺以其出色的抗震性和低成本在机载，弹载设备上有很大的应用前景，但其零位漂移对其应用有很大的制约，本发明提出一种利弹载体测角信息和载体姿态信息的组合来对MEMS陀螺零位进行消除，达到了很好的效果，能满足机载弹载设备的需要。 

首先要明确几个坐标系 

惯性坐标系：对于研究地球表面附近运载体的惯性系统时，经常将惯性参考坐标系的原点取在地心，各坐标轴相对于恒星的指向不变化。 

弹体坐标系：坐标系的原点与飞行器的质心重合，X轴沿飞行器纵轴指向前方，Z轴沿飞行器横轴指向右方，Y轴沿飞行器竖轴指向天空并与X轴、Z轴构成右手直角坐标系。飞行器相对于惯性坐标系下的俯仰角、滚转角和偏航角统称为姿态角。 

框架坐标系：当飞行载体上存在活动框架设备时，原点都取在活动的旋转中心上， 当框架的方位与俯仰处于零位时，其三个轴与载体坐标系重合。 

这三个坐标系之间关系如下式： 

${\left|\begin{array}{c}x\\ y\\ z\end{array}\right|}_{\mathrm{fz}}=\left|A\right|\·{\left|\begin{array}{c}x\\ y\\ z\end{array}\right|}_{\mathrm{dz}}=\left|A\right|\·\left|B\right|\·{\left|\begin{array}{c}x\\ y\\ z\end{array}\right|}_{\mathrm{cz}}$

上式中 ${\left|\begin{array}{c}x\\ y\\ z\end{array}\right|}_{\mathrm{fz}}$ 代表惯性坐标系， ${\left|\begin{array}{c}x\\ y\\ z\end{array}\right|}_{\mathrm{dz}}$ 代表载体坐标系， ${\left|\begin{array}{c}x\\ y\\ z\end{array}\right|}_{\mathrm{cz}}$ 等于 $\left|\begin{array}{c}1\\ 0\\ 0\end{array}\right|$

其中： 

|A|：在惯性坐标系中载体坐标系坐标轴的方向余弦矩阵； 

|B|：在载体坐标系中框架坐标系坐标轴的方向余弦矩阵； 

载体坐标系中空间量向惯性坐标系中转换时，需要将坐标系做三次旋转即得： 

$\left|A\right|=\left|\begin{array}{ccc}{\cos \mathrm{\υ}}_p{\cos \mathrm{\υ}}_T& {\sin \mathrm{\υ}}_p{\sin \mathrm{\υ}}_k-{\cos \mathrm{\υ}}_p{\cos \mathrm{\υ}}_k{\sin \mathrm{\υ}}_T& {\sin \mathrm{\υ}}_p{\cos \mathrm{\υ}}_k+{\cos \mathrm{\υ}}_p{\sin \mathrm{\υ}}_T{\sin \mathrm{\υ}}_k\\ {\sin \mathrm{\υ}}_T& {\cos \mathrm{\υ}}_T{\cos \mathrm{\υ}}_k& -{\cos \mathrm{\υ}}_T{\sin \mathrm{\υ}}_k\\ {-\cos \mathrm{\υ}}_T{\sin \mathrm{\υ}}_p& {\cos \mathrm{\υ}}_p{\sin \mathrm{\υ}}_k+{\sin \mathrm{\υ}}_p{\cos }_k{\sin \mathrm{\υ}}_T& {-\sin \mathrm{\υ}}_p{\sin \mathrm{\υ}}_T{\sin \mathrm{\υ}}_k\end{array}\right|$

υ_р,υ_Т,υ_к为载体姿态信息。 

框架坐标系中空间量向载体坐标系转换时由于载体坐标系横滚与框架坐标系横滚固连，所以只需将坐标系在航向角和俯仰角上作两次旋转即得： 

$\left|B\right|=\left|\begin{array}{ccc}\cos \mathrm{\β}\cos \mathrm{\ϵ}& -\cos \mathrm{\β}\sin \mathrm{\ϵ}& \sin \mathrm{\β}\\ \sin \mathrm{\ϵ}& \cos \mathrm{\ϵ}& 0\\ -\cos \mathrm{\ϵ}\sin \mathrm{\β}& \sin \mathrm{\β}\sin \mathrm{\ϵ}& \cos \mathrm{\β}\end{array}\right|$

其中：ε，β为天线俯仰和方位测角数据； 

本实施例步骤如下： 

1>.测出时间t时刻框架坐标系方位和俯仰的角度，通过空间坐标变换将其投影到到惯性坐标系。 

$\left\{\begin{array}{c}\left|\begin{array}{c}{x}_{\mathrm{ft}}\\ y_{\mathrm{ft}}\\ z_{\mathrm{ft}}\end{array}\right|=\left|A\left(\begin{array}{ccc}{\mathrm{\υ}}_{\mathrm{pt}}& {\mathrm{\υ}}_{\mathrm{Tt}}& {\mathrm{\υ}}_{\mathrm{kt}}\end{array}\right)\right|\·\left|B\left(\begin{array}{ccc}{\mathrm{\β}}_t& {\mathrm{\ϵ}}_t& 0\end{array}\right)\right|\·\left|\begin{array}{c}1\\ 0\\ 0\end{array}\right|\\ {\mathrm{\ϵ}}_{\mathrm{ft}}=\arcsin y_{\mathrm{ft}}\\ {\mathrm{\β}}_{\mathrm{ft}}=-\mathrm{arctg}\frac{z_{\mathrm{ft}}}{x_{\mathrm{ft}}}\\ \end{array}\right.$

其中：x_ft，y_ft，z_ft为俯仰和方位的角度在惯性坐标系下投影的直角坐标值；ε_ft，β_ft为俯仰和方位的角度在惯性坐标系下投影的极坐标值；A(υ_рtυ_Ttυ_kt)为弹体坐标系向惯性坐标系转换所乘的方向余弦矩阵，其中υ_рt,υ_Тt,υ_кt为t时刻弹体姿态信息；B(β_tε_t0)为框架坐标系向弹体坐标系转换所乘的方向余弦矩阵，其中ε_t，β_t为t时刻位标器俯仰和方位测角数据； 

2>.测出时间t+Δt时刻框架坐标系方位和俯仰的角度，通过空间坐标变换将其投影到到惯性坐标系。 

$\left[\begin{array}{c}\left|\begin{array}{c}{x}_{f(t+\mathrm{\Δt})}\\ y_{f(t+\mathrm{\Δt})}\\ z_{f(t+\mathrm{\Δt})}\end{array}\right|=\left|A\left(\begin{array}{ccc}{\mathrm{\υ}}_{p(t+\mathrm{\Δt})}& {\mathrm{\υ}}_{T(t+\mathrm{\Δt})}& {\mathrm{\υ}}_{k(t+\mathrm{\Δt})}\end{array}\right)\right|\·\left|B\left(\begin{array}{ccc}{\mathrm{\β}}_{(t+\mathrm{\Δt})}& {\mathrm{\ϵ}}_{(t+\mathrm{\Δt})}& 0\end{array}\right)\right|\·\left|\begin{array}{c}1\\ 0\\ 0\end{array}\right|\\ {\mathrm{\ϵ}}_{f(t+\mathrm{\Δt})}=\arcsin y_{f(t+\mathrm{\Δt})}\\ {\mathrm{\β}}_{f(t+\mathrm{\Δt})}=-\mathrm{arctg}\frac{z_{f(t+\mathrm{\Δt})}}{x_{f(t+\mathrm{\Δt})}}\\ \end{array}\right]$

3>.将t时刻和t+Δt时刻角度在惯性坐标系的投影求差，即 

Δε_ft(Δt)＝ε_f(t+Δt)-ε_ft

Δβ_ft(Δt)＝β_f(t+Δt)-β_ft

4>.陀螺测得框架在惯性坐标系中角速度ω_y,ω_z，由欧拉运动方程得到

 和角的变化速度： 

${\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{ft}}=\frac{{\mathrm{\ω}}_z}{\cos \mathrm{\β}}$ ${\stackrel{\·}{\mathrm{\β}}}_{\mathrm{ft}}={\mathrm{\ω}}_y$

5>.对

和

进行积分： $\mathrm{\Δ}{\mathrm{\ϵ}}_{\mathrm{fti}}\left(\mathrm{\Δt}\right)=\mathrm{\Δt}\underset{j=t}{\overset{t+\mathrm{\Δt}}{\mathrm{\Σ}}}{\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{ft}},$ ${\mathrm{\Δ\β}}_{\mathrm{fti}}\left(\mathrm{\Δt}\right)=\mathrm{\Δt}\underset{j=t}{\overset{t+\mathrm{\Δt}}{\mathrm{\Σ}}}{\stackrel{\·}{\mathrm{\β}}}_{\mathrm{ft}};$

6>.计算在惯性坐标系中△t时间内天线固有漂移速度为： 

${\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{f\Δt}}=\frac{{\mathrm{\Δ\ϵ}}_{\mathrm{fti}}\left(\mathrm{\Δt}\right)-{\mathrm{\Δ\ϵ}}_{\mathrm{ft}}\left(\mathrm{\Δt}\right)}{\mathrm{\Δt}}$

${\stackrel{\·}{\mathrm{\β}}}_{\mathrm{f\Δt}}=\frac{{\mathrm{\Δ\β}}_{\mathrm{fti}}\left(\mathrm{\Δt}\right)-{\mathrm{\Δ\β}}_{\mathrm{ft}}\left(\mathrm{\Δt}\right)}{\mathrm{\Δt}};$

7>漂移角速度在惯性坐标系坐标轴上的投影为： 

$\left\{\begin{array}{c}{\mathrm{\Δ\ω}}_{\mathrm{x\Δt}}={\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{f\Δt}}\sin {\mathrm{\β}}_{\mathrm{ft}}\\ {\mathrm{\Δ\ω}}_{\mathrm{y\Δt}}={\stackrel{\·}{\mathrm{\β}}}_{\mathrm{f\Δt}}\\ {\mathrm{\Δ\ω}}_{\mathrm{z\Δt}}={\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{f\Δt}}\cos {\mathrm{\β}}_{\mathrm{ft}}\end{array}\right.;$

8>每隔△t时间按照上述步骤计算一次飘移值。然后将此飘移值与陀螺测得框架在惯性坐标系中角速度ω_y,ω_z求和，即得到校正后的框架角速度ω_yt，ω_zt如下。 

ω_yt＝ω_y+Δω_yΔtω_zt＝ω_z+Δω_zΔt 。

Claims (1)

1.一种MEMS陀螺零位飘移的校正方法，其特征在于步骤如下：

步骤1：测出时间t时刻的框架坐标系方位和俯仰的角度，通过空间坐标变换将其投影到到惯性坐标系：

$\left\{\begin{array}{c}\left|\begin{array}{c}{x}_{\mathrm{ft}}\\ y_{\mathrm{ft}}\\ z_{\mathrm{ft}}\end{array}\right|=\left|A\left(\begin{array}{ccc}{\mathrm{\υ}}_{\mathrm{pt}}& {\mathrm{\υ}}_{\mathrm{Tt}}& {\mathrm{\υ}}_{\mathrm{kt}}\end{array}\right)\right|\·\left|B\left(\begin{array}{ccc}{\mathrm{\β}}_t& {\mathrm{\ϵ}}_t& 0\end{array}\right)\right|\·\left|\begin{array}{c}1\\ 0\\ 0\end{array}\right|\\ {\mathrm{\ϵ}}_{\mathrm{ft}}=\arcsin y_{\mathrm{ft}}\\ {\mathrm{\β}}_{\mathrm{ft}}=-\mathrm{arctg}\frac{z_{\mathrm{ft}}}{x_{\mathrm{ft}}}\\ \end{array}\right.$

其中：x_ft，y_ft，z_ft为俯仰和方位的角度在惯性坐标系下投影的直角坐标值；ε_ft，β_ft为俯仰和方位的角度在惯性坐标系下投影的极坐标值；A(υ_ptυ_Ttυ_kt)为弹体坐标系向惯性坐标系转换所乘的方向余弦矩阵，其中υ_pt，υ_Tt，υ_kt为t时刻弹体姿态信息；B(β_tε_t0)为框架坐标系向弹体坐标系转换所乘的方向余弦矩阵，其中ε_t，β_t为t时刻位标器俯仰和方位测角数据；

所述框架坐标系：原点都取在活动的旋转中心上，当框架的方位与俯仰处于零位时，其三个轴与载体坐标系重合；

所述惯性坐标系：坐标系的原点取在地心，各坐标轴相对于恒星的指向不变化；

所述弹体坐标系：坐标系的原点与导弹的质心重合，X轴沿导弹纵轴指向前方，Z轴沿导弹横轴指向右方，Y轴沿导弹竖轴指向天空并与X轴、Z轴构成右手直角坐标系；导弹相对于惯性坐标系下的俯仰角、滚转角和偏航角统称为姿态角；

步骤2：测出时间t+Δt时刻的框架坐标系中方位和俯仰的角度，通过空间坐标变换将其投影到到惯性坐标系：

$\left[\begin{array}{c}\left|\begin{array}{c}{x}_{f(t+\mathrm{\Δt})}\\ y_{f(t+\mathrm{\Δt})}\\ z_{f(t+\mathrm{\Δt})}\end{array}\right|=\left|A\left(\begin{array}{ccc}{\mathrm{\υ}}_{p(t+\mathrm{\Δt})}& {\mathrm{\υ}}_{T(t+\mathrm{\Δt})}& {\mathrm{\υ}}_{k(t+\mathrm{\Δt})}\end{array}\right)\right|\·\left|B\left(\begin{array}{ccc}{\mathrm{\β}}_{(t+\mathrm{\Δt})}& {\mathrm{\ϵ}}_{(t+\mathrm{\Δt})}& 0\end{array}\right)\right|\·\left|\begin{array}{c}1\\ 0\\ 0\end{array}\right|\\ {\mathrm{\ϵ}}_{f(t+\mathrm{\Δt})}=\arcsin y_{f(t+\mathrm{\Δt})}\\ {\mathrm{\β}}_{f(t+\mathrm{\Δt})}=-\mathrm{arctg}\frac{z_{f(t+\mathrm{\Δt})}}{x_{f(t+\mathrm{\Δt})}}\\ \end{array}\right]$

步骤3：将t时刻和t+△t时刻角度在惯性坐标系的投影求差：

Δε_ft(Δt)＝ε_f(t+Δt)-ε_ft

Δβ_ft(Δt)＝β_f(t+Δt)-β_ft

步骤4：利用陀螺测得框架在惯性坐标系中角速度ω_y,ω_z，根据欧拉运动方程得到

和角的变化速度：

${\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{ft}}=\frac{{\mathrm{\ω}}_z}{\cos \mathrm{\β}},$ ${\stackrel{\·}{\mathrm{\β}}}_{\mathrm{ft}}={\mathrm{\ω}}_y;$

步骤5：对

和

进行积分：

${\mathrm{\Δ\ϵ}}_{\mathrm{fti}}\left(\mathrm{\Δt}\right)=\mathrm{\Δt}\underset{j=t}{\overset{t+\mathrm{\Δt}}{\mathrm{\Σ}}}{\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{ft}}$

${\mathrm{\Δ\β}}_{\mathrm{fti}}\left(\mathrm{\Δt}\right)=\mathrm{\Δt}\underset{j=t}{\overset{t+\mathrm{\Δt}}{\mathrm{\Σ}}}{\mathrm{\β}}_{\mathrm{ft}};$

步骤6：计算在惯性坐标系中△t时间内天线固有漂移速度为：

${\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{f\Δt}}=\frac{{\mathrm{\Δ\ϵ}}_{\mathrm{fti}}\left(\mathrm{\Δt}\right)-{\mathrm{\Δ\ϵ}}_{\mathrm{ft}}\left(\mathrm{\Δt}\right)}{\mathrm{\Δt}}$

${\stackrel{\·}{\mathrm{\β}}}_{\mathrm{f\Δt}}=\frac{{\mathrm{\Δ\β}}_{\mathrm{fti}}\left(\mathrm{\Δt}\right)-{\mathrm{\Δ\β}}_{\mathrm{ft}}\left(\mathrm{\Δt}\right)}{\mathrm{\Δt}}$

步骤7：漂移角速度在惯性坐标系坐标轴上的投影为：

$\left\{\begin{array}{c}{\mathrm{\Δ\ω}}_{\mathrm{x\Δt}}={\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{f\Δt}}\sin {\mathrm{\β}}_{\mathrm{ft}}\\ {\mathrm{\Δ\ω}}_{\mathrm{y\Δt}}={\stackrel{\·}{\mathrm{\β}}}_{\mathrm{f\Δt}}\\ {\mathrm{\Δ\ω}}_{\mathrm{z\Δt}}={\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{f\Δt}}\cos {\mathrm{\β}}_{\mathrm{ft}}\end{array}\right.;$

步骤8：每隔△t时间计算一次上述步骤的飘角速度移，然后将此飘移角速度与陀螺测得框架在惯性坐标系中角速度ω_y,ω_z求和，即得到校正后的框架角速度ω_yt，ω_zt如下：

ω_yt＝ω_y+Δω_yΔt

ω_zt＝ω_z+Δω_zΔt。