CN114608583A - 一种高超声速飞行器动态杆臂效应智能补偿方法 - Google Patents

Abstract

本发明公开了一种高超声速飞行器动态杆臂效应智能补偿方法，属于惯性导航领域；具体包括：首先，针对高超声速飞行器，建立高动态弹性变形下的动态杆臂效应误差模型；并同时建立高超声速飞行器INS误差模型；然后对动态杆臂效应误差模型中的固有频率进行强化学习，实现动态环境下飞行器杆臂的补偿后代入原动态杆臂效应误差模型，进行INS/GNSS组合导航；最后将当前导航结果与GNSS的定位结果的误差作为强化学习标志位，当误差大于阈值时，采集距离飞行器当前时刻最近的N秒IMU和GNSS数据，对固有频率进行自适应学习，并反馈给动态杆臂效应误差模型继续滤波和定位，进行导航结果的修正。本发明具有准确率高、适应性强的优点。

Description

一种高超声速飞行器动态杆臂效应智能补偿方法

技术领域

本发明属于惯性导航领域，涉及一种高超声速飞行器动态杆臂效应智能补偿方法。

背景技术

在现代化武器飞行作战的环境下，对飞行器的精确打击提出了更高的要求。导航精度是 衡量武器精度的重要指标，而惯导系统由于其自主导航的优点，一般被选为飞行器的主导航 系统。但是惯导系统中的惯性组件在安装过程中一般很难满足与飞行器质心重合的要求，因 此惯组器件与飞行器质心间存在的杆臂长度会产生杆臂效应误差。为了提高飞行器的导航精 度，杆臂效应误差的补偿是十分重要的一步。

高超声速飞行器是目前备受关注的尖端武器，其高超声速与高机动、难拦截的特性受到 各国的追捧。但是其严酷的飞行环境也为其精确导航的实现增加了难度，在高超声速的基础 上还存在弹性变形与质心偏移的影响。而在高速及高机动的环境下，其惯组器件产生的杆臂 效应误差的影响也在进一步凸显。因此，研究高超声速飞行器惯组系统的动态杆臂效应误差 具有十分重要的意义。

现有的杆臂效应误差补偿方法一般分为力学补偿法和数字滤波法，见文献[1]。力学补偿 法的前提是杆臂长度已知，计算出杆臂效应误差从而实现对其补偿。数字滤波法的前提是杆 臂效应误差和加速度计输出信号之间的频率特性已知，从而利用滤波的方法实现对杆臂效应 误差的补偿。Liu Z,Wang L,Li K,et al.利用物理平台来分析惯性测量单元(IMU)的快速 旋转、加速度计与旋转中心之间的位置误差所引起的杆臂效应误差，见文献[2]。Chang G,Liu M.研究了具有恒定杆臂效应的INS/GPS组合导航系统，见文献[3]。Xiong Z,Peng H,Wang J,et al.提出了一种高超声速巡航飞行器捷联惯导系统杆臂效应的动态标定方法。通过将杆 臂长度作为卡尔曼滤波的状态变量，建立了杆臂效应模型，对杆臂长度进行标定，补偿杆臂 效应误差，提高捷联惯导系统的精度，见文献[4]。

但是，上述文献[1-4]中对于杆臂效应误差的补偿，采用的前提均是杆臂长度不发生改变 的情况，不能反映飞行器在实际飞行环境中的动态杆臂误差。虽然也有学者针对杆臂的挠性 变形进行补偿研究，见文献[5]，但是并没有考虑其他影响杆臂变化的因素，并且对于挠性变 形的模型参数也缺少更加详细的分析，只针对随机挠性变形进行了补偿，在适用性上还有待 完善。

发明内容

本发明针对高超声速飞行器复杂动态环境下的杆臂效应补偿，建立了更加完善的弹性变 形动态杆臂模型，并提出了一种高超声速飞行器动态杆臂效应智能补偿方法，有效提高了飞 行器的导航精度。

所述高超声速飞行器动态杆臂效应智能补偿方法，具体步骤如下：

步骤一、针对高超声速飞行器，基于现有的杆臂效应误差模型，建立高动态弹性变形下 的动态杆臂效应误差模型；

当考虑杆臂长度的变化时，弹性变形下的动态杆臂效应误差模型为：

其中，

为杆臂效应产生的加速度计误差，r_A为杆臂长度矢量；

为本体坐标系相 对惯性坐标系的转动角速度，

为

的反对称矩阵，r_A0代表初始杆臂长度；

ω_i为 第i阶振型的固有频率，ζ_i为第i阶振型的阻尼系数，W_i(x)表示飞行器纵轴各点的相对横移 关系，称为第i次固有振型函数；q为弹性振动的广义坐标，u_q为弹性变形控制量，w_q为弹 性变形噪声项，u_r为杆臂效应误差控制量，w_r为杆臂效应误差噪声项；

步骤二、建立高超声速飞行器INS误差模型；

步骤三、利用强化学习对动态杆臂效应误差模型中的固有频率进行学习，实现对动态环 境下飞行器杆臂的补偿，将补偿后的杆臂代入动态杆臂效应误差模型，并进行INS/GNSS 组合导航得到导航结果；

强化学习元素如下表1所示：

表1

将强化学习过程作为马尔可夫决策过程，即(s,α,P,r,ρ₀)；

其中s是一组状态量，α是一组动作，P：s×α×s→R≥0为由智能体表示的转移概率分 布，r：s×α→R为从环境中获得的奖励价值，ρ₀：s→R≥0是初始状态；

首先，将动态杆臂效应误差模型中的广义坐标q扩展到组合导航系统中的状态量X，即：

[φ_E φ_N φ_U]^T为飞行器惯导姿态失准角；[δV_E δV_N δV_U]^T为飞行器在导航系下的速度 误差；[δL δλ δh]^T为飞行器纬—经—高度位置误差；ε_bi为陀螺仪的常值漂移项，ε_ri为陀 螺仪的一阶马尔科夫过程漂移项，

为加速度计的一阶马尔科夫过程漂移项，q为弹性振动 的广义坐标。

然后，基于状态量X，得到动态杆臂效应误差在线标定系统的状态方程和量测方程；

其中，F(t)为系统矩阵，B(t)为系统控制矩阵，G(t)为系统状态噪声系数矩阵，W(t)为 系统噪声矩阵，H(t)为量测系数矩阵，V(t)为量测噪声矩阵，U(t)为控制矩阵。

最后，使用带确定性控制的卡尔曼滤波器，将各状态信息进行融合，得到组合导航的结 果；

步骤四、将当前导航结果与GNSS的定位结果的误差作为强化学习标志位，当误差大于 阈值时，采集距离飞行器当前时刻最近的N秒IMU和GNSS数据，对固有频率进行自适应学习，并反馈给态杆臂效应误差模型继续滤波和定位，进行导航结果的修正。

本发明的优点在于：

(1)本发明一种高超声速飞行器动态杆臂效应智能补偿方法，利用强化学习的方法自适 应估计动态杆臂模型参数，建立了更加完善的弹性变形动态杆臂模型，实现高超声速飞行器 在复杂动态环境下杆臂效应的误差补偿，具有准确率高、适应性强的优点。

附图说明

图1为本发明一种高超声速飞行器动态杆臂效应智能补偿方法的流程图；

图2为本发明传统杆臂效应的原理图；

图3为本发明动态杆臂对导航精度的影响；

图4为本发明飞行器弹性变形示意图；

图5为本发明高超声速飞行器结构和系统分布图；

图6为本发明根据导航结果对固有频率进行自适应学习修正导航结果的流程图；

图7为本发明与常规杆臂效应算法KF-FLAC对杆臂进行补偿的对比图；

图8为本发明高超声速飞行器在负载的动态环境下模拟飞行轨迹示意图；

图9为本发明高动态环境下杠杆臂的变化示意图；

图10为本发明RL-DLAC和KF-FLAC方法分别对动态杆臂进行标定的对比图；

图11为本发明高动态环境下常规算法KF-FLAC的导航误差图；

图12为本发明高动态环境下本发明算法RL-DLAC的导航误差图。

具体实施方式

下面将结合附图和实例对本发明作进一步的详细说明；

本发明一种高超声速飞行器动态杆臂效应智能补偿方法，简称RL-DLAC，通过强化学 习的方法自适应估计，使动态杆臂模型参数更加精确，达到了标定动态杆臂和补偿杆臂效应 误差的作用。

如图1所示，包括以下步骤：

步骤一、针对高超声速飞行器，基于现有的杆臂效应误差模型，建立高动态弹性变形下 的动态杆臂效应误差模型；

理想情况下，惯性仪表测得的比力信息应来自于惯性测量单元(inertialmeasurement unit，IMU)坐标系的同一测量点，但在实际系统中，由于飞行器结构的限制，IMU上加速 度计的质心与飞行器质心不一定重合，此时，若飞行器存在角运动，加速度计的输出中则包 含杆臂效应误差(由于IMU上3个加速度计距离较近，这里暂不考虑不同加速度计之间的 内杆臂误差)。

如图2所示，O_i和O_b分别为惯性坐标系和本体坐标系的中心；O_A为IMU的质心(这里将加速度计与陀螺仪看作一体的IMU元件)；r_A为杆臂长度矢量；ω_ib为本体坐标系相对惯性坐标系的转动角速度；根据坐标系转换得到传统杆臂效应产生的加速度计误差

在传统杆臂效应补偿方法中，关于

与

的处理，一般假定飞行器和IMU的相对位置 不发生改变，即

然后将杆臂长度扩展为组合导航系统中的状态量，利用卡尔曼滤 波进行估计；这种方法遇到杆臂变化的情况会影响导航精度，甚至导致导航误差发散，如图 3所示，随着振动模态的增强，导航精度逐渐下降。

飞行器结构具有一定的结构刚度，但是在外力作用下如气动力以及高动态机动时，飞行 器结构会产生变形和弯曲振动。这时IMU与飞行器质心间的杆臂长度已经不能按照常量来进 行滤波估计，同样也不能以简单的二阶马尔科夫过程来进行近似，因为实际的飞行器结构杆 臂变形并不是简单的随机过程，需要考虑实际的物理过程。改进挠性变形下的动态杆臂效应 误差模型的推导如下：

将高超声速飞行器近似为细长梁结构，沿纵向方向的横截面面积变化不大，因此可以用 简化的两端梁代替飞行器结构，其在空间的横向振动可视为两端自由的非均匀弹性梁的运动， 此梁的弹性位移、抗弯刚度及质量惯性沿长度的分布由飞行器结构参数决定。飞行器弹性变 形示意图如图4所示，对于长梁结构来说，其弹性挠曲位移方程为：

其中，W_i(x)表示飞行器纵轴各点的相对横移关系，称为第i次固有振型函数；q_i(t)为弹 性振动的广义坐标，在弹性振型函数确定后可决定弹性振动幅值，用于描述弯曲振型。

弹性振动广义坐标q_i(t)是随时间变化的量，它满足下列方程

其中，ω_i为第i阶振型的固有频率；ζ_i为第i阶振型的阻尼系数；Q_i为第i阶振型的广义 力；M_i为第i阶振型的广义质量；Q_i和M_i满足如下关系

其中，l_k为飞行器等效长度，f_yb(x)是作用于飞行器的外力在法向的投影，m(x)是沿飞 行器纵轴的质量分布；这里将飞行器的外力简化为引力、推力和气动力的作用，即

∑f_yb＝F_gyb+F_kyb+F_ayb

其中，F_gyb、F_kyb、F_ayb分别为引力、推力和气动力在O_bY_b轴的投影。

针对上述飞行器杆臂变形模型与随机挠性变形，得到其弹性振动广义坐标满足关系：

u_qi为弹性变形控制量，w_qi为弹性变形噪声项。

以三阶振型为例，得到弹性变形长度的关系：

δy(x,t)＝q₁(t)W₁(x)+q₂(t)W₂(x)+q₃(t)W₃(x)

根据几何关系得到当前杆臂长度关系：

r_A0代表初始杆臂长度；u_q为弹性变形控制量；w_q为弹性变形噪声项；

将上式带入杆臂效应误差方程得到：

w_r＝w_q

鉴于吸气式高超音速飞行器的广泛应用，采用其中一类飞行器结构进行研究。高超音速 飞行器的结构和系统分布图如图5所示，高超声速飞行器的前体和后体安装有设备子系统， 中部及两侧装有液态氧气和氢气储箱。

步骤二、建立高超声速飞行器INS误差模型；

为了对动态杆臂效应误差模型进行补偿，需要建立起杆臂效应误差影响下的IMU误差方 程(这里不考虑安装误差和标度因数误差)如下：

其中：δf^b为加速度计误差，包含漂移误差和杆臂效应误差，漂移误差满足一阶马尔科 夫过程，

T_a为加速度计相关时间，w_a为加速度计一阶马尔科夫驱动噪声，

为陀螺仪的漂移误差，包含常值漂移和一阶马尔科夫过程，即ε^b＝ε_b+ε_r+w_g，其中

ε_r为陀螺仪一阶马尔科夫过程误差，T_g为陀螺仪相关时间，w_r为陀螺仪一 阶马尔科夫驱动噪声。

以东—北—天地理坐标系作为导航坐标系，则INS的姿态误差方程为：

φ为飞行器惯导姿态失准角，

为地球自转角速度，

导航系相对于地球系的转动角速 度，εⁿ为陀螺仪漂移误差。

展开得到：

其中，[φ_E φ_N φ_U]^T为飞行器惯导姿态失准角；[V_E V_N V_U]^T为飞行器在导航系下的速 度；R_M为子午圈曲率半径，R_N为卯酉圈曲率半径，[L λ h]^T为飞行器纬—经—高度位置； ω_ie为地球自转角速度；误差项满足：

为本体坐标系到 导航坐标系的转换矩阵。

速度误差微分方程如下：

φⁿ为导航系下飞行器惯导姿态失准角，fⁿ为导航系下比力，Vⁿ为导航系下速度，

导 航系相对于地球系的转动角速度，展开得到：

其中，

为飞行器导航坐标系下的惯导比力；

比力误差项满足：

为本体坐标系到导航坐标系的 转换矩阵。

位置误差微分方程如下：

步骤三、利用强化学习对动态杆臂效应误差模型中的固有频率进行学习，实现对动态环 境下飞行器杆臂的补偿，将补偿后的杆臂代入动态杆臂效应误差模型，并进行INS/GNSS 组合导航得到导航结果；

强化学习模块的目标是学习动态杆臂模型中的固有频率，该固有频率能够使得利用本发 明算法得到当前采集数据的位置误差最小，强化学习模块的组成元素定义如表1所示：

表1

上述过程所实现的任务符合连续时间的马尔可夫决策过程(MDP)的标准接口；

即(s,α,P,r,ρ₀)，其中s是一组状态量，α是一组动作，P：s×α×s→R≥0为由智能体 表示的转移概率分布；r：s×α→R为从环境中获得的奖励价值。ρ₀：s→R≥0是初始状态；

这些特点可以将RL-DLAC转换为MDP的形式，其中动作导致状态转换，从而影响奖励 的提升值。通过捕捉动作和奖励之间的依赖关系，强化学习方法可以为观察到的状态找到良 好的提升策略，从而可以获得学习到最优状态。

组合导航作为杆臂效应补偿的常用算法，将INS与GNSS导航定位方法相结合来进行杆 臂估计。本发明将强化学习的环境因素定位为基于INS/GNSS的组合导航系统，这个环境 将给出当前动态杆臂模型的优缺点评估。

首先，将动态杆臂效应误差模型中的广义坐标q扩展到组合导航系统中的状态量X，即：

[φ_E φ_N φ_U]^T为飞行器惯导姿态失准角；[δV_E δV_N δV_U]^T为飞行器在导航系下的速度 误差；[δL δλ δh]^T为飞行器纬—经—高度位置误差；ε_bi为陀螺仪的常值漂移项，ε_ri为陀 螺仪的一阶马尔科夫过程漂移项，

为加速度计的一阶马尔科夫过程漂移项，q为弹性振动 的广义坐标。

然后，基于状态量X，得到动态杆臂效应误差在线标定系统的状态方程和量测方程；

其中，F(t)为系统矩阵，B(t)为系统控制矩阵，G(t)为系统状态噪声系数矩阵，W(t)为 系统噪声矩阵，H(t)为量测系数矩阵，V(t)为量测噪声矩阵，U(t)为控制矩阵。

最后，使用带确定性控制的卡尔曼滤波器，将各状态信息进行融合，得到组合导航的结 果；

融合表示为：

X_k＝Φ_k,k-1X_k-1+B_k-1u_k-1+Γ_k-1W_k-1

P_k＝(I-K_kH_k)P_k/k-1

其中，X_k为估计时刻k的状态；Z_k为估计时刻k的量测量；Φ_k,k-1为k-1时刻到k时刻的 状态转移矩阵；B_k-1为k-1时刻的系统控制矩阵，u_k-1为k-1时刻的控制量，Γ_k-1为k-1时刻的噪声矩阵，W_k-1为k-1时刻的噪声，P_k/k-1为k-1时刻到k时刻的状态协方差阵，R_k为k-1 时刻的量测噪声矩阵，Q_k-1为k-1时刻的系统噪声矩阵，I为单位矩阵，H_k为估计时刻k的 量测矩阵；P_k为时刻k估计状态的协方差矩阵；K_k为时刻k滤波器的增益矩阵。

一般飞行器在生产完成后通常会为相应类型的结构提供一组默认校准值。在本申请中， 设定弹性变形固有频率的初始值，如表2所示；实际飞行过程中，对于高超声速飞行器恶劣 的飞行环境而言，其固有频率会发生改变，这里将固有频率作为强化学习的状态量： ω＝[ω₁ ω₂ ω₃]^T；

表2

当环境收到导致状态改变的动作时，需要定义一个足够好的奖励机制来评估当前动作。 对于此问题，本申请将奖励建模为IMU和GNSS数据在N秒内定位误差的相反数；奖励值 被配置为与最终定位误差的相反数；通过最大化回报，将最终获得弹性变形固有频率的最优 值，并且可以最小化定位误差。

步骤四、将当前导航结果与GNSS的定位结果的误差作为强化学习标志位，当误差大于 阈值时，采集距离飞行器当前时刻最近的N秒IMU和GNSS数据，对固有频率进行自适应学习，并反馈给态杆臂效应误差模型继续滤波和定位，进行导航结果的修正。

高超声速飞行器在高速飞行中，随着燃料的消耗，质量下降；受高超声速飞行器表面气 动加热影响，表面温度的升高将降低材料的抗弯刚度，故而高超声速飞行器的固有频率振型 模态会发生改变。即实际飞行过程中的杆臂模型是变化且未知的，因此单纯利用地面标定的 模型参数会产生一定的偏差，本发明利用强化学习对其模型变化参数进行自适应学习，来对 动态杆臂模型造成的误差进行补偿，达到更精准的补偿效果。

如图6所示，该算法通过IMU获得陀螺仪的输入

和加速度的输入

经过漂移修正 和动态杆臂效应误差补偿环节得到惯导解算需要的

和f^b，给定初始状态X₀和初始状态协 方差矩阵P₀，如果当前时刻存在GNSS测量输入，将进行测量更新，得到最终的误差状态估 计结果

然后经过惯导修正后得到当前位置估计结果

在算法运行开始时，该过程中的 杆臂模型中的固有频率参数ω_i由默认值

(飞行器初始校准结果或先前的估计结果)提供， 这里采用三阶振动模型，即

考虑到全球导航卫星系统载波相位差分定位结果(r_G)具有精度高、环境要求高等特点， 该算法将对全球导航卫星系统的定位状态进行一致性监测，将计算当前组合导航系统与r_G的 定位误差。如果计算误差大于阈值，说明动态杆臂模型中的固有频率存在一定的偏差，导致 组合导航结果不准确。然后，采集距离当前时刻最近的N秒IMU和GNSS数据，对固有频 率进行自适应学习，最后将学习的固有频率反馈给先前的动态杆臂补偿系统继续滤波和定位。

对本发明RL-DLAC方法进行仿真验证，分别检验其在常规杆臂效应和动态杆臂效应下 的误差补偿结果，并将RL-DLAC算法和常态杆臂算法KF-FLAC进行对比，可以发现 RL-DLAC算法在解决动态杆臂效应补偿时具有更明显的优势。

在飞行器平稳飞行过程中，杆臂不发生明显变化的情况下利用常规杆臂效应算法KF-FLAC与本发明RL-DLAC算法对其进行补偿，杆臂长度取为1m，估计的杆臂长度如图 7所示。可以看出，RL-DLAC算法和KF-FLAC算法相比，对常态杆臂效应都能进行相应的 补偿，但是本发明RL-DLAC算法具有更高的精度。

在实际飞行过程中，高超声速飞行器面临着复杂的动态环境，如图8所示，根据实际高 超声速飞行器模拟动态飞行轨迹，包括爬升、俯仰、滚转和转弯等各种机动。高超声速飞行 器的初始位置是北纬30.011°，东经120.094°，海拔30公里；沿E-N-U导航架三轴的 初始速度分别为0m/s、1700m/s和0m/s；初始位置误差(5m、5m、10m)、初始速度 误差(0.2m/s、0.2m/s、0.2m/s)和初始姿态误差(0.1°、0.1°)；陀螺的恒定漂移和 白噪声为0.1°/h和0.01°/h、加速计的零偏差和白噪声分别为10^-3g和10^-4g。GNSS位 置和速度的均方根误差分别为5m和0.1m/s，INS和GNSS的采样周期分别为0.02s和 0.1s。仿真时间为300s，INS/GNSS集成滤波周期为0.1s。

在高动态环境下，杠杆臂的变化如图9所示，随着飞行时间的延长与飞行状态的改变会 发生变化；为了仿真符合实际，利用某次测试数据进行模拟固有频率的变化如表3所示。

表3

利用RL-DLAC算法进行动态杆臂的识别，并对其固有频率的变化进行学习，完善实际 飞行过程中的动态杆臂变化模型，对固有频率的学习结果如表4所示。

表4

通过表4可以发现，RL-DLAC对固有频率的学习效果很好；固有频率决定了动态杆臂 模型，分别应用RL-DLAC和KF-FLAC方法对动态杆臂进行标定，如图10所示，可以看 出RL-DLAC算法对动态杆臂效应具有更好的标定补偿结果，从图11和图12可以看出与传 统的KF-FLAC方法相比，使用RL-DLAC方法补偿动态杆臂效应误差的导航精度有所提高， 并且对于后续导航结果尤其是长时间误差发散过程会有较好的抑制作用。

Claims (3)

1.一种高超声速飞行器动态杆臂效应智能补偿方法，其特征在于，具体步骤如下：

首先，针对高超声速飞行器，基于现有的杆臂效应误差模型，建立高动态弹性变形下的动态杆臂效应误差模型；同时，建立高超声速飞行器INS误差模型；

然后、对动态杆臂效应误差模型中的固有频率进行强化学习，实现对动态环境下飞行器杆臂的补偿后代入原动态杆臂效应误差模型，进行INS/GNSS组合导航得到导航结果；

最后、将当前导航结果与GNSS的定位结果的误差作为强化学习标志位，当误差大于阈值时，采集距离飞行器当前时刻最近的N秒IMU和GNSS数据，对固有频率进行自适应学习，并反馈给态杆臂效应误差模型继续滤波和定位，进行导航结果的修正。

2.如权利要求1所述的一种高超声速飞行器动态杆臂效应智能补偿方法，其特征在于，所述高动态弹性变形下的动态杆臂效应误差模型为：

其中，

为杆臂效应产生的加速度计误差，r_A为杆臂长度矢量；

为本体坐标系相对惯性坐标系的转动角速度，

为转动角速度

的反对称矩阵，r_A0代表初始杆臂长度；

ω_i为第i阶振型的固有频率，ζ_i为第i阶振型的阻尼系数，W_i(x)表示飞行器纵轴各点的相对横移关系，称为第i次固有振型函数；q为弹性振动的广义坐标，u_q为弹性变形控制量，w_q为弹性变形噪声项，u_r为杆臂效应误差控制量，w_r为杆臂效应误差噪声项。

3.如权利要求1所述的一种高超声速飞行器动态杆臂效应智能补偿方法，其特征在于，所述强化学习元素如下表所示：

将强化学习过程作为马尔可夫决策过程，即(s,α,P,r,ρ₀)；

其中s是一组状态量，α是一组动作，P：s×α×s→R≥0为由智能体表示的转移概率分布，r：s×α→R为从环境中获得的奖励价值，ρ₀：s→R≥0是初始状态；

首先，将动态杆臂效应误差模型中的广义坐标q扩展到组合导航系统中的状态量X，即：

[φ_E φ_N φ_U]^T为飞行器惯导姿态失准角；[δV_E δV_N δV_U]^T为飞行器在导航系下的速度误差；[δL δλ δh]^T为飞行器纬—经—高度位置误差；ε_bi为陀螺仪的常值漂移项，ε_ri为陀螺仪的一阶马尔科夫过程漂移项，

为加速度计的一阶马尔科夫过程漂移项，q为弹性振动的广义坐标；

然后，基于状态量X，得到动态杆臂效应误差在线标定系统的状态方程和量测方程；

其中，F(t)为系统矩阵，B(t)为系统控制矩阵，G(t)为系统状态噪声系数矩阵，W(t)为系统噪声矩阵，H(t)为量测系数矩阵，V(t)为量测噪声矩阵，U(t)为控制矩阵；

最后，使用带确定性控制的卡尔曼滤波器，将各状态信息进行融合，得到组合导航的结果。

Images