CN111351482A - 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 - Google Patents

Abstract

本发明公开了一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法。该方法包括：首先，建立传感器的测量模型，推导四元数形式的导航方程，并对该方程进行线性化处理得到误差状态方程；然后，基于GPS、磁力计和加速度计的测量信息，建立用于卡尔曼滤波估计的测量方程；最后，结合惯性‑GPS松组合模式，采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计，并实现对导航状态的校正。本发明使用四元数时计算量较小，而且能够消除惯性导航系统存在的累计误差，实现高精度导航。

Description

基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法

技术领域

本发明属于导航技术领域，特别是一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法。

背景技术

导航系统及其配套子系统是自主无人机研究的重点。自主导航系统是利用各个子系统提供的信息来实现三个基本任务的系统，三个基本任务包括：1)定位：估计无人机的位置；2)障碍检测和躲避：识别周围障碍物并因此采取行动以躲避它们；3)发出控制指令：发送指令以使无人机稳定姿态并遵循导航指令。

随着SUAVs(小型无人机)和MAVs(微型无人机)的出现，比如Parrot公司的Ar.Drone Parrot、大疆无人机公司的DJI Phantom series、Walkera公司的Voyager3、TALIH500和3D Robotics公司的3DR SOLO等，无人机的尺寸越来越小、重量越来越小，导致有效载荷变小，需要找到合适的板载传感器来实现相应的导航目的。

在大多数用于IMU姿态估计的卡尔曼滤波方法中，首先通过积分来自陀螺仪的角速率传播姿态，以用作输入信号，然后使用来自加速度计和磁力计的信息更新姿态。在静态或低动力条件下，加速度计可测量地球的重力并提供横滚和俯仰的信息，而磁力计主要是通过测量地球的磁场来提供偏航信息。由于除了地球赤道以外，磁场的倾斜角都不为零，因此磁力计还提供了一些关于俯仰和横滚的信息。所以，可以将同时使用加速度计和磁力计测量的信息融合过程用作卡尔曼滤波方案中的测量更新过程。然而，由于测量噪声的存在，IMU对位置的估计存在长期不稳定性，借助GPS进行辅助导航是一种较为实际的方案。

GPS/IMU组合导航系统是将来自IMU和GPS的信息融合在一起以补偿两个传感器产生的误差，提高定位过程的精度。因为GPS和IMU测量噪声近似为高斯噪声，所以线性卡尔曼滤波器能够将来自多个天线GPS的数据与来自机载IMU的信息融合。为了减小位置误差，文献1(Barczyk M,Lynch AF.Integration of a Triaxial Magnetometer into aHelicopter UAV GPS-Aided INS[J].IEEE Transactions on Aerospace and ElectronicSystems,2012,48(4):2947-2960.)采用扩展卡尔曼滤波器，文献2(Yang Yuhong,Zhou J,Loffeld O.Quaternion-based Kalman filtering on INS/GPS[C]Information Fusion(FUSION),2012 15th International Conference.IEEE,2012:511-518.)分析了使用基于四元数的EKF和UKF算法的紧密耦合的IMU/GPS系统的系统性能。然而，这些方法都是直接法，即直接以导航参数作为估计对象，由于各子系统的误差源和量测误差都是随机的，导致惯性导航系统存在大量累计误差，降低了导航精度。

发明内容

本发明的目的在于提供一种精度高、实时性好、计算量小的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法。

实现本发明目的的技术解决方案为：一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，包括以下步骤：

步骤1，建立传感器的测量模型，推导四元数形式的导航方程，并对该方程进行线性化处理得到误差状态方程；

步骤2，基于GPS、磁力计和加速度计的测量信息，建立用于卡尔曼滤波估计的测量方程；

步骤3，结合惯性-GPS松组合模式，采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计，并实现对导航状态的校正。

本发明与现有技术相比，其显著优点为：(1)采用误差状态卡尔曼滤波方法ESKF，误差状态较小并且位于原点附近，从而避免了可能的奇异性和万向节锁定问题，同时始终确保线性化的有效性；(2)ESKF方法可确保数值稳定性，并允许四元数以小信号表示进行处理，使得雅可比矩阵的计算相对容易，减少了计算量；(3)将ESKF用作基本滤波器，并将GPS的测量信息加入到IMU测量信息中进行信息融合，获得多旋翼状态的优化估计，并提高系统的容错性；(4)能够消除惯性导航系统存在的累计误差，实现高精度导航，且实时性好。

附图说明

图1为本发明基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法的流程图。

图2为本发明基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法的原理图。

图3为本发明实施例提供的MPU9250模型示意图，其中(a)为加速度和陀螺仪方向和极性示意图，(b)为磁力计方向示意图。

图4为本发明实施例提供的GPS测量状态仿真曲线图，其中(a)为静态GPS位置测量曲线图，(b)为静态GPS速度测量曲线图。

图5为本发明实施例提供的静态组合导航姿态估计仿真曲线图，其中(a)为静态组合导航姿态估计状态曲线图，(b)为静态组合导航姿态误差状态曲线图。

图6为本发明实施例提供的静态组合导航速度估计仿真曲线图，其中(a)为静态组合导航速度估计状态曲线图，(b)为静态组合导航速度误差状态曲线图。

图7为本发明实施例提供的静态组合导航位置估计仿真曲线图，其中(a)为静态组合导航位置估计状态曲线图，(b)为静态组合导航位置误差状态曲线图。

图8为本发明实施例提供的动态组合导航姿态估计仿真曲线图，其中(a)为动态组合导航姿态估计状态曲线图，(b)为动态组合导航姿态误差状态曲线图。

图9为本发明实施例提供的动态组合导航速度估计仿真曲线图，其中(a)为动态组合导航速度估计状态曲线图，(b)为动态组合导航速度误差状态曲线图。

图10为本发明实施例提供的动态组合导航位置估计仿真曲线图，其中(a)为动态组合导航位置估计状态曲线图，(b)为动态组合导航位置误差状态曲线图。

具体实施例

本发明基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，包括以下步骤：

步骤1，建立传感器的测量模型，推导四元数形式的导航方程，并对该方程进行线性化处理得到误差状态方程；

步骤2，基于GPS、磁力计和加速度计的测量信息，建立用于卡尔曼滤波估计的测量方程；

步骤3，结合惯性-GPS松组合模式，采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计，并实现对导航状态的校正。

进一步地，步骤1所述建立传感器的测量模型，具体如下：

基于惯性+GPS组合导航的信息测量元件包括陀螺仪、加速度计、磁力计和GPS，做出以下4条假设：

(1)陀螺仪和加速度计测量信息的零位偏差为固定偏差；

(2)陀螺仪和加速度计的测量噪声为高斯噪声，满足正态分布；

(3)磁力计不存在零位偏差，测量噪声为高斯噪声，满足正态分布；

(4)GPS不存在零位偏差，测量噪声为高斯噪声，满足正态分布；

基于以上假设，陀螺仪的测量模型为：

其中，ω_m角为速度测量值，

为机体系相对于惯性系的角速度在机体系下的坐标表示，b_g为陀螺仪的零偏；n_g为高斯噪声，各个方向不相关，满足正态分布：

其中，E(n_g)＝0为高斯噪声的均值，

为高斯噪声的协方差矩阵；σ_g是高斯噪声的标准差，I是3阶单位阵；

加速度计的测量模型为：

其中，a_m是加速度的测量值，a_i是惯性加速度，g_i是重力加速度在惯性系下的坐标表示，

是惯性系到机体系的旋转矩阵，b_a为加速度计的零偏；n_a表示高斯白噪声，各个方向不相关，满足正态分布：

其中，E(n_a)＝0为高斯白噪声的均值，

为高斯白噪声的协方差矩阵；σ_a为角速度计噪声的标准差；

磁力计的测量模型为：

其中，M_m是磁场强度的测量值，

是导航系到机体系的旋转矩阵，M_n是磁场强度在导航系下的坐标表示；n_M是磁力计的测量噪声，满足正态分布：

其中，σ_m磁力计噪声的标准差。

进一步地，步骤1所述推导四元数形式的导航方程，并对该方程进行线性化处理得到误差状态方程，具体如下：

基于误差状态卡尔曼滤波器ESKF的工作原理，将真实状态向量x视为标称状态向量x_nom和误差状态向量δx的组合，其中误差状态表示标称状态与真实状态之间的差，产生以下关系：

其中，

表示除四元数状态之外的所有属于

空间状态变量的典型加法运算即标量和矢量的加减运算；

四元数误差状态在机体系中的定义为：

其中q是真实四元数，q_nom是标称四元数，δq是四元数偏差，

为四元数运算：

所述用于导航信息估计的ESKF状态由包含19个元素的真实状态向量x，包含19个元素的标称状态向量x_nom和包含18个元素的误差状态向量δx组成，表示如下：

其中，r＝[x_n,y_n,z_n]^Τ是多旋翼在导航系下的位置，v＝[v_xn,v_yn,v_zn]^Τ是多旋翼在导航系下的速度；q＝[q₀ q₁ q₂ q₃]^Τ为四元数，表示多旋翼的姿态；b_g＝[b_ωx,b_ωy,b_ωz]^Τ是三轴陀螺仪的固定零偏，b_a＝[b_ax,b_ay,b_az]^Τ是三轴加速度计的固定零偏，g＝[0,0,g]^Τ是重力加速度在导航系下的表示；

角速度ω和旋转偏差矢量δθ是在机体坐标系下的表示，

是机体系到导航系的旋转矩阵；

所述标称状态运动学方程为：

其中，k表示t_k时刻，k+1表示t_k+1时刻，x_k＝x(t_k),t_k＝kΔt，Δt＝t_k+1-t_k；

所述误差状态运动学方程为：

其中，δg表示重力加速度误差；v_i,θ_i,b_ai,b_gi是速度噪声、角度噪声、加速度偏差噪声和陀螺仪偏差噪声的离散化能量；v_i,θ_i,b_ai,b_gi的均值是0，协方差矩阵通过对n_a,n_g,b_a,b_g的方差在间隔时间Δt内进行积分获得：

其中，σ_a,σ_g根据IMU数据表中的信息确定，σ_a单位为m/s²，σ_g单位为rad/s；σ_wa是加速度偏差噪声的标准差，σ_wg是陀螺仪偏差噪声的标准差，V_i是速度噪声的协方差矩阵、Θ_i是角度噪声的协方差矩阵、A_i是加速度偏差噪声的协方差矩阵、Ω_i是陀螺仪偏差噪声的协方差矩阵；

所述扰动向量w_i＝[v_i,θ_i,a_i,ω_i]^Τ，那么误差状态方程为：

δx_k+1＝F_x,kδx_k+F_i,kw_i,k (14)

其中，F_x是误差状态转移矩阵，F_i是扰动向量w_i的雅可比矩阵，F_x和F_i表达式如下式所示：

误差状态卡尔曼滤波器的误差状态的更新方程为：

误差状态卡尔曼滤波器的协方差P_k的更新方程为：

其中，Q_i是系统噪声的协方差矩阵，表达式如下式所示：

进一步地，步骤2所述基于GPS、磁力计和加速度计的测量信息，建立用于卡尔曼滤波估计的测量方程，测量方程包括GPS测量方程、加速度计测量方程、磁力计测量方程；

所述GPS的位置测量噪声用υ_pos表示，位置测量噪声的标准差用σ_r表示，速度测量噪声用υ_vel表示，速度测量噪声的标准差用σ_v表示；

所述导航系下的位置和经纬度关系为：

其中，地球的经度、纬度和高度分别为L,λ,h，参考椭球子午面曲率半径用R_M表示，卯酉面曲率半径用R_N表示，[δx_n,δy_n,δz_n]^Τ是导航系下的位置变化量；

所述多旋翼的经纬高度和导航系下位置的关系为：

其中，[L₀,λ₀,h₀]^Τ是起飞点的经纬高度，[L_m,λ_m,h_m]^Τ是经纬高度的测量值；υ_pos是经纬高度的测量噪声，满足正态分布；

所述加速度计的测量方程为：

所述磁力计的测量方程为：

其中，地磁场强度用M表示，磁偏角用D表示，磁倾角用I表示；

设测量状态向量为y，组成为y＝[L_m,λ_m,h_m,v_xm,v_ym,v_zm,a_m,M_m]^Τ，测量噪声为υ，组成为υ＝[υ_pos,υ_vel,n_a,n_M,w_a]^Τ，所述测量方程为：

y_k＝h(x_nom,k)+υ_k (24)

其中，h()是系统状态的非线性函数，υ是方差为V的高斯噪声，满足υ～N{0,V}。

进一步地，步骤3所述结合惯性-GPS松组合模式，采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计，并实现对导航状态的校正，具体如下：

根据t_k+1时刻的测量值y_k+1，通过滤波器估计误差状态，得到滤波器增益矩阵：

K_k+1＝P_k+1/kH_k+1 ^Τ(H_k+1P_k+1/kH_k+1 ^Τ+V_k+1)^-1 (25)

其中，P_k+1/k为第k+1时刻误差状态的协方差的预测值，H_k+1是第k+1时刻h()关于误差状态δx的雅可比矩阵，V_k+1是第k+1时刻测量方程噪声的协方差矩阵，K_k+1是第k+1时刻的滤波器增益矩阵；

所述t_k+1时刻的误差状态的最优估计为：

协方差矩阵为：

P_k+1/k+1＝(I-K_k+1H_k+1)P_k+1/k (27)

根据所述要求定义关于误差状态δx的雅可比矩阵H，并以最佳真实状态估计值

进行估计；由于此阶段的误差状态平均值为零，假设

使用标称状态x_nom作为评估点，从而得出雅可比矩阵：

所述雅可比矩阵利用链式规则来计算：

其中，H_x是h()的标准雅可比矩阵，取决于所使用的传感器的测量功能；X_δx是真实状态关于误差状态的雅可比行列式，由于仅取决于状态的ESKF组成，因此得到：

其中，

为a对b求偏导数的形式；

除了q其余状态的微分结果都是3阶单位阵I，设

得到：

化简得Q_δθ：

推导出：

相对于标称状态的雅可比H_x满足：

获得ESKF更新方程之后，通过误差状态在t_k+1时刻的近似最优估计，对标称状态进行更新，得到：

其中，

即t_k+1时刻的真实值的最优估计，

是第k+1时刻真实状态的估计值，

是第k+1时刻误差状态的最优估计。

下面结合附图及具体实施例对本发明做进一步详细说明。

实施例

结合图1～2，本实施例提供一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，包括：

S1：首先，建立传感器的测量模型，推导四元数形式的导航方程，并对该方程进行线性化处理得到误差状态方程；

S2：然后，基于GPS、磁力计和加速度计的测量信息建立用于卡尔曼滤波估计的测量方程；

S3：最后，结合惯性-GPS松组合思想，采用误差状态卡尔曼滤波器(ESKF)对误差状态进行最优估计，并实现对导航状态的校正。

本实施例所述的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，首先建立四元数形式的导航方程并对该方程进行线性化，得到误差状态方程；然后，基于GPS、磁力计和加速度计的测量信息建立用于卡尔曼滤波估计的测量方程；最后，结合惯性-GPS松组合思想，采用误差状态卡尔曼滤波器(ESKF)对误差状态进行最优估计，并实现对导航状态的校正。该方法的优势在于，和欧拉角及方向余弦矩阵相比，使用四元数时计算量较小，而且能够消除惯性导航系统存在的累计误差，实现高精度导航。

在上述基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法的具体实施方式中，可选地，根据所述传感器测量模型和精度要求，建立传感器模型包括：

采用MPU9250作为惯性测量单元，MPU9250是一种惯性传感器，它集成了三轴加速度、三轴角速度(陀螺仪)和三轴磁力计，MPU9250模型示意图如图3(a)～(b)所示。惯性传感器和GPS的关键参数如表1所示。

表1惯性传感器和GPS关键参数

设置GPS的刷新频率为5Hz，磁力计的刷新频率为50Hz，陀螺仪和加速度计的刷新频率为100Hz。初始经纬高度对应导航坐标系下的坐标为[0,0,0]，初始姿态为0。

位置估计误差初值为[-2m,-2m,-4m]，速度估计误差初值为[0m/s,0m/s,0m/s]，姿态估计误差初值为[3°,-3°,5°]；陀螺仪零偏为[0.5o/s,-0.5o/s,1o/s]，测量噪声标准差为σ_g＝0.1°/s；加速度计零偏为[0.06m/s²,-0.06m/s²,0.08m/s²]，测量噪声标准差为σ_a＝2.94×10^-2m/s²；磁力计的测量噪声标准差为σ_m＝2×10^-3Gauss；GPS水平位置测量噪声标准差为σ_xy＝5m，高度测量噪声标准差为σ_z＝10m。速度测量噪声标准差为σ_v＝0.1m/s。

根据上述给定条件，得到滤波器的参数如下：

(1)初始协方差矩阵：P₀＝diag(E(x₀)²)；

(2)测量噪声协方差矩阵：V＝diag([σ_xy ²Δt²,σ_z ²Δt²,σ_v ²Δt²,σ_a ²Δt²,σ_m ²Δt²])；

(3)系统噪声协方差矩阵：

基于上述初始条件，对静态时的IMU+GPS组合导航进行仿真，得到仿真结果如图4(a)～(b)、图5(a)～(b)、图6(a)～(b)、图7(a)～(b)所示。

图4为GPS测量的位置和速度静态信息仿真曲线。图5～图7表明，导航状态经过短暂调整后到达稳定状态。姿态角的估计偏差在±3°以内，北向速度、东向速度和地向速度的估计偏差均在±0.02m/s以内，北向位置、东向位置和地向位置的估计偏差均在±0.5m以内。稳态误差是系统噪声和测量噪声共同造成的。图5(b)、图6(b)和图7(b)是误差状态估计曲线，姿态角的误差状态稳定在±1.5°以内，速度的误差状态稳定在±0.02m/s，位置的误差状态稳定在±0.5m以内。仿真结果表明，本章设计的组合导航方案在静态时基本能够消除惯性导航系统带来的状态偏差，使得导航精度得到明显的提升。

下面进行动态过程仿真，考虑陀螺仪和加速度的零偏和测量噪声，真实状态为多旋翼跟踪位置指令(0,0,0)→(0,0,5)→(5,0,5)→(5,5,5)→(0,5,5)→(0,0,5)时多旋翼的真实航迹，仿真时间为25s/初始条件保持不变，使用IMU+GPS组合导航算法对多旋翼动态时的状态进行估计，通过仿真得到状态估计曲线如图8(a)～(b)、图9(a)～(b)、图10(a)～(b)所示。

仿真曲线表明，组合导航系统估计的状态基本能够跟踪真实状态。状态误差曲线反映出，当真实状态发生变化时，组合导航系统能够及时估计系统的误差状态，并对原惯性导航系统的估计状态进行校正，从而得到精度更高的估计。

Claims (5)

1.一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，其特征在于，包括以下步骤：

步骤1，建立传感器的测量模型，推导四元数形式的导航方程，并对该方程进行线性化处理得到误差状态方程；

步骤2，基于GPS、磁力计和加速度计的测量信息，建立用于卡尔曼滤波估计的测量方程；

步骤3，结合惯性-GPS松组合模式，采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计，并实现对导航状态的校正。

2.根据权利要求1所述的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，其特征在于，步骤1所述建立传感器的测量模型，具体如下：

基于惯性+GPS组合导航的信息测量元件包括陀螺仪、加速度计、磁力计和GPS，做出以下4条假设：

(1)陀螺仪和加速度计测量信息的零位偏差为固定偏差；

(2)陀螺仪和加速度计的测量噪声为高斯噪声，满足正态分布；

(3)磁力计不存在零位偏差，测量噪声为高斯噪声，满足正态分布；

(4)GPS不存在零位偏差，测量噪声为高斯噪声，满足正态分布；

基于以上假设，陀螺仪的测量模型为：

其中，ω_m角为速度测量值，

为机体系相对于惯性系的角速度在机体系下的坐标表示，b_g为陀螺仪的零偏；n_g为高斯噪声，各个方向不相关，满足正态分布：

其中，E(n_g)＝0为高斯噪声的均值，

为高斯噪声的协方差矩阵；σ_g是高斯噪声的标准差，I是3阶单位阵；

加速度计的测量模型为：

其中，a_m是加速度的测量值，a_i是惯性加速度，g_i是重力加速度在惯性系下的坐标表示，

是惯性系到机体系的旋转矩阵，b_a为加速度计的零偏；n_a表示高斯白噪声，各个方向不相关，满足正态分布：

其中，E(n_a)＝0为高斯白噪声的均值，

为高斯白噪声的协方差矩阵；σ_a为角速度计噪声的标准差；

磁力计的测量模型为：

其中，M_m是磁场强度的测量值，

是导航系到机体系的旋转矩阵，M_n是磁场强度在导航系下的坐标表示；n_M是磁力计的测量噪声，满足正态分布：

其中，σ_m磁力计噪声的标准差。

3.根据权利要求1所述的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，其特征在于，步骤1所述推导四元数形式的导航方程，并对该方程进行线性化处理得到误差状态方程，具体如下：

基于误差状态卡尔曼滤波器ESKF的工作原理，将真实状态向量x视为标称状态向量x_nom和误差状态向量δx的组合，其中误差状态表示标称状态与真实状态之间的差，产生以下关系：

其中，

表示除四元数状态之外的所有属于

空间状态变量的典型加法运算即标量和矢量的加减运算；

四元数误差状态在机体系中的定义为：

其中q是真实四元数，q_nom是标称四元数，δq是四元数偏差，

为四元数运算：

所述用于导航信息估计的ESKF状态由包含19个元素的真实状态向量x，包含19个元素的标称状态向量x_nom和包含18个元素的误差状态向量δx组成，表示如下：

其中，r＝[x_n,y_n,z_n]^Τ是多旋翼在导航系下的位置，v＝[v_xn,v_yn,v_zn]^Τ是多旋翼在导航系下的速度；q＝[q₀ q₁ q₂ q₃]^Τ为四元数，表示多旋翼的姿态；b_g＝[b_ωx,b_ωy,b_ωz]^Τ是三轴陀螺仪的固定零偏，b_a＝[b_ax,b_ay,b_az]^Τ是三轴加速度计的固定零偏，g＝[0,0,g]^Τ是重力加速度在导航系下的表示；

角速度ω和旋转偏差矢量δθ是在机体坐标系下的表示，

是机体系到导航系的旋转矩阵；

所述标称状态运动学方程为：

其中，k表示t_k时刻，k+1表示t_k+1时刻，x_k＝x(t_k),t_k＝kΔt，Δt＝t_k+1-t_k；

所述误差状态运动学方程为：

其中，δg表示重力加速度误差；v_i,θ_i,b_ai,b_gi是速度噪声、角度噪声、加速度偏差噪声和陀螺仪偏差噪声的离散化能量；v_i,θ_i,b_ai,b_gi的均值是0，协方差矩阵通过对n_a,n_g,b_a,b_g的方差在间隔时间Δt内进行积分获得：

其中，σ_a,σ_g根据IMU数据表中的信息确定，σ_a单位为m/s²，σ_g单位为rad/s；σ_wa是加速度偏差噪声的标准差，σ_wg是陀螺仪偏差噪声的标准差，V_i是速度噪声的协方差矩阵、Θ_i是角度噪声的协方差矩阵、A_i是加速度偏差噪声的协方差矩阵、Ω_i是陀螺仪偏差噪声的协方差矩阵；

所述扰动向量w_i＝[v_i,θ_i,a_i,ω_i]^Τ，那么误差状态方程为：

δx_k+1＝F_x,kδx_k+F_i,kw_i,k (14)

其中，F_x是误差状态转移矩阵，F_i是扰动向量w_i的雅可比矩阵，F_x和F_i表达式如下式所示：

误差状态卡尔曼滤波器的误差状态的更新方程为：

误差状态卡尔曼滤波器的协方差P_k的更新方程为：

其中，Q_i是系统噪声的协方差矩阵，表达式如下式所示：

4.根据权利要求1所述的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，其特征在于，步骤2所述基于GPS、磁力计和加速度计的测量信息，建立用于卡尔曼滤波估计的测量方程，测量方程包括GPS测量方程、加速度计测量方程、磁力计测量方程；

所述GPS的位置测量噪声用υ_pos表示，位置测量噪声的标准差用σ_r表示，速度测量噪声用υ_vel表示，速度测量噪声的标准差用σ_v表示；

所述导航系下的位置和经纬度关系为：

其中，地球的经度、纬度和高度分别为L,λ,h，参考椭球子午面曲率半径用R_M表示，卯酉面曲率半径用R_N表示，[δx_n,δy_n,δz_n]^Τ是导航系下的位置变化量；

所述多旋翼的经纬高度和导航系下位置的关系为：

其中，[L₀,λ₀,h₀]^Τ是起飞点的经纬高度，[L_m,λ_m,h_m]^Τ是经纬高度的测量值；υ_pos是经纬高度的测量噪声，满足正态分布；

所述加速度计的测量方程为：

所述磁力计的测量方程为：

其中，地磁场强度用M表示，磁偏角用D表示，磁倾角用I表示；

设测量状态向量为y，组成为y＝[L_m,λ_m,h_m,v_xm,v_ym,v_zm,a_m,M_m]^Τ，测量噪声为υ，组成为υ＝[υ_pos,υ_vel,n_a,n_M,w_a]^Τ，所述测量方程为：

y_k＝h(x_nom,k)+υ_k (24)

其中，h()是系统状态的非线性函数，υ是方差为V的高斯噪声，满足υ～N{0,V}。

5.根据权利要求1所述的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，其特征在于，步骤3所述结合惯性-GPS松组合模式，采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计，并实现对导航状态的校正，具体如下：

根据t_k+1时刻的测量值y_k+1，通过滤波器估计误差状态，得到滤波器增益矩阵：

K_k+1＝P_k+1/kH_k+1 ^Τ(H_k+1P_k+1/kH_k+1 ^Τ+V_k+1)^-1 (25)

其中，P_k+1/k为第k+1时刻误差状态的协方差的预测值，H_k+1是第k+1时刻h()关于误差状态δx的雅可比矩阵，V_k+1是第k+1时刻测量方程噪声的协方差矩阵，K_k+1是第k+1时刻的滤波器增益矩阵；

所述t_k+1时刻的误差状态的最优估计为：

协方差矩阵为：

P_k+1/k+1＝(I-K_k+1H_k+1)P_k+1/k (27)

根据所述要求定义关于误差状态δx的雅可比矩阵H，并以最佳真实状态估计值

进行估计；由于此阶段的误差状态平均值为零，假设

使用标称状态x_nom作为评估点，从而得出雅可比矩阵：

所述雅可比矩阵利用链式规则来计算：

其中，H_x是h()的标准雅可比矩阵，取决于所使用的传感器的测量功能；X_δx是真实状态关于误差状态的雅可比行列式，由于仅取决于状态的ESKF组成，因此得到：

其中，

为a对b求偏导数的形式；

除了q其余状态的微分结果都是3阶单位阵I，设

得到：

化简得Q_δθ：

推导出：

相对于标称状态的雅可比H_x满足：

获得ESKF更新方程之后，通过误差状态在t_k+1时刻的近似最优估计，对标称状态进行更新，得到：

其中，

即t_k+1时刻的真实值的最优估计，

是第k+1时刻真实状态的估计值，

是第k+1时刻误差状态的最优估计。

Images