2017 年秋季学期研究生课程考核 （读书报告、研究报告） 考核科目:雷达系统导论 学生所在（系）:电子与信息工程学院 学生所在学科:电子与同学工程 学生姓名: 学号: 学生类别: 考核结果阅卷人 第 1 页（共页）
几种非线性滤波算法的介绍与性能分析 作者姓名：学号： 专业院系：电信学院电子工程系 电子邮件： 摘要—非线性滤波算法在雷达目标跟踪中有着重要的应用，对雷达的跟踪性能有着至关重要的影响。好的滤波算法有利于目标航迹的建立及保持，能够得到较精确的目标位置，为发现目标后的后续工作提供可靠的数据依据。本文重点介绍了雷达数据处理中的几种非线性滤波算法：扩展卡尔曼滤波（EKF）、不敏卡尔曼滤波（UKF）、粒子滤波（PF），并且给出了一个利用这三种算法进行数据处理的一个实例，通过这个实例对比分析了这三种算法的性能以及优劣。 关键字—非线性滤波算法；扩展卡尔曼滤波；不敏卡尔曼滤波；粒子滤波； I.概述（一级表题格式） 在雷达对目标进行跟踪前要先对目标进行检测。对于满足检测条件的目标就需要进行跟踪，在跟踪的过程中可以利用新获得的数据完成对目标的进一步检测比如去除虚假目标等，同时利用跟踪获得数据可以进一步完成对目标动态特性的检测和识别。因此对目标进行准确的跟踪是雷达性能的一个重要指标。在检测到满足条件的目标后，根据目标运动状态建立目标运动模型，然后对目标跟踪算法进行设计，这是雷达目标跟踪中的核心部分。 目前主要的跟踪算法包括线性自回归滤波，两点外推滤波、维纳滤波、- αβ滤波、加权最小二乘滤波、维纳滤波和卡尔曼滤波[1]。对于线性系统而言最优滤波的方法就是卡尔曼滤波，卡尔曼滤波是线性高斯模型下的最优状态估计算法。但是实际问题中目标的运动模型往往不是线性的，因此卡尔曼滤波具有很大的局限性。目前主要用的非线性滤波算法可以分为高斯滤波和粒子滤波[2]。不敏卡尔曼滤波和扩展卡尔曼滤波就是高斯滤波中的典型代表，也是应用相对较为广泛的。粒子滤波的应用范围比高斯滤波的适用范围要广，对于系统状态非线性，观测模型非高斯等问题都有很好的适用性。本文具体分析阐述了扩展卡尔曼滤波算法，不敏卡尔曼滤波算法，粒子滤波算法，并且通过一个实例利用仿真的方法分析了这三种算法在滤波性能上的优劣，最后对这三种算法做了一定的总结。 我本科毕业设计题目为《基于历史数据的路径生成算法研究》，由于我是跨专业保研到电信学院，该课题所研究内容不属于雷达系统研究范围，是一种城市路网最快路径生成算法。 II.几种非线性滤波算法 A.扩展卡尔曼滤波 扩展卡尔曼滤波是将非线性系统转换为近似的线性系统的一种方法，其核心思想是围绕滤波值将非线性函数展开成泰勒级数并略去二阶及以上的项，得到一个近似的线性化模型，然后应用卡尔曼滤波完成状态估计。 扩展卡尔曼滤波状态空间模型： k k k w x f+ = + ) ( x 1 状态方程 k k k v x h+ =) ( z观测方程 其中(.) f和(.) h为非线性函数 在扩展卡尔曼滤波中，状态的预测以及观测值的预测由非线性函数计算得出，线性卡尔曼滤波中的状态转移矩阵A阵和观测矩阵H阵由f和h函数的雅克比矩阵代替。 对 (.) f和(.) h Taylor展开，只保留一次项有： ) ? ( ) ?( ) ( k k k k k x x A x f x f- + ≈ ) ? ( ) ?( ) ( k k k k k x x H x h x h- + ≈ 其中： k k x x k k dx df A ?= =为f对 1- k x求导的雅克比矩阵 k k x x k k dx dh H ?= =为h对 1- k x求导的雅克比矩阵 ) ?( ? 1-k k x f x=，于是可以得出： k k k k k k k w x A x f x A x+ - + ≈ + ) ? ) ?( ( 1 k k k k k k k v x H x h x H z+ - + ≈ + ) ? ) ?( ( 1 通过以上变换，将非线性问题线性化。接下来EKF 滤波过程同线性卡尔曼滤波相同，公式如下: )) | (?( ) |1 ( X?k k X f k k= + ) ( ) ( ) | ( ) ( ) |1 (P k Q k k k P k k k+ Φ' Φ = + )1 ( )1 ( ) |1 ( )1 ( )1 (S+ + + ' + + = +k R k H k k P k H k )1 ( )1 ( ) |1 ( )1 ( K1+ + ' + = +-k S k H k k P k
Kalman_Filter(float Gyro,float Accel) { Angle+=(Gyro - Q_bias) * dt; Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; Pdot[1]= - PP[1][1]; Pdot[2]= - PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; PP[0][1] += Pdot[1] * dt; PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * PP[0][1]; PP[0][0] -= K_0 * t_0; PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; Gyro_x = Gyro - Q_bias; } 首先是卡尔曼滤波的5个方程： -=--+（1）先验估计 X k k AX k k Bu k (|1)(1|1)() -=--+（2）协方差矩阵的预测(|1)(1|1)' P k k AP k k A Q
3DM-GX4-25 是一个高性能的微型航姿参考系统(AHRS)，使用最先进MEMS 传感技术。它联合使用一个三向加速度计、一个三向陀螺仪、一个三向磁力计、一个温度传感器、一个气压高度计和一个运算复杂的卡尔曼滤波器的内嵌双核微处理器。从而为用户提供精确的静态和动态姿态估计值和惯性测量。这些技术使得3DM-GX4-25成为同级别产品中体积最小和重量最轻的设备。 优点 ? 高性能陀螺仪 ? 噪声密度: 0.005°/sec/√Hz ? 漂移: 10°/hr ? g 2 灵敏度: 0.003°/s/g 2 rms ? 专用的运算微处理器，提供精确的位置、速度和姿态估值数据 ? 高速采样频率及多种数据输出方式 ? -40 °C to +85 °C 工作环境温度 ? 动态环境下高性能和高稳定性 ? 市场上最小，最轻的带自适应卡尔曼滤波器的微型航姿参考系统(AHRS) ? SDK 软件开发包及开放通信协议，易于集成开发自主系统 应用 在动态环境下为用户提供非常稳定和精确的导航和姿 态数据 ?无人机车导航 ? 平台稳定性和人工地平线 ? 天线和相机指向 ? 机车健康及使用状态监测 ? 侦察, 监视,目标截获 ? 机器人控制 ? 人员追踪 简介 3DM-GX4-25微型航姿参考系统提供各种输出数据参量，从完全标定的惯性测量（加速度，角速度和磁场或角度增量和速度向量增量）到计算的定向估值（欧拉角--俯仰、滚动、偏转；旋转矩阵和四元素。由于使用了复杂的自适应卡尔曼滤波器，运算的估值数据不会受到磁场和直线运动的干扰。偏移追踪和传感器噪声模式可以让用户对自己的设备应用进行微调，从而达到更好的测量效果。所有参量都经过温度补偿和数学运算再转换到正交坐标系统。 系统的架构设计已经充分消除了多种可能的误差源：如由于温度变化引起的增益和补偿误差；由于电源电压波动引起的灵敏度变化等因素。陀螺仪漂移非常小，基于复杂的运算估值技术，3DM-GX4-25性能已经接近满足战术应用级别的要求。
车载“动中通”通信系统的应用极为广泛，从行驶在戈壁搓板路面的“神舟”系列搜救车，到行驶在高楼林立的城市公路的公安系统通信车，再到不可知环境的应急系统通信车，林林总总。“动中通”通信系统工作的前提是通信天线要在载车行驶条件下精确对准卫星，而载车的机动性和路面的复杂性增加了其难度，一方面要求天线响应快，在桥梁、树木、建筑物、山体和隧道等遮挡后迅速完成对卫星目标的重捕，另一方面在载体剧烈扰动时能精确地自动跟踪卫星目标[1]。本文通过高精度单脉冲自跟踪系统和光纤捷联航姿系统在天线控制系统中的联合使用，使这一要求得以实现。 1天线控制系统设计 高精度的“动中通”天线控制系统一般采用的工作方式 为伪单脉冲单通道[2]的自动跟踪模式。所谓单脉冲就是在一个脉冲上可得到完整的目标偏离天线电轴的误差信息，因此响应快；伪单脉冲自跟踪也为零值跟踪，工作在和信号的最大点，差信号的零点，该处差斜率最大[3]，所以跟踪精度高，抗颠簸、冲击、振动能力强。“动中通”天线的半功率波束宽度较窄，动态条件下没有航姿系统的引导，自身捕获卫星目标的时效性较差。 光纤捷联航姿系统是一种把光纤陀螺和加速度计直接连接在动载体上测量载体的角加速度分量和角速度分量，航姿处理器根据车体坐标系和导航坐标系的方向余弦阵，计算出沿导航坐标系3个轴上的加速度分量，进而计算出载体的航向和姿态信息[4]。光纤捷联航姿系统是一种辅助式惯性系统，成本低可靠性高[5]，它不具备自对准功能，利用单脉冲跟踪系统对卫星目标高精度跟踪的角度数据，实时标定光纤捷联航姿系统初值，可以使航姿系统长期保持在高精度的惯性导航水平上。天线控制系统在进行卫星目标捕获时，工作方式为航姿系统引导天线指向卫星目标[6]，目标进入天线电轴的半功率波束宽度内，天线控制系统自动转换到单脉冲自动跟踪模式，完成天线对卫星目标的捕获任务而进入高精度自动跟踪状态。高精度单脉冲自跟踪系统和光纤捷联航姿系统在天线控制系统中的联合使用，使“动中通”通信系统天线控制系统设备同时具备快速捕星和精确跟踪的能力。 “动中通”通信系统天线控制系统框图如图1所示。当单脉冲自跟踪支路故障时，作为一种冗余手段，天线 光纤捷联航姿系统在“动中通”的应用 董蓉霞 （中国电子科技集团公司第39研究所陕西 西安710065） 摘要：为解决各种环境下“动中通”天线对目标的快速捕获和精确跟踪，天线控制系统采用了低成本光纤捷联航姿系统与单脉冲自跟踪系统相结合的控制技术，目标可见时采用单脉冲自跟踪，同时利用单脉冲自跟踪精度高的特点对光纤捷联航姿系统实时标校，使航姿系统长期保持在高精度的惯性导航水平上；目标被遮挡后利用光纤捷联航姿系统引导跟踪，通过试验及数据分析，该技术的使用使“动中通”天线在任何环境下，跟踪精度达1/10半功率波束宽度，对目标的捕获时间小于1s 。 关键词：动中通；光纤捷联航姿系统；单脉冲自跟踪系统；捕获；跟踪中图分类号：TN820.3 文献标识码：A 文章编号：1674－6236（2010）05-0076-02 Application of FOG strapdown attitude and jeading reference system in -motion satellite communication DONG Rong -xia （The 39th Institute ，Electronic Science and Technology Group Corporation of China ，Xi ’an 710065，China ） Abstract ：To find a solution to search for communication satellite quickly and track satellite accurately in -Motion satellite communication.There is a new control technique which combines low cost FOG strapdown attitude and heading reference system and monopulse autotracking system in antenna control equipment.Monopulse autotracking system is adopted when communication satellite is seen ，at one time ，FOG strapdown attitude and heading reference system is real time proofread with monopulse autotracking system.When communication satellite is obstructed ，FOG strapdown attitude and heading ref -erence system is adopted for guiding tracking.Finally via test data analysis ，it is achieved that tracking accuracy is 1/10HPBW and capture time is less than 1second in kinds of environment. Key words ：in -motion satellite communication ；FOG strapdown attitude and heading reference system ；monopulse auto -tracking system ；search ；track 收稿日期：2009-10-13 稿件编号：200910040 作者简介：董蓉霞（1971—），女，山西临猗人，高级工程师。研究方向：天线控制技术。 电子设计工程 Electronic Design Engineering 第18卷 Vol.18 第5期No.52010年5月May.2010 －76－
微型航姿参考系统MAHRS 3DM-S10A/B 技术手册 Version 1.02a 2011年9月10日
1. 简介 微型航姿参考系统（MAHRS）3DM-S10A/B是一款微型的全姿态测量传感装置，它由三轴MEMS陀螺、三轴MEMS加速度计、三轴磁阻型磁强计等三种类型的传感器构成。三轴陀螺用于测量载体三个方向的的绝对角速率，三轴加速度计用于测量载体三个方向的加速度，在系统工作中，主要作用是感知系统的水平方向的倾斜，并用于修正陀螺在俯仰和滚动方向的漂移，三轴磁阻型磁强计测量三维地磁强度，用于提供方向角的初始对准以及修正航向角漂移。微型航姿参考系统3DM-S10A/B可提供的输出数据有：原始数据、四元数、姿态数据等（产品外形如图1所示）。 图1 微型航姿参考系统（MAHRS）3DM-S10A/B外形（左 A型、右 B型） 2. 特性 实时三轴惯性输出； 大于100Hz的内部更新率； 尺寸小、重量轻； 低功耗； RS-485A串行接口，方便连接。 3. 应用领域 车载控制系统； 航空航天电子系统； 虚拟现实系统； 平台稳定系统； 机器人控制等。 4. 工作原理及软硬件框图 微型航姿参考系统（MAHRS）3DM-S10A/B采用陀螺来测量载体的绝对角速率，然后载体的姿态角（航向角、俯仰角和滚动角）就可以通过陀螺输出数据的特定的解算方法来获得。加速度计用于确定3DM-S10A/B初始的姿态，并修正并确定陀螺在水平方向（俯仰角和滚动角）的漂移。三轴磁阻
型磁强计用于提供方位角的初始对准以及修正陀螺的航向角漂移。微型航姿参考系统（MAHRS）3DM-S10A/B内部的嵌入式处理器中设计有传感器融合算法，重力测量值（通过加速度计）和磁北向测量值（通过磁力计）有机融合并对漂移进行补偿，避免了因为角速率数据的积分运算带来的误差的无限累积。而微型航姿参考系统（MAHRS）3DM-S10A/B比无陀螺的电子罗盘具有很大的优势，在动态情况下它能够精确解算航姿，而一般的电子罗盘只能在静态条件下运用，在动态情况下无法得到精确的航姿信息。微型航姿参考系统（MAHRS）3DM-S10A/B可以可靠提供全姿态信息和原始数据，数据更新率可达到100Hz以上。 图2 微型航姿参考系统（MAHRS）3DM-S10A/B硬件框图 图3 微型航姿参考系统（MAHRS）3DM-S10A/B软件框图
第十三章 卡尔曼滤波 在本章中，我们介绍一种被称为卡尔曼滤波的十分有用的工具。卡尔曼滤波的基本思想是将动态系统表示成为一种称为状态空间表示的特殊情形。卡尔曼滤波是对系统线性投影进行序列更新的算法。除了一般的优点以外，这种算法对计算确切的有限样本预测、计算Gauss ARMA 模型的确切似然函数、估计具有时变参数的自回归模型等，都提供了重要方法。 §13.1 动态系统的状态空间表示 我们已经介绍过一些随机过程的动态表示方法，下面我们在以前的假设基础上，继续分析动态系统的表示方法。 13.1.1 继续使用的假设 假设t y 表示时刻t 观测到的n 维随机向量，一类非常丰富的描述t y 动态性的模型可以利用一些可能无法观测的被称为状态向量(state vector)的r 维向量t ξ表示，因此表示t y 动态性的状态空间表示(state-space representation)由下列方程系统给出： 11+++=t t t v ξF ξ 状态方程(state model) (13.1) t t t w ξH x A y t +'+'= 量测方程(observation model) (13.2) 这里F ，A '和H '分别是阶数为r r ?，k n ?和r n ?的参数矩阵，t x 是1?k 的外生或者前定变量。方程(13.1)被称为状态方程(state model)，方程(13.2)被称为量测方程(observation model)，1?r 维向量t v 和1?n 维向量t w 都是向量白噪声，满足： ???≠=='τ ττ t t E t ,,)(0Q v v (13.3) ? ??≠=='τττt t E t ,,)(0R w w (13.4) 这里Q 和R 是r r ?和n n ?阶矩阵。假设扰动项t v 和t w 对于所有阶滞后都是不相关的，即对所有t 和τ，有： 0w v =')(τ t E (13.5) t x 是外生或者前定变量的假定意味着，在除了包含在121,,,y y y Λ--t t 内的信息以外，t x 没有为s t +ξ和s t +w (Λ,2,1,0=s )提供任何新的信息。例如，t x 可以包括t y 的滞后值，也可以包括与τξ和τw (任意τ)不相关的变量。 方程系统中方程(13.1)至方程(13.5)可以表示有限观测值的序列 },,,{21T y y y Λ，这时需要状态向量初始值1ξ。假设1ξ与t v 和t w 的任何实现都不
10.6 卡尔曼滤波器简介 本节讨论如何从带噪声的测量数据把有用信号提取出来的问题。通常，信号的频谱处于有限的频率范围内，而噪声的频谱则散布在很广的频率范围内。如前所述，为了消除噪声，可以把 FIR滤波器或IIR滤波器设计成合适的频带滤波器，进行频域滤波。但在许多应用场合，需要进行时域滤波，从带噪声的信号中提取有用信号。虽然这样的过程其实也算是对信号的滤波，但所依据的理论,即针对随机信号的估计理论,是自成体系的。人们对随机信号干扰下的有用信号不能“确知”，只能“估计”。为了“估计”，要事先确定某种准则以评定估计的好坏程度。最小均方误差是一种常用的比较简单的经典准则。典型的线性估计器是离散时间维纳滤波器与卡尔曼滤波器。 对于平稳时间序列的最小均方误差估计的第一个明确解是维纳在1942年2月首先给出的。当时美国的一个战争研究团体发表了一个秘密文件，其中就包括维纳关于滤波问题的研究工作。这项研究是用于防空火力控制系统的。维纳滤波器是基于最小均方误差准则的估计器。为了寻求维纳滤波器的冲激响应，需要求解著名的维纳-霍夫方程。这种滤波理论所追求的是使均方误差最小的系统最佳冲激响应的明确表达式。这与卡尔曼滤波（Kalman filtering）是很不相同的。卡尔曼滤波所追求的则是使均方误差最小的递推算法。 在维纳进行滤波理论研究并导出维纳-霍夫方程的十年以前，在1931年，维纳和霍夫在数学上就已经得到了这个方程的解。 对于维纳-霍夫方程的研究，20世纪五十年代涌现了大量文章，特别是将维纳滤波推广到非平稳过程的文章甚多，但实用结果却很少。这时正处于卡尔曼滤波问世的前夜。 维纳滤波的困难问题，首先在上世纪五十年代中期确定卫星轨道的问题上遇到了。1958年斯韦尔林（Swerling）首先提出了处理这个问题的递推算法，并且立刻被承认和应用。1960年卡尔曼进行了比斯韦尔林更有意义的工作。他严格地把状态变量的概念引入到最小均方误差估计中来，建立了卡尔曼滤波理论。空间时代的到来推动了这种滤波理论的发展。 维纳滤波与卡尔曼滤波所研究的都是基于最小均方误差准则的估计问题。 维纳滤波理论的不足之处是明显的。在运用的过程中，它必须把用到的全部数据存储起来，而且每一时刻都要通过对这些数据的运算才能得到所需要的各种量的估值。按照这种滤波方法设置的专用计算机的存储量与计算量必然很大，很难进行实时处理。虽经许多科技工作者的努力，在解决非平稳过程的滤波问题时，给出能用的方法为数甚少。到五十年代中期，随着空间技术的发展，这种方法越来越不能满足实际应用的需要，面临了新的挑战。尽管如此，维纳滤波理论在滤波理论中的开拓工作是不容置疑的，维纳在方法论上的创见，仍然影响着后人。 五十年代中期，空间技术飞速发展，要求对卫星轨道进行精确的测量。为此，人们将滤波问题以微分方程表示，提出了一系列适应空间技术应用的精练算法。1960年
卡尔曼滤波算法实现代码 C++实现代码如下： ============================kalman.h================= =============== // kalman.h: interface for the kalman class. // ////////////////////////////////////////////////////////////////////// #if !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__IN CLUDED_) #define AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLU DED_ #if _MSC_VER > 1000 #pragma once #endif// _MSC_VER > 1000 #include
kalman(int x=0,int xv=0,int y=0,int yv=0); //virtual ~kalman(); }; #endif// !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C 0__INCLUDED_) ============================kalman.cpp=============== ================= #include "kalman.h" #include
微型航姿参考系统 MAHRS 3DM-S10A/B 用户指南 Version 1.31 2012 年 6 月 10 日 1. 简介 微型航姿参考系统(MAHRS 3DM-S10系列是一款微型的全姿态测量传感装置,它由三轴 MEMS 陀螺、三轴 MEMS 加速度计、三轴磁阻型磁强计等三种类型的传感器构成。三轴陀螺用于测量载体三个方向的绝对角速率,三轴加速度计用于测量载体三个方向的加速度,在系统工作中, 主要作用是感知系统的水平方向的倾斜,并用于修正陀螺在俯仰和滚动方向的漂移,三轴磁阻型磁强计测量三维地磁强度,用于提供方向角的初始对准以及修正航向角漂移。微型航姿参考系统 3DM-S10系列,可提供的输出数据有:原始数据、四元数、姿态数据等(产品外形如图 1所示。 图 1 微型航姿参考系统(MAHRS 3DM-S10系列外形(A 型 / B型 2. 特性 实时三轴惯性输出; 大于 100Hz 的内部更新率; 尺寸小、重量轻;
低功耗; RS-232/RS-485A串行接口,方便连接。 3. 应用领域 车载控制系统; 航空航天电子系统; 虚拟现实系统; 平台稳定系统; 机器人控制等。 4. 工作原理及软硬件框图 微型航姿参考系统(MAHRS 3DM-S10系列采用陀螺来测量载体的绝对角速率,然后载体的姿态角(航向角、俯仰角和滚动角就可以通过陀螺输出数据的特定的解算方法来获得。加速度计用于确定 3DM-S10系列初始姿态角,并修正并确定陀螺在水平方向(俯仰角和滚动角的漂移。 三轴磁阻型磁强计用于提供方位角的初始对准以及修正陀螺的航向角漂移。微型航姿参考系统 (MAHRS 3DM-S10系列内部的嵌入式处理器中设计有传感器融合算法,重力测量值(通过加速度计和磁北向测量值(通过磁力计有机融合并对漂移进行补偿,避免了因为角速率数据的积分运算带来的误差的无限累积。而微型航姿参考系统(MAHRS 3DM-S10系列相比无陀螺的电子罗盘具有很大的优势, 在动态情况下它能够精确解算航姿, 而一般的电子罗盘只能在静态条件下运用, 在动态情况下无法得到精确的航姿信息。微型航姿参考系统(MAHRS 3DM-S10系列可以可靠提供全姿态信息和原始数据,数据更新率可达到 100Hz 以上。
卡尔曼滤波的原理以及应用 滤波，实质上就是信号处理与变换的过程。目的是去除或减弱不想要成分，增强所需成分。卡尔曼滤波的这种去除与增强过程是基于状态量的估计值和实际值之间的均方误差最小准则来实现的，基于这种准则，使得状态量的估计值越来越接近实际想要的值。而状态量和信号量之间有转换的关系，所以估计出状态量，等价于估计出信号量。所以不同于维纳滤波等滤波方式，卡尔曼滤波是把状态空间理论引入到对物理系统的数学建模过程中来，用递归方法解决离散数据线性滤波的问题，它不需要知道全部过去的数据，而是用前一个估计值和最近一个观察数据来估计信号的当前值，从而它具有运用计算机计算方便，而且可用于平稳和不平稳的随机过程(信号)，非时变和时变的系统的优越性。 卡尔曼滤波属于一种软件滤波方法，概括来说其基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。其所得到的解是以估计值的形式给出的。 卡尔曼滤波过程简单来说主要包括两个步骤：状态变量的预估以及状态变量的校正。预估过程是不考虑过程噪声和量测噪声，只是基于系统本身性质并依靠前一时刻的估计值以及系统控制输入的一种估计；校正过程是用量测值与预估量测值之间的误差乘以一个与过程
噪声和量测噪声相关的增益因子来对预估值进行校正的，其中增益因子的确定与状态量的均方误差有关，用到了使均方误差最小的准则。而这一过程中体现出来的递归思想即是：对于当前时刻的状态量估计值以及均方误差预估值实时进行更新，以便用于下一时刻的估计，使得系统在停止运行之前能够源源不断地进行下去。 下面对于其数学建模过程进行详细说明。 1.状态量的预估 （1）由前一时刻的估计值和送给系统的可控制输入来预估计当前时刻状态量。 X(k|k-1)=A X(k-1|k-1)+B U(k) 其中，X(k-1|k-1)表示前一时刻的估计值，U(k)表示系统的控制输入，X(k|k-1)表示由前一时刻估计出来的状态量的预估计值，A表示由k-1时刻过渡到k时刻的状态转移矩阵，B表示控制输入量与状态量之间的一种转换因子，这两个都是由系统性质来决定的。 （2）由前一时刻的均方误差阵来预估计当前时刻的均方误差阵。 P(k|k-1)=A P(k-1|k-1)A’+Q 其中，P(k-1|k-1)是前一时刻的均方误差估计值，A’代表矩阵A 的转置，Q代表过程噪声的均方误差矩阵。该表达式具体推导过程如下： P(k|k-1)=E{[Xs(k|k)-X(k|k-1)][Xs(k|k)-X(k|k-1)]’}------ 其中Xs(k|k)=A Xs(k-1|k-1)+B U(k)+W(k-1)表示当前时刻的实际值，Xs(k-1|k-1)表示前一时刻的实际值，可以看出与当前时刻的预估计值
卡尔曼滤波简介及其算法实现代码 卡尔曼滤波算法实现代码（C，C＋＋分别实现） 卡尔曼滤波器简介 近来发现有些问题很多人都很感兴趣。所以在这里希望能尽自己能力跟大家讨论一些力所能及的算法。现在先讨论一下卡尔曼滤波器，如果时间和能力允许，我还希望能够写写其他的算法，例如遗传算法，傅立叶变换，数字滤波，神经网络，图像处理等等。 因为这里不能写复杂的数学公式，所以也只能形象的描述。希望如果哪位是这方面的专家，欢迎讨论更正。 卡尔曼滤波器– Kalman Filter 1．什么是卡尔曼滤波器 （What is the Kalman Filter?） 在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！ 卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载： https://www.doczj.com/doc/b311928506.html,/~welch/media/pdf/Kalman1960.pdf。 简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2．卡尔曼滤波器的介绍 （Introduction to the Kalman Filter） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就
摘要：诺斯罗普·格鲁曼和利特夫公司正在通过使用目标精度为5°/小时的陀螺仪和2.5毫克的加速度计为未来的航姿参考系统（AHRS）开发基于惯性测量单元（IMU）的MEMS （微机电系统）。在科技发展阶段，原型单轴陀螺仪已经能够实现和广泛的测试包括温度，声音和振动敏感的效果。这些设备采用深反应离子蚀刻（DRIE）来处理微加工全硅陀螺仪传感器芯片。硅熔融粘结确保压力比310-2毫巴小。在复杂的模拟电子和数字信号处理条件中电容拾取信号，并实现全闭环操作。目前的结果与整体偏差小于2°/ h到5°/ h，比例因子误差<1200 ppm，测量范围>1000°/ s和角度随机游走较小<0.4°/√h表明，稳定生产5°/ h的陀螺仪是现实的。电容的制造工艺，摆式加速度计芯片是基于仅在陀螺上增加了封闭的压力来获得过临界阻尼。在一个数字控制环路中的脉冲宽度调制（PWM）被用来实现闭环操作。加速度计芯片已经测试过一个残留偏移误差<2.0毫克和比例因子误差<1400ppm的温度。这些传感器芯片已集成到一个IMU，其中传感器电子的功率预算和尺寸已经进行了优化。本文中的陀螺仪和加速度计的设计的显著特征是与IMU系统架构的概述一起呈现的。测量结果侧重于环保特性和耐用性。 1.引言 航姿参考系统（AHRS）提供相对于地球重力矢量的俯仰角和横滚角和相对于真北方向的偏航角。这样的系统都需要一个惯性测量单元（IMU），其能够测量三轴角速率和加速度。可以采用增强磁力的方法来以提高长期稳定性。迄今为止，角速度测量通常用于使用机械，光纤或环形激光陀螺仪的测量。然而所有三种技术都能够提供AHRS系统所需的性能，它们都有在实现低生产成本的解决方案时的限制。相比之下微机电系统（MEMS），产生了使用间歇过程，有着大幅调低一个AHRS系统成本的潜力。在超过十年的时间里，MEMS传感器测量加速度和角速度已经非常普遍的存在于许多应用领域中，如汽车安全系统和消费电子产品[1]。然而，虽然MEMS加速度计已经成功的应用于AHRS系统[2]本领域迄今仍然是MEMS 陀螺仪的一个挑战。 据我们所知，根据美国联邦航空管理局（FAA）技术标准超五类令（TSO-超五类）关于陀螺（DG）模型的操作方向，没有AHRS利用MEMS陀螺仪就已经被认定为合格。 DG模式用于多种应用包括大型商用飞机，飞行接近地磁北极以及直升机从石油钻井平台或其他大型金属结构来回。IMU被用于DG模式操作所需要的总误差不应超过5°/小时（1σ）。与此相反，典型的汽车传感器在每秒度被指定。 AHRS加速度计需要总误差不超过2.5毫克（1σ）。 MEMS陀螺仪的整体偏移误差由传感器的环境，即温度，振动和噪音的变化造成的。在典型AHRS环境中振动和噪声的电平是中等的并且挑战主要是获得足够低的温度误差。对于在苛刻环境条件下衍生应用的潜在用途的IMU（如战术导弹），它在用于设计传感器元件对振动和声学干扰的高耐用性，同时保持较低的温度敏感性时就成了必不可少的。 2. MEMS陀螺仪 MEMS陀螺仪已经开发并演示了近二十年[1]同时被广泛应用于汽车和消费应用。最近公布的MEMS陀螺仪[3]显示了在角度随机游走（ARW）和偏置稳定性（即闪烁噪声，有时也被称为偏置漂移）方面有显著地提高性能。然而，在结合优秀的ARW和偏置不稳定与其他要求包括带宽与温度稳定性时， MEMS陀螺仪有着困难。首次提出以下的概念，介绍了航空电子/战术级IMU闭环MEMS陀螺仪。 A．基本概念 开盖传感器陀螺仪的一个截面示意图显示在图1（a），一个俯视示意图显示在图1（b），装置的一个显微照显示在图1（c）。微硅陀螺的工作原理是基于一种改性的两平面内振动模式的音叉配置。所谓“双线结构”（DLC）的激发和检测模型是由两个正交，线性反相振荡器组成。由于永久驱动励磁转速绕垂直于被两个振荡模式所定义平面的轴，结果在科里奥
自适应卡尔曼滤波 卡尔曼滤波发散的原因 如果卡尔曼滤波是稳定的，随着滤波的推进，卡尔曼滤波估计的精度应该越来越高，滤波误差方差阵也应趋于稳定值或有界值。但在实际应用中，随着量测值数目的增加，由于估计误差的均值和估计误差协方差可能越来越大，使滤波逐渐失去准确估计的作用，这种现象称为卡尔曼滤波发散。 引起滤波器发散的主要原因有两点： （1）描述系统动力学特性的数学模型和噪声估计模型不准确，不能直接真实地反映物理过程，使得模型与获得的量测值不匹配而导致滤波发散。这种由于模型建立过于粗糙或失真所引起的发散称为滤波发散。 （2）由于卡尔曼滤波是递推过程，随着滤波步数的增加，舍入误差将逐渐积累。如果计算机字长不够长，这种积累误差很有可能使估计误差方差阵失去非负定性甚至失去对称性，使滤波增益矩阵逐渐失去合适的加权作用而导致发散。这种由于计算舍入误差所引起的发散称为计算发散。 针对上述卡尔曼滤波发散的原因，目前已经出现了几种有效抑制滤波发散的方法，常用的有衰减记忆滤波、限定记忆滤波、扩充状态滤波、有限下界滤波、平方根滤波、和自适应滤波等。这些方法本质上都是以牺牲滤波器的最优性为代价来抑制滤波发散，也就是说，多数都是次优滤波方法。 自适应滤波 在很多实际系统中，系统过程噪声方差矩阵Q和量测误差方差阵R事先是不知道的，有时甚至连状态转移矩阵 或量测矩阵H也不能确切建立。如果所建立的模型与实际模型不符可能回引起滤波发散。自适应滤波就是这样一种具有抑制滤波发散作用的滤波方法。在滤波过程中，自适应滤波一方面利用量测值修正预测值，同时也对未知的或不确切的系统模型参数和噪声统计参数进行估计修正。自适应滤波的方法很多，包括贝叶斯法、极大似然法、相关法与协方差匹配法，其中最基本也是最重要的是相关法，而相关法可分为输出相关法和新息相关法。 在这里只讨论系统模型参数已知，而噪声统计参数Q和R未知情况下的自适应滤波。由于Q和R等参数最终是通过增益矩阵K影响滤波值的，因此进行自适应滤波时，也可以不去估计Q和R等参数而直接根据量测数据调整K就可以了。
Kalman 滤波算法 姓名：刘金强 专业：控制理论与控制工程 学号：2007255 ◆实验目的： （1）、掌握klman 滤波实现的原理和方法 （2）、掌握状态向量预测公式的实现过程 （3）、了解Riccati 差分方程实现的过程和新息的基本性质和过程的计算 ◆实验要求： 问题： F=[a1,a2,a3],其中a1=[1.0 0 0]的转置，a2=[0.3 1.0 0]的转置，a3=[0.1 0.2 0.4]的转置，x(0)=[3,-1,2]的转置；C=[b1,b2,b3],其中b1=[0.3 0.5]的转置，b2=[1,0.4]的转置，b3=[0.8 -0.7]的转置；V1(n)=[0 0 n1(n)sin(0.1n)]的转置，V2(n)=[n2(n) n3(n)];n1(n)为均值为零，方差为1的均匀分布白噪声;n2(n),n3(n)为均值为0，方差为0.1的均匀分布白噪声,n1(n),n2(n),n3(n)相互独立，试用卡尔曼滤波器算法估计x^(n). ◆实验原理： 初始条件： 1?(1)x =E{x(1)} K(1,0)=E{[x(1)- (1)x ][x(1)- (1)H x ]},其中(1)x =E{x(1)} 输入观测向量过程： 观测向量序列={y(1),…………y(n)} 已知参数： 状态转移矩阵F(n+1,n) 观测矩阵C(n) 过程噪声向量的相关矩阵1()Q n 观测噪声向量的相关矩阵2()Q n 计算：n=1,2,3,………………. G(n)=F(n+1,n)K(n,n+1) ()H C n 12[()(,1)()()]H C n K n n C n Q n --+ Kalman 滤波器是一种线性的离散时间有限维系统。Kalman 滤波器的估计性能是：它使滤波后的状态估计误差的相关矩阵P(n)的迹最小化。这意味着，kalman 滤波器是状态向量x(n)的线性最小方差估计。 ◆实验结果： ◆程序代码： （1）主程序
高精度航姿参考系统AHRS 3DT-1000 3DT-1000是一款高性能的小型航姿参考系统，可用于动静态环境下对载体的横滚角、俯仰角和航向角进行高精度测量。高精度360 度全方位位置姿态输出，高效的数据融合算法快速动态响应与长时间稳定性（无漂移，无积累误差）相结合。 1、传感器建模 3DT-1000是一款高性能的小型航姿参考系统，可用于动静态环境下对载体的横滚角、俯仰角和航向角进行高精度测量。 基于三轴陀螺仪、三轴加速度计和三轴磁力计，3DT-1000采用自适应kalman数据融合算法，可实时输出载体的惯性运动信息（三轴角速度、三轴加速度）、最优姿态角（横滚角、俯仰角和航向角）等参数，并且，通过对传感器的安装误差、轴间正交误差和温度误差进行补偿，极大地提高了3DT-1000的测量精度。
2、传感器的数据融合算法 基于对陀螺仪、加速度计和磁力计的性能分析，3DT-1000以积分角速度的姿态角作为状态量、以重力加速度和地球磁场获得姿态角作为观测量、以载体运动状态信息建立增益调整因子，设计了基于四元数的自适应kalman滤波数据融合算法，获得载体在各个状态（静态和动态）下的最优姿态角，从而实现姿态和航向信息的快速动态和长时间稳定性（无漂移、无累计误差）相结合的效果。 为了获得高精度的载体姿态角，3DT-1000在进行传感器数据融合算法之前，对三轴陀螺仪、三轴加速度计和三轴磁力计进行了标定以及温度漂移等误差补偿。 默认情况下，3DT-1000以50Hz的输出频率，连续输出传感器信息，波特率默认为115200。提供的硬件接口为UART232。 3、软硬件设计
自主AHRS系统3DT-1000 荷兰VTi 4、传感器标定 1）俯仰角和横滚角的误差都会影响航向角精度，所以必须进行磁场倾斜补偿；必须提 高姿态角的精度，例如抗震性能； 俯仰角误差造成的航向角误差 2）载体和周围环境的磁场都会影响航向角精度，所以必须进行磁场软硬体补偿；在产品应用过程中，周围肯定或多或少存在磁场干扰，如铁块等硬体； 在应对这些干扰的情况，我们采用了磁场倾斜补偿和软硬体补偿： 提出全新磁场倾斜补偿模型，该模型考虑了俯仰角和横滚角对磁场影响，实现全方位的 磁场补偿；