下载文档原格式
ardupilot(EKF)扩展卡尔曼滤波一、初识卡尔曼滤波器为了描述方便我从网上找了一张卡尔曼滤波器的5大公式的图片。
篇幅所限，下图所示的是多维卡尔曼滤波器（因为EKF2是多维扩展卡尔曼滤波器，所以我们从多维说起），为了跟好的理解卡尔曼滤波器可以百度一下，从一维开始。
这5个公式之外还有一个观测模型，根据你实际的观测量来确定，它的主要作用是根据实际情况来求观测矩阵H。
因为卡尔曼滤波器是线性滤波器，状态转移矩阵A和观测矩阵H是确定的。
在维基百科上状态转移矩阵用F表示。
在ardupilot EKF2算法中，状态转移矩阵也是用F表示的。
下面是维基百科给出的线性卡尔曼滤波器的相关公式。
上述更新（后验）估计协方差的公式对任何增益K k都有效，有时称为约瑟夫形式。
为了获得最佳卡尔曼增益，该公式进一步简化为P k|k=(I-K k H k)P k|k-1，它在哪种形式下应用最广泛。
但是，必须记住它仅对最小化残差误差的最佳增益有效。
为了使用卡尔曼滤波器来估计仅给出一系列噪声观测过程的内部状态，必须根据卡尔曼滤波器的框架对过程进行建模，这意味着指定一下矩阵：只要记住一点就行了，卡尔曼滤波器的作用就是输入一些包含噪声的数据，得到一些比较接近真是情况的数据。
比如无人机所使用的陀螺仪和加速度计的读值，他们的读值都是包含噪声的，比如明明真实的角速度是俯仰2°/s,陀螺仪的读值却是2.5°/s。
通过扩展卡尔曼之后的角速度值会变得更加接近2º/s的真实值，有可能是2.1º/s。
二、扩展卡尔曼滤波器因为卡尔曼滤波器针对的是线性系统，状态转移模型（说的白话一点就是知道上一时刻被估计量的值，通过状态转移模型的公式可以推算出当前时刻被估计量的值）和观测模型。
注：有的资料显示状态模型中有,有的没有，目前我也不清楚是为什么，有可能和被估计的对象有关。
但看多了你就会发现不管网上给的公式有怎样的不同，但总体的流程是一样的，都是这5大步骤。
c语言实现扩展卡尔曼滤波
扩展卡尔曼滤波是一种常用于传感器融合的滤波算法，可以有效地估计目标的状态并减小测量误差。
在本文中，我将以人类的视角来描述扩展卡尔曼滤波的原理和应用。
让我简要介绍一下卡尔曼滤波的基本原理。
卡尔曼滤波是一种递归的滤波算法，通过预测和更新两个步骤来估计系统的状态。
在预测步骤中，根据系统的动态模型和先验信息，预测下一个状态的均值和方差。
然后，在更新步骤中，利用测量值对预测结果进行修正，得到最优的状态估计。
扩展卡尔曼滤波是卡尔曼滤波的一种扩展，可以处理非线性系统。
与传统的卡尔曼滤波不同，扩展卡尔曼滤波使用线性化的动态模型和测量模型来进行状态预测和更新。
具体而言，扩展卡尔曼滤波通过在每次迭代中对非线性函数进行线性化，将非线性系统转化为线性系统进行处理。
在实际应用中，扩展卡尔曼滤波广泛用于目标跟踪、导航和定位等领域。
例如，在自动驾驶汽车中，扩展卡尔曼滤波可以利用车载传感器（如雷达和摄像头）的测量数据，对车辆的位置和速度进行估计，从而实现精确的自动驾驶控制。
扩展卡尔曼滤波还可以应用于机器人定位和导航。
通过结合惯性测量单元（IMU）和全向视觉传感器的测量数据，扩展卡尔曼滤波可以
实现机器人在未知环境中的定位和导航。
这对于无人机、无人车等智能机器人的自主导航非常重要。
扩展卡尔曼滤波是一种强大的滤波算法，能够在非线性系统中有效地估计目标的状态。
它在传感器融合、目标跟踪和机器人导航等应用中发挥着重要的作用。
通过合理地选择动态模型和测量模型，并利用适当的线性化方法，我们可以利用扩展卡尔曼滤波来解决各种实际问题，并取得良好的估计效果。
本栏目责任编辑：梁书计算机工程应用技术基于卡尔曼滤波和互补滤波的AHRS 系统研究蔡阳，胡杰❋（长江大学计算机科学学院，湖北荆州434023）摘要：AHRS 航姿参考系统中通常需要融合MEMS 传感器数据来进行姿态解算，由于MEMS 传感器自身的一些缺陷导致在姿态解算中会出现较为严重的误差。
AHRS 中常见对加速度计、陀螺仪和磁力计进行卡尔曼滤波、互补滤波的方法，由于使用单一的滤波算法时会出现误差，导致姿态角解算精度不高。
本文采用卡尔曼滤波融合互补滤波的滤波算法，通过卡尔曼滤波对加速度计和陀螺仪起抑制漂移作用，进而得到最优估计姿态角，减小传感器引起的误差，再由估计值和磁力计经过互补滤波滤除噪声，提高姿态角的解算精度。
仿真实验表明：融合滤波算法可以抑制漂移和滤除噪声，在静态和动态条件下，都有良好表现。
关键词：AHRS;MEMS ；姿态解算；卡尔曼滤波；互补滤波中国分类号：TP301文献标识码：A 文章编号：1009-3044(2021)10-0230-03开放科学（资源服务）标识码（OSID ）:Research on AHRS System Based on Kalman Filter and Complementary Filter CAI Yang,HU Jie(School of Computer Science,Yangtze University,Jingzhou 434023,China)Abstract:AHRS heading and attitude reference system usually needs to fuse MEMS sensor data for attitude calculation.Due to some defects of MEMS sensor itself,there will be more serious errors in attitude calculation.Kalman filtering and complementary filtering methods for accelerometers,gyroscopes,and magnetometers are common in AHRS.Due to errors when a single filtering al⁃gorithm is used,the accuracy of the attitude angle calculation is not high.In this paper,the Kalman filter fusion complementary fil⁃ter filter algorithm is used to suppress drift of the accelerometer and gyroscope through Kalman filter,and then obtain the optimal estimated attitude angle,reduce the error caused by the sensor,and then pass the estimated value and the ple⁃mentary filtering filters out noise and improves the accuracy of attitude angle calculation.Simulation experiments show that the fu⁃sion filtering algorithm can suppress drift and filter noise,and it performs well under static and dynamic conditions.Keywords:AHRS;MEMS;attitude calculation;Kalman filter;complementary filter航姿参考系统AHRS(Attitude and Heading Reference Sys⁃tem)由MEMS(Micro-Electro Mechanical System)惯性传感器三轴陀螺仪、三轴加速度计和磁力计的数据融合来进行姿态解算[1]。
基于扩展卡尔曼滤波的目标跟踪定位算法及matlab程序实现扩展卡尔曼滤波（Extended Kalman Filter，EKF）是一种用于非线性系统状态估计的算法。
在目标跟踪定位中，它可以用于估计目标的运动轨迹。
下面是一个简单的基于扩展卡尔曼滤波的目标跟踪定位算法的描述，以及一个简化的MATLAB程序实现。
算法描述1. 初始化：设置初始状态估计值（例如位置和速度）以及初始的估计误差协方差矩阵。
2. 预测：根据上一时刻的状态估计值和模型预测下一时刻的状态。
3. 更新：结合观测数据和预测值，使用扩展卡尔曼滤波算法更新状态估计值和估计误差协方差矩阵。
4. 迭代：重复步骤2和3，直到达到终止条件。
MATLAB程序实现这是一个简化的示例，仅用于说明扩展卡尔曼滤波在目标跟踪定位中的应用。
实际应用中，您需要根据具体问题和数据调整模型和参数。
```matlab% 参数设置dt = ; % 时间间隔Q = ; % 过程噪声协方差R = 1; % 观测噪声协方差x_est = [0; 0]; % 初始位置估计P_est = eye(2); % 初始估计误差协方差矩阵% 模拟数据：观测位置和真实轨迹N = 100; % 模拟数据点数x_true = [0; 0]; % 真实轨迹初始位置for k = 1:N% 真实轨迹模型（这里使用简化的匀速模型）x_true(1) = x_true(1) + x_true(2)dt;x_true(2) = x_true(2);% 观测模型（这里假设有噪声）z = x_true + sqrt(R)randn; % 观测位置% 扩展卡尔曼滤波更新步骤[x_est, P_est] = ekf_update(x_est, P_est, z, dt, Q, R);end% 扩展卡尔曼滤波更新函数（这里简化为2D一维情况）function [x_est, P_est] = ekf_update(x_est, P_est, z, dt, Q, R)% 预测步骤：无观测时使用上一时刻的状态和模型预测下一时刻状态F = [1 dt; 0 1]; % 状态转移矩阵（这里使用简化的匀速模型）x_pred = Fx_est + [0; 0]; % 预测位置P_pred = FP_estF' + Q; % 预测误差协方差矩阵% 更新步骤：结合观测数据和预测值进行状态更新和误差协方差矩阵更新K = P_predinv(HP_pred + R); % 卡尔曼增益矩阵x_est = x_pred + K(z - Hx_pred); % 更新位置估计值P_est = (eye(2) - KH)P_pred; % 更新误差协方差矩阵end```这个示例代码使用扩展卡尔曼滤波对一个简化的匀速运动模型进行估计。
deepsort 拓展卡尔曼滤波拓展卡尔曼滤波（Extended Kalman Filter, EKF）是一种常见的目标跟踪算法，而DeepSORT则是在EKF基础上进行的拓展，用于更加准确地实现目标跟踪。
下面将从EKF的基本原理开始，介绍DeepSORT 算法的原理以及其在目标跟踪领域的应用。
1.卡尔曼滤波（Kalman Filter）的基本原理卡尔曼滤波是一种递归滤波算法，用于估计在不完整和有噪声的测量数据下的状态变量。
简而言之，卡尔曼滤波算法通过结合先验信息和观测结果来实现对目标状态的最优估计。
卡尔曼滤波算法包括两个主要步骤：预测（Predict）和更新（Update）。
预测步骤中，根据上一时刻的状态估计和系统模型，通过状态转移方程得到当前时刻的状态预测。
预测的结果包括状态估计和状态协方差矩阵。
更新步骤中，利用预测的状态估计和观测模型，将测量结果与预测结果进行比对，得到当前时刻的最优状态估计。
更新的结果也包括状态估计和状态协方差矩阵。
通过不断迭代预测和更新步骤，卡尔曼滤波算法可以实现对目标状态的最优估计。
2. DeepSORT的原理DeepSORT是一种将深度学习与卡尔曼滤波相结合的目标跟踪算法，旨在提升目标跟踪的准确性与鲁棒性。
DeepSORT的核心思想是利用深度学习网络（如卷积神经网络）来提取目标特征，然后将这些特征作为观测值输入到卡尔曼滤波器中进行状态估计。
DeepSORT算法的主要步骤如下：(1)特征提取：利用预训练的深度学习网络，如ResNet、VGG等，对目标进行特征提取。
通过将目标图像输入到网络中，可以得到代表目标特征的向量。
(2)目标匹配：根据特征向量计算目标之间的相似度，并利用匈牙利算法或最小权重匹配算法来建立观测与目标的对应关系。
(3)卡尔曼滤波：对每个目标的运动进行预测，并将预测的结果作为观测值输入到卡尔曼滤波器中进行状态估计。
利用卡尔曼滤波器的预测步骤和更新步骤，可以得到每个目标的最优状态估计。
卡尔曼滤波是一种利用时间序列数据进行状态估计和预测的算法，它可以通过对系统状态和观测值的预测误差进行修正，不断优化估计结果，从而提高估计精度。
卡尔曼滤波的基本思想是将系统状态和观测值分别作为状态向量和观测向量，建立数学模型，通过递归计算估计状态向量的值。
卡尔曼滤波的基本流程包括预测和更新两个步骤，其中预测步骤根据上一时刻的状态向量和系统噪声进行状态预测，更新步骤则根据当前时刻的观测向量和观测噪声对预测状态进行修正，得到更精确的状态向量估计值。
卡尔曼滤波的公式比较复杂，但是它可以被应用于很多领域，如导航、机器人、信号处理等。
卡尔曼滤波的扩展包括扩展卡尔曼滤波、无迹卡尔曼滤波、粒子滤波等。
扩展卡尔曼滤波是在卡尔曼滤波基础上引入了更高阶的状态变量，可以处理非线性系统；无迹卡尔曼滤波则是通过将非线性系统线性化，近似为线性系统进行滤波；粒子滤波则是一种基于蒙特卡罗方法的滤波算法，可以处理非线性、非高斯系统。
这些扩展算法在不同的应用场景中都具有一定的优势和适用性。
扩展卡尔曼公式
扩展卡尔曼滤波（EKF）是一种线性化非线性系统的方法，它基于一阶泰勒展开来线性化系统模型。
在每个时间步，EKF通过计算预测值和测量值之间的卡尔曼增益，来更新状态估计值。
具体来说，扩展卡尔曼滤波使用非线性动力学方程来描述系统状态的变化，这个非线性方程在每个时间步被线性化，以便进行状态估计。
在状态更新步骤中，EKF通过计算卡尔曼增益来权衡预测值和测量值的权重，从而得到最优的状态估计值。
扩展卡尔曼滤波的公式可以表示为：
x = f(x, u)
x = x + K(z - h(x))
其中，x表示当前时刻的状态估计值，f表示非线性系统模型，u 表示控制输入，z表示测量值，h表示非线性观测方程。
卡尔曼增益K是在每个时间步计算得到的，它反映了预测值和测量值之间的置信度。
与传统的卡尔曼滤波相比，扩展卡尔曼滤波能够处理更复杂的非线性系统，因为它使用非线性方程来描述系统状态的变化。
然而，扩展卡尔曼滤波仍然存在一些挑战，例如如何选择合适的状态转移矩阵和观测矩阵，以及如何处理系统的非线性和不确定性。
四元数姿态解算和卡尔曼滤波在四元数姿态解算和卡尔曼滤波领域，姿态解算是一个重要的问题。
姿态解算主要是通过传感器来获取物体在空间中的方位信息，其中四元数是一种广泛应用的姿态表示方法。
而卡尔曼滤波是一种常用的姿态解算算法。
四元数是一种用来表示旋转的数学工具，它由一个实部和三个虚部组成。
通过四元数，我们可以方便地描述物体在三维空间中的旋转和方位。
在姿态解算中，我们常常使用四元数来表示物体的姿态。
四元数具有很好的性质，比如可以方便地进行插值和运算，因此在姿态解算中得到了广泛的应用。
卡尔曼滤波是一种常见的姿态解算算法。
它主要通过融合多传感器的数据来进行姿态解算。
卡尔曼滤波利用传感器提供的测量值和先验知识，通过递推的方式对系统状态进行估计和修正，从而得到更准确的姿态信息。
在实际应用中，四元数姿态解算和卡尔曼滤波常常结合起来使用。
通过四元数来表示物体的姿态，然后利用卡尔曼滤波对姿态进行优化和修正。
这种组合可以有效地提高姿态解算的准确性和稳定性。
在具体的实现过程中，四元数姿态解算和卡尔曼滤波需要考虑多个因素，比如传感器类型、传感器误差、姿态模型等。
需要根据实际情况选择合适的算法和参数，以获得最佳的姿态解算效果。
总而言之，在姿态解算和卡尔曼滤波领域，四元数是一种重要的姿态表示方法，而卡尔曼滤波是一种常用的姿态解算算法。
它们的结合可以有效提高姿态解算的准确性和稳定性。
在实际应用中，我们需要根据具体情况选择合适的算法和参数，以获得最佳的姿态解算效果。
通过不断的研究和实践，我们相信四元数姿态解算和卡尔曼滤波在未来会有更广泛的应用。
第４４卷第９期 ２０１７年９月 计算机科学
ＣＯＭＰＵＴＥＲ ＳＣＩＥＮＣＥ Ｖｏ１．４４ Ｎｏ．９
ＳｅＤ．２０１７
基于改进扩展卡尔曼滤波的姿态解算算法研究 冯少江 徐泽字。 石明全。 王晓东ｚ （重庆邮电大学计算机科学与技术学院 重庆４０００６５）１ （中国科学院重庆绿色智能技术研究院 重庆４００７１４）２
摘要为了解决标准扩展卡尔曼滤波器（ＥＫＦ）在多旋翼无人机姿态解算ｑ－精度较低的ｒ－－ｉ ，提出了一种改进扩展 卡尔曼滤波算＂￣（ＢＰＮＮ－ＥＫＦ），使得解算精度得到较大提升。针对ＥＫＦ存在预测模型参数要求具有先验已知性，在 工程实践中难以获得准确的参数，以及标准ＥＫＦ对非线性系统采用线性化模型带来的误差等问题，利用神经网络的 非线性映射能力和自适应能力对标准ＥＫＦ的估计值进行补偿，减小模型以及滤波参数误差对最优估计值的影响，从 而提高最优估计精度。仿真实验证明，ＢＰＮＮ—ＥＫＦ对多旋翼无人机姿态解算精度的提升具有显著作用。 关键词 扩展卡尔曼滤波器，姿态解算算法，非线性系统，ＢＰ神经网络 中图法分类号ＴＰ３９１ 文献标识码Ａ ＤＯＩ １０．１１８９６／ｊ．ｉｓｓｎ １００２—１３７Ｘ．２０１７．０９．０４２
Ｒｅｓｅａｒｃｈ ｏｎ Ａｔｔｉｔｕｄｅ Ａｌｇｏｒｉｔｈｍ Ｂａｓｅｄ ｏｎ Ｉｍｐｒｏｖｅｄ Ｅｘｔｅｎｄｅｄ Ｃａｉｍａｎ Ｆｉｌｔｅｒ ＦＥＮＧ Ｓｈａｏ－ｊｉａｎｇ ＸＵ Ｚｅ－ｙｕｚ ＳＨＩ Ｍｉｎｇ－ｑｕａｎ２ ＷＡＮＧ Ｘｉａｏ－ｄｏｎｇ￣ （Ｃｏｌｌｅｇｅ ｏｆ Ｃｏｍｐｕｔｅｒ Ｓｃｉｅｎｃｅ ａｎｄ Ｔｅｃｈｎｏｌｏｇｙ，Ｃｈｏｎｇｑｉｎｇ Ｕｎｉｖｅｒｓｉｔｙ ｏｆ Ｐｏｓｔｓ ａｎｄ Ｔｅｌｅｃｏｍｍｕｎｉｃａｔｉｏｎｓ，Ｃｈｏｎｇｑｉｎｇ ４０００６５，Ｃｈｉｎａ）１ （Ｒｅｓｅａｒｃｈ Ｉｎｓｔｉｔｕｔｅ ｏｆ Ｃｈｏｎｇｑｉｎｇ Ｇｒｅｅｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｔｅｃｈｎｏｌｏｇｙ，Ｃｈｉｎｅｓｅ Ａｃａｄｅｍｙ ｏｆ Ｓｃｉｅｎｃｅｓ，Ｃｈｏｎｇｑｉｎｇ ４００７１４，Ｃｈｉｎａ）２
Ａ￣ｔｍｅｔ Ｉｎ ｏｒｄｅｒ ｔｏ ｓｏｌｖｅ ｔｈｅ ｐｒｏｂｌｅｍ ｔｈａｔ ｓｔａｎｄａｒｄ ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ（ＥＫＦ）ｉｎ ｔｈｅ ｍｕｌｔｉ—ｒｏｔｏｒ ＵＡＶ ａｔｔｉｔｕｄｅ ｓｏｌｖｅｒ ｈａｓ ｌｏｗｅｒ ａｃｃｕｒａｃｙ，ａｎ ｉｍｐｒｏｖｅｄ ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ ａｌｇｏｒｉｔｈｍ（ＢＰＮＮ－ＥＫＦ）ｗａｓ ｐｒｏｐｏｓｅｄ ｔｏ ｉｍｐｒｏｖｅ ｔｈｅ ａｃｃｕｒａｃｙ ｇｒｅａｔｌｙ．ＥＫＦ ｐｒｅｄｉｃｔｉｏｎ ｍｏｄｅｌ ｐａｒａｍｅｔｅｒｓ ｒｅｑｕｉｒｅ ｔｈｅ ｐｒｅｓｅｎｃｅ ｏｆ ｐｒｉｏｒｉ ｋｎｏｗｎ ｐｒｏｐｅｒｔｉｅｓ，ｂｕｔ ｉｎ ｅｎｇｉｎｅｅｒｉｎｇ ｐｒａｃｔｉｃｅ ｉｔ ｉｓ ｄｉｆｆｉｃｕｌｔ ｔｏ ｏｂｔａｉｎ ａｃｃｕｒａｔｅ ｐａｒａｍｅｔｅｒｓ．Ａｎｄ ｎｏｎｌｉｎｅａｒ ｓｙｓｔｅｍｓ ｕｓｉｎｇ ｌｉｎｅａｒ ｍｏｄｅｌ ｗｉｌｌ ｃａｕｓｅ ｅｒｒｏｒ ｐｒｏｂｌｅｍ ｆｏｒ ｓｔａｎｄａｒｄ ＥＫＦ．Ａｉｍｉｎｇ ａｔ ａｂｏｖｅ ｐｒｏｂｌｅｍｓ，ｗｅ ｕｓｅｄ ｎｏｎｌｉｎｅａｒ ｍａｐｐｉｎｇ ａｂｉｌｉｔｙ ｏｆ ｎｅｕｒａｌ ｎｅｔｗｏｒｋ ａｎｄ ａｄａｐｔｉｖｅ ａｂｉｌｉｔｙ ｔｏ ｃｏｍｐｅｎｓａｔｅ ｔｈｅ ｅｓｔｉｍａｔｅｄ ｖａｌｕｅ ｏｆ ｔｈｅ ｓｔａｎｄａｒｄ ＥＫＦ，ｒｅｄｕｃｅ ｔｈｅ ｉｍｐａｃｔ ｏｆ ｔｈｅ ｍｏｄｅｌ ａｓ ｗｅｌｌ ａｓ ｆｉｌｔｅｒｉｎｇ ｐａｒａｍｅｔｅｒｓ ｅｒｒｏｒ ｆｏｒ ｏｐｔｉｍａｌ ｅｓｔｉｍａｔｅｓ，ｔｈｅｒｅｂｙ ｅｎｈａｎｃｉｎｇ ｏｐｔｉｍａｌ ｅｓｔｉｍａｔｉｏｎ ａｃｃｕｒａｃｙ．Ｔｈｅ ｓｉｍｕｌａｔｉｏｎ ｒｅｓｕｌｔｓ ｓｈｏｗ ｔｈａｔ ＢＰＮＮ－ ＥＫＦ ｐｌａｙｓ ａ ｓｉｇｎｉｆｉｃａｎｔ ｒｏｌｅ ｉｎ ｉｍｐｒｏｖｉｎｇ ｍｕｌｔｉ－ｒｏｔｏｒ ＵＡＶ ａｔｔｉｔｕｄｅ ｓｏｌｖｅｒ ａｃｃｕｒａｃｙ． Ｋｅｙｗｏｒｄｓ Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ，Ａｔｔｉｔｕｄｅ ａｌｇｏｒｉｔｈｍ，Ｎｏｎｌｉｎｅａｒ ｓｙｓｔｅｍ，ＢＰ ｎｅｕｒａｌ ｎｅｔｗｏｒｋ
１ 引言 卡尔曼滤波算法广泛应用在多传感器数据融合、航天器 姿态解算与控制中。卡尔曼滤波在线性系统的假设下要求预 测模型和观测模型的噪声具有先验已知性＿】］。但在工程实践 中所遇到的物理系统数学模型往往不满足这些前提，例如姿 态解算系统的预测模型是非线性的，噪声具有非先验性。无 人机的姿态解算系统采用扩展卡尔曼滤波算法（ＥＫＦ）进 行参数估计与信息融合，该算法针对系统姿态解算参数建 模，滤波器的预测方程是非线性的，在线性化时会引入模 型误差 。 人工神经网络自提出以来，由于良好的非线性映射能力、 较强的自学习能力，应用领域不断拓展＿３］。其中ＢＰ网络模 型是常用的神经网络模型之一。随着神经网络技术的不断发 展以及应用领域的不断延伸，神经网络与卡尔曼滤波有机结 合为新的数据融合方法，成为了国内外在该领域的研究 热点 ＿６＿。 本文采用描述多旋翼无人机姿态变化的运动学模型，从 惯性传感器输出的当前姿态数据人手，将各种误差因素综合 起来看作一个总误差因素，提出了一种改进的扩展卡尔曼滤 波算法。该算法采用ＢＰ神经网络在时间更新部分对标准扩 展卡尔曼滤波器进行补偿，提高了其在动态噪声扰动环境中 的自适应能力。最后，仿真结果表明ＢＰＮＮ—ＥＫＦ有效提升了 姿态角的解算精度。
到稿日期：２０１６—０８—１７返修日期：２０１６—１２—０９ 本文受国家质检公益性行业科研专项项目：民用涵道式无人飞行器和工业机器人质量安全检 测技术研究（Ｙ４２Ｚ１３０ＩｌＯ）资助。 冯少￣＇Ｚ（１９９１一），男，硕士生，主要研究方向为多旋翼无人机姿态解算算法，Ｅ－ｍａｉｌ：ｆｅｎｇｓｈａｏｊｉａ￣＠ｄｇｉｔ．ａｃ，ｏｎ；徐泽宇（１９７３一），硕士，高级工程师， 主要研究方向为精密测量与智能控制技术，Ｅ－ｍａｉｌ：）（ｚｙ＠ｄ西Ｌ ａｃ．ｃｎ；石明全（１９７５一），男，博士，副教授，主要研究方向为虚拟样机技术、复杂机械系 统动态仿真及优化，Ｅ－ｍａｉｌ：ｓｈｉｍｑ＠ｃｉｇｉｔ．ａｃ．ｏｎ；王晓东（１９８７一），男，硕士，工程师，主要研究方向为机电一体化，Ｅ－ｍａｉｌ：Ｓｄｍｎｇ＠ｄ西ｔ ａｃ．ｍ。 ２２８ 计算机科学 ２０１７征 ２改进的扩展卡尔曼滤波算法设计 ２．１ 标准的扩展卡尔曼滤波算法分析 非线性离散系统的方程可由以下形式描述 ： ｆ 一尢 ，是一１］＋ｎ・ Ｉ ＝ｈＥ ，奄］＋ 其中，尢・］是 维向量函数，对其自变量而言是非线性的； ＾ｒ・］是 维向量函数，它对自变量而言也是非线性的；Ｗｋ一 与 为彼此不相关的零均值白噪声序列，与初始状态 不 相关； 为志一１时刻状态方程误差向量的系数矩阵； 为ｋ 时刻的状态向量， 为ｋ时刻的观测向量。对于是一ｌ≥０，有： ｆＥＥＷｋ １＝０，Ｅ［ ｗ ］一 … ＩＥ［ ］一０， Ｅ［ 、］一 ， 对于式（１）对应的非线性系统，由于＿厂［・］和 ［・］不能 直接应用在协方差矩阵的推导中，因此将其转化为求雅克比 矩阵来实现非线性方程的线性化 】。ＥＫＦ算法主要包括以 下递推方程： （１）状态一步预测 ＾ ＾ Ｘｋ１ ｌ— ．☆１ 一１ （３）
（２）状态估计 ＾ ＾ Ａ Ｘ＾一Ｘ ｌ＋Ｋ女（ 一Ｈ Ｘ女Ｉ ｅ １） （４）
（３）滤波增益 Ｋ 一（Ｐｋ ｌＨ２ ）１（Ｈ Ｐ 】Ｈ２、＋ ） （５） （４）一步预测均方误差 一 ｌ 】 ｌ＋ｎ １ ｌ 一１ （６） （５）估计均方误差 一（Ｊ—Ｋ Ｈ ） Ｉ ｌ （７） 其中， 一－为第走一１时刻到第ｋ时刻的状态转移矩阵，Ｈ 为第走时刻的观测方程系数矩阵，Ｑ 为第志时刻的动态噪 声，尺 为第是时刻的量测噪声。 式（３）式（７）是卡尔曼滤波的基本关系式，它是一种递 推式的方法，结合前－－ＨＥＮ的估计值、系统本身的状态方程及 新数据的更新，即可求得新的估计值。从以上推导过程中可 以看到标准卡尔曼滤波的问题： （１）非线性模型线性化的最优估计问题。卡尔曼滤波是 一种线性最小方差估计，一般通过保留泰勒展开式的线性部 分来实现非线性模型的线性化，显然存在线性化误差。 （２）滤波增益的计算与维数问题。由增益的表达式（５）可 以看出，增益是由状态转移矩阵、量测矩阵、过程噪声协方差 矩阵、量测噪声协方差矩阵计算得到的＿ ，这些参数要求具有 先验已知性，但在实际问题中往往不具有这种特性，由于参数 的不准确会导致最优估计值的误差较大，且状态预测协方差 矩阵依然较小，因此增益矩阵将会逐渐失去调整作用，最终导 致滤波值失去收敛性 。 ２．２ ＢＰ网络辅助的扩展卡尔曼滤波算法设计 ＢＰ神经网络是一种智能信息融合技术，具有高度非线性 映射能力和自适应能力，适合于比较复杂的系统Ⅲ］，因此采 用ＢＰ神经网络对扩展卡尔曼滤波估计值进行补偿，从而减 小模型以及滤波参数误差对最优估计值的影响，提高滤波算 法的精度。 ＢＰ网络辅助的扩展卡尔曼滤波算法的原理是：通过设计 好的神经网络结构的初始参数，根据输入样本对ＢＰ网络进 行训练，然后保存训练好的神经网络权值和闯值；在扩展卡尔 曼滤波器执行姿态参数估计时，将影响姿态估计误差的参数 作为神经网络的输入，利用神经网络对整体误差进行调控，利 用神经网络的非线性映射能力和自适应能力从补偿角度修正 扩展卡尔曼滤波的结果，从而提高滤波精度。具体的训练方 法为：获取扩展卡尔曼滤波的状态一步Ｎ，Ｎ值与状态估计值 的差值藏一 作为ＢＰ神经网络的输入样本，将真实值与 Ａ 状态估计值的差值 一 作为ＢＰ神经网络的输 样本，神 经网络通过学习扩展卡尔曼滤波预测误差与实际误差的映射 关系，就可以近似输出滤波估计值与实际值之间的误差 Ｂｐｅｒｒ。 改进的扩展卡尔曼滤波算法流程如图１所示，将传感器 信息以及用于预测方程推导的三轴角加速度输入扩展卡尔曼 滤波，经过时间更新与量测更新，得到滤波结果，同时将滤波 ＾ ＾ 的状态估计值与状态一步预测值差值 一 川输入到训练 好的ＢＰ网络中；然后，网络给出滤波结果与实际值的误差； 最后，对ＥＫＦ滤波的最优估计值与神经网络的输出求和即为 改进的ＥＫＦ算法最优估计值： ＾ ＾ 一Ｘ“ｋ＋Ｂｐｅｒｒ （８） ＾ 其中， 为改进后算法的输出值，Ｂｐｅｒｒ为ＢＰ神经网络的输 ＾ 出值， 为ＥＫＦ状态估计值。 机体坐标系下多旋翼无人 机的角速度、角加速度、 加速度、磁力计输出值 銮 銮 ｌ预测值与估计值之差ｔ ＢＰ神经网络 网络输出 图ｌ改进的扩展卡尔曼滤波算法示意陶 ３姿态解算仿真实验与结果分析 ３．１样本数据说明 从多旋翼无人机飞行控制器飞控日志中提取２０００组 ＩＭＵ与ＭＡＧ数据作为样本，其中ＩＭＵ为三轴陀螺仪和三 轴加速度计的测量值，ＭＡＧ为三轴磁力计的测量值。多旋 翼无人机在飞行过程中的姿态变化较慢，姿态角的变化范围 在１Ｏ。以内。 ３．２ ＢＰ神经网络训练及性能评价 由于姿态角的变化具有时序性，取２０００组样本的前 １８００组作为ＢＰ网络的离线训练样本，其余２００组作为验证 样本。采用ＭＡＴＡＬＡＢ Ｒ２０１２ｂ编程，网络结构为９—２０—９，因 为为了方便角速度的动力学建模而在预测模型中引人了角加 速度，但没有对应的传感器，所以状态值中的第３～５维数据 即角加速度无法作为神经网络的输入值，因此输入样本为 （ （１：３）； （７：１２））一（ ｌ（１：３）； Ｉ １（７：１２）），输出样 本为 一（ （１：３）； （７：１２）），隐层单元的传递函数取ｔａｎ— ｓｉｇ，其表达式为： 一２／［１－－ｅｘｐ（－－２ ）］一１ （９） 输出层神经元采用线性函数ｐｕｒｅｌｉｎ对神经网络进行训
