CN105318876A - 一种惯性里程计组合高精度姿态测量方法 - Google Patents

Abstract

本发明属于导航方法技术领域，具体涉及一种惯性/里程计组合高精度姿态测量方法，可应用于车载长航时组合导航，复杂地形测绘等领域。本发明技术方案以航位推算结果为基准，进行组合卡尔曼滤波计算，对水平失准角和水平陀螺漂移进行估计并修正。其中卡尔曼滤波状态方程取19个状态变量，建立了系统误差模型，利用航位推算结果，对水平失准角和水平陀螺漂移进行实时在线估计，并进行反馈修正，提高姿态精度，从而有效的提高了系统组合导航精度。

Description

一种惯性里程计组合高精度姿态测量方法

技术领域

本发明属于导航方法技术领域，具体涉及一种惯性/里程计组合高精度姿态测量方法，可应用于车载长航时组合导航，复杂地形测绘等领域。

背景技术

对自主性、可靠性、精度等要求高的车辆导航应用场合,常使用多传感器组合导航系统,尤其在军用场合,组合导航更是首选方案。惯性导航系统可以完全自主地提供全面的导航信息,但其误差会随着时间不断积累。里程计作为车载导航系统中十分重要的组成部分，其特点是成本较低，且误差不随时间积累，可以测得较精确的车速或里程，所以可以将惯导系统与里程计进行组合，以取得较高精度的测量结果。

惯性/里程计组合的常用方法为航位推算，即利用惯导系统进行姿态测量，利用里程计进行速度或路程测量，并经过刻度系数修正后，借助惯导系统的姿态信息，从载体坐标系转换到导航坐标系，从而得到载体不随时间发散的、较精确的位置信息。该方法的精度主要受里程计刻度系数和姿态精度的影响，因此如何提高姿态测量精度，成为影响惯性/里程计组合导航精度的主要因素。

综上，亟需提出一种基于惯性/里程计的高精度姿态测量方法，将航位推算结果和惯性导航解算结果进行组合，利用卡尔曼滤波算法对惯导系统姿态误差和陀螺漂移误差进行估计并修正，从而提高系统姿态测量精度。

发明内容

本发明要解决的技术问题是提出一种惯性/里程计组合高精度姿态测量方法，对惯导系统姿态误差和陀螺漂移误差进行估计并修正，提高系统姿态测量精度，进而提高系统组合导航精度。

为了实现这一目的，本发明采取的技术方案是：

一种惯性里程计组合高精度姿态测量方法，包括以下步骤：

(1)确定卡尔曼滤波状态方程状态变量

卡尔曼滤波状态方程取19个状态变量，分别为：北向速度误差δV_N、天向速度误差δV_U、东向速度误差δV_E、纬度误差高度误差δh、经度误差δλ、北向失准角φ_N、天向失准角φ_U、东向失准角φ_E、X陀螺漂移ε_x、Y陀螺漂移ε_y、Z陀螺漂移ε_z、X加表零偏Y加表零偏Z加表零偏航位推算纬度误差航位推算高度误差δh_D、航位推算经度误差δλ_D、里程计刻度系数误差δK_D；

(2)确定系统各误差方程

a)速度误差方程

式中：

V_N、V_U、V_E——北向、天向、东向速度，单位：m/s；

RM——子午圈半径，单位：m；

RN——卯酉圈半径，单位：m；

h——惯性高度，单位：m；

ω_ie——地球自转角速率，常数，取7.292115×10^-5rad/s；

——纬度，单位：rad；

f_N、f_U、f_E——滤波计算用n系下三轴比力分量，单位：m/s²；

C₁₁、C₂₁、C₃₁、C₁₂、C₂₂、C₃₂、C₁₃、C₂₃、C₃₃——姿态矩阵中的元素；

b)位置误差方程

c)姿态角误差方程

d)陀螺和加速度计误差方程

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_i=0\\ {\stackrel{\·}{\▿}}_i=0\end{array}\right.(i=x,y,z)\·\·\·\left(4\right)$

e)航位推算位置误差方程

f)里程计刻度系数误差方程

$\mathrm{\δ}{\stackrel{\·}{K}}_D=0\·\·\·\left(6\right)$

综合式(1)～(6)，可得系统状态方程如下：

$\stackrel{\·}{X}\left(t\right)=\mathrm{FX}\left(t\right)+W\left(t\right)\·\·\·\left(7\right)$

式中：

X(t)——系统状态变量，即上述的卡尔曼滤波状态方程19个状态变量；

F——根据公式(1)～(6)确定的系统矩阵；

W(t)——系统噪声；

(3)确定系统观测方程

系统观测量取导航解算位置与航位推算位置之差，则系统观测方程如下：

式中：

Z(t)——观测变量；

H——观测矩阵；

V(t)——观测噪声；

λ、h——惯性纬度、经度、高度，单位：rad，rad，m；

λ_D、h_D——航位推算纬度、经度、高度，单位：rad，rad，m；

将公式(7)和(8)离散化，可得：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+W_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.\·\·\·\left(9\right)$

式中：

X_k——状态变量；

Φ_k,k-1——上一滤波周期至当前滤波周期的一步转移矩阵；

Z_k——观测量；

H_k——观测矩阵；

W_k——系统激励噪声序列；

V_k——观测噪声序列；

一步转移矩阵Φ_k,k-1可用下式计算：

${\mathrm{\Φ}}_{k,k-1}=I+\underset{i=1}{\overset{n}{\mathrm{\Σ}}}F{T}_n\·\·\·\left(10\right)$

式中：

I——单位矩阵；

T_n——导航周期；

n——相邻两次滤波时刻之间的导航周期个数；

(4)确定卡尔曼滤波递推方程组

卡尔曼滤波递推方程组如下：

a)状态一步预测

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\·\·\·\left(11\right)$

b)状态估计

${\hat{X}}_{k/k-1}{=\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})\·\·\·\left(12\right)$

c)滤波增益

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\·\·\·\left(13\right)$

d)一步预测均方误差

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}\·\·\·\left(14\right)$

e)估计均方误差

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T\·\·\·\left(15\right)$

式中：

——当前滤波周期的实时状态估计值；

——上一滤波周期对当前滤波周期的状态预测值；

K_k——当前滤波周期的滤波增益阵；

P_k/k-1——上一滤波周期对当前滤波周期的预测误差估计的协方差阵；

P_k——当前滤波周期的实时误差估计协方差阵；

Q_k-1——系统噪声方差阵；

R_k——观测噪声方差阵；

(5)卡尔曼滤波递推计算和反馈修正

经过卡尔曼滤波递推计算后，估计出北向失准角φ_N、东向失准角φ_E、X陀螺漂移ε_x、Z陀螺漂移ε_z；然后对其进行反馈修正以提高姿态精；修正方法如下：

a)姿态修正，用表示修正后的姿态矩阵

失准角计算公式如下：

$\begin{array}{c}{\mathrm{\φ}}_N={\hat{X}}_k\left(7\right)\\ {\mathrm{\φ}}_U={\hat{X}}_k\left(8\right)=0\\ {\mathrm{\φ}}_E={\hat{X}}_k\left(9\right)\end{array}\·\·\·\left(16\right)$

X_k(i)，i＝1～19，表示上述的卡尔曼滤波状态方程19个状态变量中的第i个状态变量；

姿态修正公式如下：

$C_F=\left[\begin{array}{ccc}c{\mathrm{\φ}}_{E}c{\mathrm{\φ}}_U& s{\mathrm{\φ}}_E& -c{\mathrm{\φ}}_{E}s{\mathrm{\φ}}_U\\ -c{\mathrm{\φ}}_{N}s{\mathrm{\φ}}_{E}c{\mathrm{\φ}}_U+s{\mathrm{\φ}}_{N}s{\mathrm{\φ}}_U& c{\mathrm{\φ}}_{N}c{\mathrm{\φ}}_E& c{\mathrm{\φ}}_{N}s{\mathrm{\φ}}_{E}s{\mathrm{\φ}}_U+s{\mathrm{\φ}}_{N}c{\mathrm{\φ}}_U\\ s{\mathrm{\φ}}_{N}s{\mathrm{\φ}}_{E}c{\mathrm{\φ}}_U+c{\mathrm{\φ}}_{N}s{\mathrm{\φ}}_U& -s{\mathrm{\φ}}_{N}c{\mathrm{\φ}}_E& -s{\mathrm{\φ}}_N{\mathrm{s\φ}}_{E}s{\mathrm{\φ}}_U+c{\mathrm{\φ}}_{N}c{\mathrm{\φ}}_U\end{array}\right]\·\·\left(17\right)$

式中：c代表cos，s代表sin；

$C_n^{b1}=C_n^b\·C_F\·\·\·\left(18\right)$

b)陀螺漂移修正

陀螺漂移修正公式如下：

$\begin{array}{c}{\mathrm{\ϵ}}_x={\hat{X}}_k\left(10\right)\\ {\mathrm{\ϵ}}_y={\hat{X}}_k\left(11\right)=0\\ {\mathrm{\ϵ}}_z={\hat{X}}_k\left(12\right)\end{array}\·\·\·\left(19\right)$

将陀螺漂移补偿到载体系下的角速率中，实时参与导航解算：

${\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{ib}}^b=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ibx}0}^b\\ {\mathrm{\ω}}_{\mathrm{iby}0}^b\\ {\mathrm{\ω}}_{\mathrm{ibz}0}^b\end{array}\right]-\left[\begin{array}{c}{\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]\·\·\·\left(20\right)$

式中：

表示导航周期内载体坐标系下陀螺角速率输出，单位：rad/s。

本发明提出了一种惯性/里程计组合高精度姿态测量方法，主要测量车辆三维姿态、三维速度、三维位置等导航信息。本发明技术方案以航位推算结果为基准，进行组合卡尔曼滤波计算，对水平失准角和水平陀螺漂移进行估计并修正。其中卡尔曼滤波状态方程取19个状态变量，建立了系统误差模型，利用航位推算结果，对水平失准角和水平陀螺漂移进行实时在线估计，并进行反馈修正，提高姿态精度，从而有效的提高了系统组合导航精度。

具体实施方式

下面结合具体实施方式对本发明技术方案进行进一步详细说明。

一种惯性里程计组合高精度姿态测量方法，包括以下步骤：

(1)确定卡尔曼滤波状态方程状态变量

卡尔曼滤波状态方程取19个状态变量，分别为：北向速度误差δV_N、天向速度误差δV_U、东向速度误差δV_E、纬度误差高度误差δh、经度误差δλ、北向失准角φ_N、天向失准角φ_U、东向失准角φ_E、X陀螺漂移ε_x、Y陀螺漂移ε_y、Z陀螺漂移ε_z、X加表零偏Y加表零偏Z加表零偏航位推算纬度误差航位推算高度误差δh_D、航位推算经度误差δλ_D、里程计刻度系数误差δK_D；

(2)确定系统各误差方程

g)速度误差方程

式中：

V_N、V_U、V_E——北向、天向、东向速度，单位：m/s；

RM——子午圈半径，单位：m；

RN——卯酉圈半径，单位：m；

h——惯性高度，单位：m；

ω_ie——地球自转角速率，常数，取7.292115×10^-5rad/s；

——纬度，单位：rad；

f_N、f_U、f_E——滤波计算用n系下三轴比力分量，单位：m/s²；

C₁₁、C₂₁、C₃₁、C₁₂、C₂₂、C₃₂、C₁₃、C₂₃、C₃₃——姿态矩阵中的元素；

h)位置误差方程

i)姿态角误差方程

j)陀螺和加速度计误差方程

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_i=0\\ {\stackrel{\·}{\▿}}_i=0\end{array}\right.(i=x,y,z)\·\·\·\left(4\right)$

k)航位推算位置误差方程

l)里程计刻度系数误差方程

$\mathrm{\δ}{\stackrel{\·}{K}}_D=0\·\·\·\left(26\right)$

综合式(21)～(26)，可得系统状态方程如下：

$\stackrel{\·}{X}\left(t\right)=\mathrm{FX}\left(t\right)+W\left(t\right)\·\·\·\left(27\right)$

式中：

X(t)——系统状态变量，即上述的卡尔曼滤波状态方程19个状态变量；

F——根据公式(21)～(26)确定的系统矩阵；

W(t)——系统噪声；

(3)确定系统观测方程

系统观测量取导航解算位置与航位推算位置之差，则系统观测方程如下：

式中：

Z(t)——观测变量；

H——观测矩阵；

V(t)——观测噪声；

λ、h——惯性纬度、经度、高度，单位：rad，rad，m；

λ_D、h_D——航位推算纬度、经度、高度，单位：rad，rad，m；

将公式(27)和(28)离散化，可得：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+W_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.\·\·\·\left(29\right)$

式中：

X_k——状态变量；

Φ_k,k-1——上一滤波周期至当前滤波周期的一步转移矩阵；

Z_k——观测量；

H_k——观测矩阵；

W_k——系统激励噪声序列；

V_k——观测噪声序列；

一步转移矩阵Φ_k,k-1可用下式计算：

${\mathrm{\Φ}}_{k,k-1}=I+\underset{i=1}{\overset{n}{\mathrm{\Σ}}}F{T}_n\·\·\·\left(30\right)$

式中：

I——单位矩阵；

T_n——导航周期；

n——相邻两次滤波时刻之间的导航周期个数；

(4)确定卡尔曼滤波递推方程组

卡尔曼滤波递推方程组如下：

f)状态一步预测

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\·\·\·\left(31\right)$

g)状态估计

${\hat{X}}_{k/k-1}{=\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})\·\·\·\left(32\right)$

h)滤波增益

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\·\·\·\left(33\right)$

i)一步预测均方误差

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}\·\·\·\left(34\right)$

j)估计均方误差

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T\·\·\·\left(35\right)$

式中：

——当前滤波周期的实时状态估计值；

——上一滤波周期对当前滤波周期的状态预测值；

K_k——当前滤波周期的滤波增益阵；

P_k/k-1——上一滤波周期对当前滤波周期的预测误差估计的协方差阵；

P_k——当前滤波周期的实时误差估计协方差阵；

Q_k-1——系统噪声方差阵；

R_k——观测噪声方差阵；

(5)卡尔曼滤波递推计算和反馈修正

经过卡尔曼滤波递推计算后，估计出北向失准角φ_N、东向失准角φ_E、X陀螺漂移ε_x、Z陀螺漂移ε_z；然后对其进行反馈修正以提高姿态精；修正方法如下：

c)姿态修正，用表示修正后的姿态矩阵

失准角计算公式如下：

$\begin{array}{c}{\mathrm{\φ}}_N={\hat{X}}_k\left(7\right)\\ {\mathrm{\φ}}_U={\hat{X}}_k\left(8\right)=0\\ {\mathrm{\φ}}_E={\hat{X}}_k\left(9\right)\end{array}\·\·\·\left(36\right)$

X_k(i)，i＝1～19，表示上述的卡尔曼滤波状态方程19个状态变量中的第i个状态变量；

姿态修正公式如下：

$C_F=\left[\begin{array}{ccc}c{\mathrm{\φ}}_{E}c{\mathrm{\φ}}_U& s{\mathrm{\φ}}_E& -c{\mathrm{\φ}}_{E}s{\mathrm{\φ}}_U\\ -c{\mathrm{\φ}}_{N}s{\mathrm{\φ}}_{E}c{\mathrm{\φ}}_U+s{\mathrm{\φ}}_{N}s{\mathrm{\φ}}_U& c{\mathrm{\φ}}_{N}c{\mathrm{\φ}}_E& c{\mathrm{\φ}}_{N}s{\mathrm{\φ}}_{E}s{\mathrm{\φ}}_U+s{\mathrm{\φ}}_{N}c{\mathrm{\φ}}_U\\ s{\mathrm{\φ}}_{N}s{\mathrm{\φ}}_{E}c{\mathrm{\φ}}_U+c{\mathrm{\φ}}_{N}s{\mathrm{\φ}}_U& -s{\mathrm{\φ}}_{N}c{\mathrm{\φ}}_E& -s{\mathrm{\φ}}_N{\mathrm{s\φ}}_{E}s{\mathrm{\φ}}_U+c{\mathrm{\φ}}_{N}c{\mathrm{\φ}}_U\end{array}\right]\·\·\left(37\right)$

式中：c代表cos，s代表sin；

$C_n^{b1}=C_n^b\·C_F\·\·\·\left(38\right)$

d)陀螺漂移修正

陀螺漂移修正公式如下：

$\begin{array}{c}{\mathrm{\ϵ}}_x={\hat{X}}_k\left(10\right)\\ {\mathrm{\ϵ}}_y={\hat{X}}_k\left(11\right)=0\\ {\mathrm{\ϵ}}_z={\hat{X}}_k\left(12\right)\end{array}\·\·\·\left(39\right)$

将陀螺漂移补偿到载体系下的角速率中，实时参与导航解算：

${\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{ib}}^b=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ibx}0}^b\\ {\mathrm{\ω}}_{\mathrm{iby}0}^b\\ {\mathrm{\ω}}_{\mathrm{ibz}0}^b\end{array}\right]-\left[\begin{array}{c}{\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]\·\·\·\left(40\right)$

式中：

表示导航周期内载体坐标系下陀螺角速率输出，单位：rad/s。

Claims (2)

1.一种惯性里程计组合高精度姿态测量方法，其特征在于，包括以下步骤：

(1)确定卡尔曼滤波状态方程状态变量

卡尔曼滤波状态方程取19个状态变量，分别为：北向速度误差δV_N、天向速度误差δV_U、东向速度误差δV_E、纬度误差高度误差δh、经度误差δλ、北向失准角φ_N、天向失准角φ_U、东向失准角φ_E、X陀螺漂移ε_x、Y陀螺漂移ε_y、Z陀螺漂移ε_z、X加表零偏Y加表零偏Z加表零偏航位推算纬度误差航位推算高度误差δh_D、航位推算经度误差δλ_D、里程计刻度系数误差δK_D；

(2)确定系统各误差方程

a)速度误差方程

式中：

V_N、V_U、V_E——北向、天向、东向速度，单位：m/s；

RM——子午圈半径，单位：m；

RN——卯酉圈半径，单位：m；

h——惯性高度，单位：m；

ω_ie——地球自转角速率，常数，取7.292115×10^-5rad/s；

——纬度，单位：rad；

f_N、f_U、f_E——滤波计算用n系下三轴比力分量，单位：m/s²；

C₁₁、C₂₁、C₃₁、C₁₂、C₂₂、C₃₂、C₁₃、C₂₃、C₃₃——姿态矩阵中的元素；

b)位置误差方程

c)姿态角误差方程

d)陀螺和加速度计误差方程

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_i=0\\ {\stackrel{\·}{\▿}}_i=0\end{array}\right.(i=x,y,z)\·\·\·\left(4\right)$

e)航位推算位置误差方程

f)里程计刻度系数误差方程

$\mathrm{\δ}{\stackrel{\·}{K}}_D=0\·\·\·\left(6\right)$

综合式(1)～(6)，可得系统状态方程如下：

$\stackrel{\·}{X}\left(t\right)=\mathrm{FX}\left(t\right)+W\left(t\right)\·\·\·\left(7\right)$

式中：

X(t)——系统状态变量，即上述的卡尔曼滤波状态方程19个状态变量；

F——根据公式(1)～(6)确定的系统矩阵；

W(t)——系统噪声；

(3)确定系统观测方程

系统观测量取导航解算位置与航位推算位置之差，则系统观测方程如下：

式中：

Z(t)——观测变量；

H——观测矩阵；

V(t)——观测噪声；

λ、h——惯性纬度、经度、高度，单位：rad，rad，m；

λ_D、h_D——航位推算纬度、经度、高度，单位：rad，rad，m；

将公式(7)和(8)离散化，可得：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+W_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.\·\·\·\left(9\right)$

式中：

X_k——状态变量；

Φ_k,k-1——上一滤波周期至当前滤波周期的一步转移矩阵；

Z_k——观测量；

H_k——观测矩阵；

W_k——系统激励噪声序列；

V_k——观测噪声序列；

一步转移矩阵Φ_k,k-1可用下式计算：

${\mathrm{\Φ}}_{k,k-1}=I+\underset{i=1}{\overset{n}{\mathrm{\Σ}}}F{T}_n\·\·\·\left(10\right)$

式中：

I——单位矩阵；

T_n——导航周期；

n——相邻两次滤波时刻之间的导航周期个数；

(4)确定卡尔曼滤波递推方程组

卡尔曼滤波递推方程组如下：

a)状态一步预测

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\·\·\·\left(11\right)$

b)状态估计

${\hat{X}}_{k/k-1}{=\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})\·\·\·\left(12\right)$

c)滤波增益

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\·\·\·\left(13\right)$

d)一步预测均方误差

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}\·\·\·\left(14\right)$

e)估计均方误差

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T\·\·\·\left(15\right)$

式中：

——当前滤波周期的实时状态估计值；

——上一滤波周期对当前滤波周期的状态预测值；

K_k——当前滤波周期的滤波增益阵；

P_k/k-1——上一滤波周期对当前滤波周期的预测误差估计的协方差阵；

P_k——当前滤波周期的实时误差估计协方差阵；

Q_k-1——系统噪声方差阵；

R_k——观测噪声方差阵；

(5)卡尔曼滤波递推计算和反馈修正

经过卡尔曼滤波递推计算后，估计出北向失准角φ_N、东向失准角φ_E、X陀螺漂移ε_x、Z陀螺漂移ε_z；然后对其进行反馈修正以提高姿态精度。

2.如权利要求1所述的一种惯性里程计组合高精度姿态测量方法，其特征在于：步骤(5)中，对经过卡尔曼滤波递推计算后估计的出北向失准角φ_N、东向失准角φ_E、X陀螺漂移ε_x、Z陀螺漂移ε_z进行反馈修正，修正方法如下：

a)姿态修正，用表示修正后的姿态矩阵

失准角计算公式如下：

$\begin{array}{c}{\mathrm{\φ}}_N={\hat{X}}_k\left(7\right)\\ {\mathrm{\φ}}_U={\hat{X}}_k\left(8\right)=0\\ {\mathrm{\φ}}_E={\hat{X}}_k\left(9\right)\end{array}\·\·\·\left(16\right)$

X_k(i)，i＝1～19，表示上述的卡尔曼滤波状态方程19个状态变量中的第i个状态变量；

姿态修正公式如下：

$C_F=\left[\begin{array}{ccc}c{\mathrm{\φ}}_{E}c{\mathrm{\φ}}_U& s{\mathrm{\φ}}_E& -c{\mathrm{\φ}}_{E}s{\mathrm{\φ}}_U\\ -c{\mathrm{\φ}}_{N}s{\mathrm{\φ}}_{E}c{\mathrm{\φ}}_U+s{\mathrm{\φ}}_{N}s{\mathrm{\φ}}_U& c{\mathrm{\φ}}_{N}c{\mathrm{\φ}}_E& c{\mathrm{\φ}}_{N}s{\mathrm{\φ}}_{E}s{\mathrm{\φ}}_U+s{\mathrm{\φ}}_{N}c{\mathrm{\φ}}_U\\ s{\mathrm{\φ}}_{N}s{\mathrm{\φ}}_{E}c{\mathrm{\φ}}_U+c{\mathrm{\φ}}_{N}s{\mathrm{\φ}}_U& -s{\mathrm{\φ}}_{N}c{\mathrm{\φ}}_E& -s{\mathrm{\φ}}_N{\mathrm{s\φ}}_{E}s{\mathrm{\φ}}_U+c{\mathrm{\φ}}_{N}c{\mathrm{\φ}}_U\end{array}\right]\·\·\left(17\right)$

式中：c代表cos，s代表sin；

$C_n^{b1}=C_n^b\·C_F\·\·\·\left(18\right)$

b)陀螺漂移修正

陀螺漂移修正公式如下：

$\begin{array}{c}{\mathrm{\ϵ}}_x={\hat{X}}_k\left(10\right)\\ {\mathrm{\ϵ}}_y={\hat{X}}_k\left(11\right)=0\\ {\mathrm{\ϵ}}_z={\hat{X}}_k\left(12\right)\end{array}\·\·\·\left(19\right)$

将陀螺漂移补偿到载体系下的角速率中，实时参与导航解算：

${\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{ib}}^b=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ibx}0}^b\\ {\mathrm{\ω}}_{\mathrm{iby}0}^b\\ {\mathrm{\ω}}_{\mathrm{ibz}0}^b\end{array}\right]-\left[\begin{array}{c}{\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]\·\·\·\left(20\right)$

式中：

表示导航周期内载体坐标系下陀螺角速率输出，单位：rad/s。