攻读博士、硕士学位研究生试卷（作业）封面（2011 至2012 学年度第一学期）
题目卡尔曼滤波自由落体
科目现代信号处理
姓名郭倩
专业电子与系统
入学年月2011-9-1
摘要
集成电路发展趋势之一就是与其它学科结合诞生新的技术。现代信号处理系统正是融合
信号处理论、VLSI技术和计算机技术。集成电路和微电子技术的发展，为现代信号处理技术
的发展提供了十分优越的条件，采用数字信号处理的集成电路硬件系统正显示它的优越性，卡尔曼滤波器在数字信号处理系统中应用非常普遍。本文在研究卡尔曼滤波器的基本理论基础上，系统地讨论分析了卡尔曼滤波器在自由落体的运动中的应用，并做出仿真图直观的理解卡尔曼的应用之广泛。
关键词：卡尔曼滤波器自由落体协方差仿真
Abstract
One of the trends is the development of integrated circuits and other subjects with the birth of new technologies. Modern signal processing system is the integration of signals at the theory, VLSI technology and computer technology. The development of integrated circuits and microelectronic technology for the development of modern signal processing technology provides a very favorable conditions, the use of digital signal processing integrated circuit hardware system is showing its superiority, the Kalman filter in the application of digital signal processing system very common. Keywords:Kalman filter, free-fall, covariance, simulation
第一章绪论
1．1课题的研究背景
许多工程技术领域都涉及到如何在较强的背景噪声和干扰信号下提取真正的信号，并将其应用于实际工程这正是数字信号处理要研究解决的问题。数字信号处理广泛地应用于通信系统，航空航天系统．雷达系统、遥感遥测系统等。数字信号处理系统是由反馈构成的，而反馈包含着来源于系统的过程测量，通常在这些测量中，包含着随机不确定性，或者混有不希望有的干扰和噪声．为了使控制接近所需要的值，就要进行滤波。在现代信号处理和电子技术领域，滤波器作为一种必不可少的组成部分处在了一个十分重要的位置，并日益显示出其巨人的应用价值。尽管滤波技术的发展剑现在只有七十多年的历史，但它的发生与发展已经历了诸多变化，作为一种信号处理的技术已相当完善，并不断发挥着其重要的作埘。长期以来人们不断地探索滤波器的设计与实现方法，努力地追求着简化设计、减少体积、改善性能、提高灵活性和可靠性、便于制作等问题。
滤波是电子信息处理领域的一种最基本而又极其重要的技术。滤波它是用某一时刻及以前的数据来提取或估计该时刻感兴趣信息的一种运算过程。利用滤波技术可以从复杂的信号中提取所需要的信号．同时抑制噪声或干扰信号，以便更有效地利用原始信号。滤波器实际上是一种选频系统，它对某些频率的信号予以很小的衰减，让该部分信号顺利通过；而对其他不需要的频率信号则予以很大的衰减，尽可能阻这些信号通过。随着系统对宽带、高速、实时信号处理要求越来越高，对滤波器的处理速度、带宽等性能要求也随之提高。
1．2国内外研究动态
当前我们正处于数字化时代，数字信号处理技术受到了人们的广泛关注，其理论及算法随着计算机技术及微电子技术的发展得到了飞速的发展，在许多领域得到了广泛的应用．如语音、图像、雷达、通信等等。在数字信号处理中，滤波器扮演着不可替代的角色。
在数字信号处理中，滤波器可分为线性滤波器和非线性滤波器两种。若滤波器输出端滤波、平滑或预测的能是它的输入观测域的线性函数，则认为该滤波器是线性的，否则，认为该滤波器是1非线性的。在解线性滤波问题的统计方法中，通常假设己知有用信号及其附加噪声的某些统计参数(例如．均值和自相关函数)，而且需要设计含噪数据作为其输入的线性滤波器，使得根据某种统计准则噪声对滤波器的影响最小。实现该滤波器优化问题的一个有用方法是使误差信号(定义为期望响应与滤波器实际输出之差)的均值最小化。对下平衡输入，其解决方案通常称为维纳滤波器(Wiener filter)。该滤波器在均方误差意义上是最优的。误差信号均方值相对于线性滤波器可调参数的曲线通常称为误差性能曲面。该曲面的极小点即为维纳解。
维纳滤波器不适合于应对信号和，或噪声的非平]1[稳性．在这种情况下，必须假设最优滤波器为时变形式。对于这个更加困难的问题，十分成功的一个解决方案是采用乍尔曼滤波器：(Kalman filter)。该滤波器在各种工程应用中是一个强有力的系统。
卡尔曼滤波器的一个显著特点是状态概念来描述其数学公式，卡曼滤波器的另一个新颖特点是，它的解是递归计算的，而且可以不加修改地应用于平稳和非平稳环境。尤其是，其状态的每一次更新估计都由前一次估计和新的输入数据计算得到．因此只需存储前一次估计。
第二章卡尔曼滤波自由落体
通过课本的学习，我们对卡尔曼滤波理论的发展以及它的基本原理有了一定的了解，掌握了卡尔曼滤波器的主要思想即利用前一个估计值和最近一个观察数据来估计信号的当前值．是用状态方程和递推的方法进行估计的，是基于新观察值的信息来更新系统的状态估计以及协方差(相关方差)。下面举例说明。
2.1 卡尔曼滤波自由落体
已知一物体作自由落体运动，对其高度进行了20次测量，测量值如下表：
设高度的测量误差是均值为0、方差为1的高斯白噪声随机序列，该物体的初始高度0h 和速度0V 也是高斯分布的随机变量，且0000019001000,var 10/02Eh h m P EV m s V ????????===?
???????????
????。试求该物体]
2[高度和速度随时间变化的最优估计。（2
/80.9s m g =） 解： 令()()()h k X k v k ??
=?
???
t=1 R （k ）=1 Q(k)=0 根据离散时间卡尔曼滤波公式，则有： (1)(1,)()()X k k k X k U k φ+=++ (1)(1)(1)(1)Y k H k X k V k +=++++
(1,)k k φ+＝ 11t -??
???? ()U k ＝ 20.5gt gt ??-????
(1)H k +＝[]10 滤波初值：^
1900(0|0)(0)10X EX ??
==?
???
0100(0|0)var[(0)]2P X P ??
===?
??
?
一步预测：^
^
(1|)(1,)(|)()X k k k k X k k U k φ+=++ (1|)(1,)(|)(1,)T
P k k k k P k k k k φφ+=++
滤波增益：1
(1)(1|)(1)[(1)(1|)(1)(1)]T T K k P k k H k H k P k k H k R k -+=+++++++ 滤波计算：^
^
^
(1|1)(1|)(1)[(1)(1)(1|)]X k k X k k K k Y k H k X k k ++=++++-++ (1|1)[(1)(1)](1|)P k k I K k H k P k k ++=-+++
2.1.1实验结果
高度随时间变化估计
速度随时间变化的最优估计
高度协方差
速度协方差
从以上的结果，可以得到高度和速度的估计值，再通过所得到的高度]3[协方差和速度协方差，可见用卡尔曼滤波法，虽然刚开始的初始高度协方差很大为100，但通过2步之后减小到不超过1，逐渐接近于0，同样的速度协方差刚开始的时候也比较大，为2，但是通过5步之后迅速减小，到10步之后接近于0。
2.2 有关参数的影响（例如初始条件、噪声统计特性对滤波结果的影响等）；
2.2.1 初始条件改变时，改变初始高度值，和速度值 00230030/Eh m EV m s ????
=??????
??
由实验结果分析可得速度滤波值在开始几步接近初始值]
4[，协方差值基本不变。
2.2.2 当初始协方差值改变时，改为0001500var 010h P V ????
==?????
???
2.3 实验结果分析
高度和速度滤波值基本不变，速度协方差和高度协方差开始要接近速度协方差和高度协方差的初始值。但是经过几步之后，都趋于0。
第三章小结
卡尔曼滤波器是控制、信号处理与通信等领域最基本最重要的计算方法]5[和工具之一，并已成功地应用到航空、航天、工业过程及社会经济等不同领域。对于解决很大部分的滤波问题，它是最优，效率最高甚至是最有用的。本章主要通过分析最后得到的仿真波形，可以看出，观测值仿真设计的卡尔曼滤波器能够很好地与理论值相吻合．
参考文献
[1]UweMeyer-Baese著，刘凌，胡永生译．数字信号处理的FPGA实现．北京：清华大学出版社，2002．
[2]沈福民．自适应信号处理[M]．西安：西安电子科技大学出版社，2001．
[3]刘家祥．张骨龙．信号处理[M]．长沙：国防科技大学出版社。1989，128．210．
[4]宋万杰，罗丰,吴顺君．CPLD技术及其应用[M]．西安：西安电子科技大学山版社．1999．
[5]张贤达．现代信号处理(第二版)[M]．北京：清华大学出版社．2002．
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
UT 变换 核心思想：近似一种概率分布比近似任意一个非线性函数或非线性变换要容易。 假设n 维向量x 经过一个非线性变换得到y ，即()y g x =，x 的均值为?x ，协方差矩阵为xx P 。 步骤1：根据x 的均值?x 和协方差矩阵xx P ，采用一定的采样策略（此处采用对称采样）得到sigma 点集{}i χ。 0???1,2,...,i i i n i x x x i n χχχ+==+=-= 其中，i 表示矩阵的第i 列。 (0)(0)2() ()/() /()(1) 1/2(),1,2,...,21/2(), 1,2,...,2m c i m i c W n W n W n i n W n i n λλλλαβλλ=+=++-+=+==+= 注，这里sigma 点集{}i χ乘以对应的权重{}i m W ，可得sigma 点集的均 值为?x ，协方差为xx P 。 步骤2：对所采样的sigma 点集{}i χ中的每个sigma 点通过非线性变 换g(*),得到采样后的sigma 点集{}i y 。 ()i i y g χ= 步骤3：对变换后的sigma 点集{}i y 进行加权处理，得到输出变量y 的均值?y 和协方差yy P 。 2()02()0???()()n i m i i n i T yy c i i i y W y P W y y y y ====--∑∑
UKF 非线性系统模型为: ()((1))(1)()(())() x k f x k V k y k h x k W k =-+-=+ 1) 状态初始条件为 ?(0|0)((0|0))??(0|0)(((0|0)(0|0))((0|0)(0|0)))T xx x E x P E x x x x ==-- 2) Sigma 点采样 ??(1|1)[(1|1)(1|1)?(1|1)k k x k k x k k x k k χ--=----+-- 3) 时间更新 202020(|1)((1|1)) ?(|1)(|1) (|1)((|1)) ?(|1)(|1) ??(|1)(((|1)(|1))((|1)(|1)))(1)n i m i i n i m i i n i T xx c i i i k k f k k x k k W k k k k h k k y k k W k k P k k W k k x k k k k x k k Q k χχχμχμχχ===-=---=--=--=--=------+-∑∑∑ 4) 测量更新 20 20 1??(|1)((|1)(|1))((|1)(|1))??(|1)((|1)(|1))((|1)(|1))()(|1)*(|1)???(|)(|1)()(()(|1))(|)n i T xy c i i i n i T yy c i i i xy yy xx P k k W k k x k k k k y k k P k k W k k y k k k k y k k K k P k k P k k x k k x k k K k y k y k k P k k χμμμ==--=-------=------=--=-+--∑∑(|1)()(|1)()T xx yy P k k K k P k k K k =---
一个应用实例详解卡尔曼滤波及其算法实现 标签：算法filtermatlabalgorithm优化工作 2012-05-14 10:48 75511人阅读评论(25) 收藏举报分类： 数据结构及其算法（4） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。 我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。 好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。 假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。 由于我们用于估算k时刻的实际温度有两个温度值，分别是23 度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance（协方差）来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。 可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。 现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56 度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度
%该文件用于编写无迹卡尔曼滤波算法及其测试 %注解：主要子程序包括：轨迹发生器、系统方程 % 测量方程、UKF滤波器 %作者：Jiangfeng %日期：2012.4.16 %--------------------------------------- function UKFmain %------------------清屏---------------- close all;clear all; clc; tic; global Qf n; %定义全局变量 %------------------初始化-------------- stater0=[220; 1;55;-0.5]; %标准系统初值 state0=[200;1.3;50;-0.3]; %测量状态初值 %--------系统滤波初始化 p=[0.005 0 0 0;0 0.005 0 0; 0 0 0.005 0;0 0 0 0.005]; %状态误差协方差初值 n=4; T=3; Qf=[T^2/2 0;0 T;T^2/2 0;0 T]; %-------------------------------------- stater=stater0;state=state0; xc=state; staterout=[]; stateout=[];xcout=[]; errorout=[];tout=[]; t0=1; h=1; tf=1000; %仿真时间设置 %---------------滤波算法---------------- for t=t0:h:tf [state,stater,yc]=track(state,stater); %轨迹发生器：标准轨迹和输出 [xc,p]=UKFfiter(@systemfun,@measurefun,xc,yc,p); error=xc-stater; %滤波处理后的误差 staterout=[staterout,stater]; stateout=[stateout,state]; errorout=[errorout,error]; xcout=[xcout,xc]; tout=[tout,t]; end %---------------状态信息图像--------------- figure; plot(tout,xcout(1,:),'r',tout,staterout(1,:),'g',... tout,stateout(1,:),'black'); legend('滤波后','真实值','无滤波'); grid on; xlabel('时间 t（s）'); ylabel('系统状态A');
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
自适应卡尔曼滤波 卡尔曼滤波发散的原因 如果卡尔曼滤波是稳定的，随着滤波的推进，卡尔曼滤波估计的精度应该越来越高，滤波误差方差阵也应趋于稳定值或有界值。但在实际应用中，随着量测值数目的增加，由于估计误差的均值和估计误差协方差可能越来越大，使滤波逐渐失去准确估计的作用，这种现象称为卡尔曼滤波发散。 引起滤波器发散的主要原因有两点： （1）描述系统动力学特性的数学模型和噪声估计模型不准确，不能直接真实地反映物理过程，使得模型与获得的量测值不匹配而导致滤波发散。这种由于模型建立过于粗糙或失真所引起的发散称为滤波发散。 （2）由于卡尔曼滤波是递推过程，随着滤波步数的增加，舍入误差将逐渐积累。如果计算机字长不够长，这种积累误差很有可能使估计误差方差阵失去非负定性甚至失去对称性，使滤波增益矩阵逐渐失去合适的加权作用而导致发散。这种由于计算舍入误差所引起的发散称为计算发散。 针对上述卡尔曼滤波发散的原因，目前已经出现了几种有效抑制滤波发散的方法，常用的有衰减记忆滤波、限定记忆滤波、扩充状态滤波、有限下界滤波、平方根滤波、和自适应滤波等。这些方法本质上都是以牺牲滤波器的最优性为代价来抑制滤波发散，也就是说，多数都是次优滤波方法。 自适应滤波 在很多实际系统中，系统过程噪声方差矩阵Q和量测误差方差阵R事先是不知道的，有时甚至连状态转移矩阵 或量测矩阵H也不能确切建立。如果所建立的模型与实际模型不符可能回引起滤波发散。自适应滤波就是这样一种具有抑制滤波发散作用的滤波方法。在滤波过程中，自适应滤波一方面利用量测值修正预测值，同时也对未知的或不确切的系统模型参数和噪声统计参数进行估计修正。自适应滤波的方法很多，包括贝叶斯法、极大似然法、相关法与协方差匹配法，其中最基本也是最重要的是相关法，而相关法可分为输出相关法和新息相关法。
卡尔曼滤波算法总结-标准化文件发布号：（9556-EUATWK-MWUB-WUNN-INNUL-DDQTY-KII
2015.12.12 void 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)(1|1)() X k k AX k k Bu k -=--+（1）先验估计 (|1)(1|1)'P k k AP k k A Q -=--+（2）协方差矩阵的预测 ()(|1)'/(|1)')Kg k P k k H HP k k H R =--+（3）计算卡尔曼增益 (|)(|1)()(()(|1))X k k X k k Kg k Z k HX k k =-+--（4）进行修正 5个式子比较抽象，现在直接用实例来说： 一、卡尔曼滤波第一个式子 对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为d dt θω=?，角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移（而且还是变化的），静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。 由此我们得到了当前角度的预测值Angle Angle=Angle+(Gyro - Q_bias) * dt; 其中等号左边Angle 为此时的角度，等号右边Angle 为上一时刻的角度，Gyro 为陀螺仪测的角速度的值，dt 是两次滤波之间的时间间隔，我们的运行周期是4ms 或者6ms 。 同时 Q_bias 也是一个变化的量。 但是就预测来说认为现在的漂移跟上一时刻是相同的，即 Q_bias=Q_bias 将上面两个式子写成矩阵的形式 1_0 1_0 Angle dt Angle dt Q bias Q bia o s Gyr -= + 得到上式，这个式子对应于卡尔曼滤波的第一个式子 (|1)(1|1)() X k k AX k k Bu k -=--+ (|)(|1) P k k I Kg k H P k k =--（（））（5）更新协方差阵
第 46卷第 2期 2011年 4月 西南交通大学学报 J OURNAL OF SOUTHW EST JI A OTONG UN I VERSI T Y V o. l 46 N o . 2 A pr . 2011收稿日期 :2010 02 01 作者简介 :马磊 (1972-, 男 , 教授 , 博士 , 研究方向是网络化控制和多机器人系统 , E m ai:l m ale@i s w jt u . edu. cn , l . 文章编号 :0258 2724(2011 02 0287 07 DO I :10. 3969/.j i ssn . 0258 2724. 2011. 02. 019 多智能体系统中一致性卡尔曼滤波的研究进展 马磊 1 , 史习智 2 (1. 西南交通大学电气工程学院 , 四川成都 610031; 2. 上海交通大学机械系统与振动国家重点实验室 , 上海 200240 摘要 :从多智能体系统中一致性问题的基本概念、算法收敛性和性能分析出发 , 总结了基于一致性方法的分布式卡尔曼滤波的研究进展 . 从基于局部通讯的滤波器构造方法、信息加权和滤波器参数优化等方面对研究现状进行了评述 . 最后 , 讨论了信息损失、量化一致性和随机异步算法等前沿问题 , 以期促进相关研究 . 关键词 :一致性 ; 多智能体系统 ; 图拉普拉斯算子 ; 信息融合 ; 分布式卡尔曼滤波中图分类号 :TP242 文献标志码 :A
R ecent Developm ent on Consensus Based Kalman Fi ltering in M ulti agent System s MA Lei 1 , S H I X izhi 2 (1. Schoo l o f E lectr i ca lEng i neeri ng , Southwest Jiao tong U nivers it y , Chengdu 610031, Ch i na ; 2. Sta te K ey L aboratory o fM echanical Syste m s and V ibra tion , Shangha i Ji ao t ong U n i v ers it y , Shangha i 200240, Ch i na Abst ract :Recent deve l o pm ent of the distributed K al m an filtering usi n g the consensus m ethod w as addressed . The concep, t conver gence and perfor m ance ana l y sis of consensus prob le m s i n m ulti agent syste m s w ere i n tr oduced , and severa l aspects o f t h e consensus based K al m an filtering were discussed in deta ils , i n c l u d i n g filter constructi o n based on loca l co mmunicati o n , i n for m ation w eighti n g and para m eter opti m ization . F i n ally , so m e fronti e rs o f the research on the consensus m ethod , such as i n f o r m ation loss , quantized consensus and stochastic asynchronous a l g orithm s , w ere briefly d iscussed to pro m ote the related research . K ey w ords :consensus ; mu lti agent syste m; graph Laplacian; i n for m ation f u si o n ; distri b uted K al m an filtering
卡尔曼滤波 马尔可夫过程： 在随机理论中，把在某时刻的事件受在这之前事件的影响，其影响范围有限的随机过程，称为马尔可夫过程。一个事件受在它之前的事件的影响的深远程度，通常用在它之前的事件作为条件的概率来表达。受前一个事件的影响，简称为马尔可夫过程；受前两个事件的影响，称为二阶马尔可夫过程；受前三个事件的影响，称为三阶马尔可夫过程！ 卡尔曼滤波简介＋算法实现代码（转）： 最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Ｋолмогоров等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。 现设线性时变系统的离散状态防城和观测方程为： X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1) Y(k) = H(k)·X(k)+N(k) 其中 X(k)和Y(k)分别是k时刻的状态矢量和观测矢量 F(k,k-1)为状态转移矩阵 U(k)为k时刻动态噪声
T(k,k-1)为系统控制矩阵 H(k)为k时刻观测矩阵 N(k)为k时刻观测噪声 则卡尔曼滤波的算法流程为： 1 2预估计X(k)^= F(k,k-1)·X(k-1) 3计算预估计协方差矩阵C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)' Q(k) = U(k)×U(k)' 4计算卡尔曼增益矩阵 K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1) R(k) = N(k)×N(k)' 5更新估计 X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^] 6计算更新后估计协防差矩阵 C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)' 7X(k+1) = X(k)~ C(k+1) = C(k)~ 重复以上步骤 其c语言实现代码如下： #include "stdlib.h" #include "rinv.c" int lman(n,m,k,f,q,r,h,y,x,p,g) int n,m,k; double f[],q[],r[],h[],y[],x[],p[],g[]; { int i,j,kk,ii,l,jj,js; double *e,*a,*b; e=malloc(m*m*sizeof(double)); l=m;
卡尔曼滤波简介及其算法实现代码 卡尔曼滤波算法实现代码（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/4517875822.html,/~welch/media/pdf/Kalman1960.pdf。 简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2．卡尔曼滤波器的介绍 （Introduction to the Kalman Filter） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就
第十三章 卡尔曼滤波 在本章中，我们介绍一种被称为卡尔曼滤波的十分有用的工具。卡尔曼滤波的基本思想是将动态系统表示成为一种称为状态空间表示的特殊情形。卡尔曼滤波是对系统线性投影进行序列更新的算法。除了一般的优点以外，这种算法对计算确切的有限样本预测、计算Gauss ARMA 模型的确切似然函数、估计具有时变参数的自回归模型等，都提供了重要方法。 §13.1 动态系统的状态空间表示 我们已经介绍过一些随机过程的动态表示方法，下面我们在以前的假设基础上，继续分析动态系统的表示方法。 13.1.1 继续使用的假设 假设表示时刻观测到的n 维随机向量，一类非常丰富的描述动态性的模型可以利用一些可能无法观测的被称为状态向量(state vector)的r 维向量表示，因此表示动态性的状态空间表示(state-space representation)由下列方程系统给出： 状态方程(state model) (13.1) 量测方程(observation model) (13.2) 这里，和分别是阶数为，和的参数矩阵，是的外生或者前定变量。方程(13.1)被称为状态方程(state model)，方程(13.2)被称为量测方程(observation model)，维向量和维向量都是向量白噪声，满足： (13.3) (13.4) 这里和是和阶矩阵。假设扰动项和对于所有阶滞后都是不相关的，即对所有和，有： (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 的任何实现都不相关：
卡尔曼滤波研究综述 1 卡尔曼滤波简介 1.1卡尔曼滤波的由来 1960年卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文-《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法），在这篇文章里一种克服了维纳滤波缺点的新方法被提出来，这就是我们今天称之为卡尔曼滤波的方法。卡尔曼滤波应用广泛且功能强大，它可以估计信号的过去和当前状态甚至能估计将来的状态即使并不知道模型的确切性质。 其基本思想是以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值。算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 对于解决很大部分的问题，它是最优，效率最高甚至是最有用的。它的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 1.2标准卡尔曼滤波-离散线性卡尔曼滤波 为了描述方便我们作以下假设：物理系统的状态转换过程可以描述为一个离散时间的随机过程；系统状态受控制输入的影响；系统状态及观测过程都不可避免受噪声影响；对系统状态是非直接可观测的。在以上假设前提下，得到系统的状体方程和观测方程。
X ?? 1-1 式中:X k 为状态向量,L k 为观测向量,Φk,k-1为状态转移矩阵,U k-1为控制向量,一 般不考虑,Γk,k-1,B k 为系数矩阵,Ωk-1为系统动态噪声向量,Δk 为观测噪声向量,其 随机模型为 E(Ωk ) =0;E(Δk ) =0;cov(Ωk ,Ωj ) = D Ω(k )δkj , cov(Δk ,Δj ) = D k (k )δkj ;cov(Ωk ,Δj ) =0;E(X 0) =μx(0) var(X 0) = D(X 0);cov(X 0,Ωk ) =0;cov(X 0,Δk ) =0. 1-2 卡尔曼滤波递推公式为 X ∧(k/k) = X ∧(k/k-1)+J k (L k -B k X ∧(k/k-1)), D(k/k) = (E-J k B k )D x (k/k-1), J k = D x (k/k-1)BT k [B k D x (k/k-1)]B T k +D Δ(k)]-1, X ∧ (k/k-1) =Φk ,k-1X ∧ (k-1/k-1), D x (k/k-1) =Φk ,k-1D x (k-1/k-1)ΦT k ,k-1+Γk ,k-1D Δ(k-1)ΓT k ,k-1. 1-3 2 几种最新改进型的卡尔曼滤波算法。 2.1 近似二阶扩展卡尔曼滤波 标准的卡尔曼滤波只适用于线性系统，而工程实际问题涉及的又大多是非 线性系统，于是基于非线性系统线性化的扩展卡尔曼滤波(EKF)在上世纪70年代 被提出,目前已经成为非线性系统中广泛应用的估计方法。近似二阶扩展卡尔曼 滤 波方法(AS-EKF)基于线性最小方差递推滤波框架,应用均值变换的二阶近似从 而得到非线性系统的递推滤波滤波框架 该滤波基于线性最小方差递推框架，状态X 的最小方差估计为
目录 摘要..................................................................................................................I ABSTRACT.......................................................................................................... II 第1章绪论 (1) 1.1课题研究目的及意义 (1) 1.2移动机器人研究的发展 (1) 1.3移动机器人室内定位方法现状 (5) 1.3.1室内定位方法概述 (5) 1.3.2特征提取与匹配算法 (7) 1.3.3多传感器定位的信息融合算法 (9) 1.4本文研究内容 (11) 第2章多传感器移动机器人系统搭建 (12) 2.1弓 (12) 2.2硬件平台设计与搭建 (12) 2.2.1机械结构设计 (12) 2.2.2传感器选型 (15) 2.3多传感器系统软件开发 (17) 2.3.1运动控制模块 (18) 2.3.2基于里程计建立移动机器人运动学模型 (19) 2.3.3基于激光测距仪数据建立特征地图 (22) 2.3.4基于动态阈值的特征提取 (23) 2.3.5传感器数据的特征匹配 (28) 2.4基于Q t架构的上位机界面程序开发 (32) 2.5本章小结 (33) 第3章基于无迹卡尔曼冗余测量参数的室内定位算法 (35) 3.1引言 (35) 3.2 Kalman滤波的基本原理 (35) 3.3无迹Kalman滤波的基本原理 (36) - III -
为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。 另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。 好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。 假如我们要估算k时刻的是实际温度值。 首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。 然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。 由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance 来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg =0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23) =24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。 现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5 =2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。 就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！ 下面就要言归正传，讨论真正工程系统上的卡尔曼。
1. 卡尔曼全名Rudolf Emil Kalman ，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems 》（线性滤波与预测问题的新方法）。 基于状态空间描述对混有噪声的信号进行滤波的方法，简称卡尔曼滤波。这种方法是R.E.卡尔曼和R.S.布什于1960和1961年提出的。卡尔曼滤波是一种切实可行和便于应用的滤波方法，其计算过程通常需要在计算机上实现。实现卡尔曼滤波的装置或软件称为卡尔曼滤波器。 卡尔曼滤波器（Kalman Filter ）是在克服以往滤波方法局限性的基础上提出来的，是一个最优化自回归数据处理算法（optimal recursive data processing algorithm ）。它是针对系统的部分状态或是部分状态的线性组合，且量测值中有随机误差（常称为量测噪声）。将仅与部分状态有关的测量进行处理，得出从某种统计意义上讲误差最小的更多状态的估值，从而将混有噪声（干扰）的信号中噪声滤除、提取有用信号。 卡尔曼滤波是一种递推线性最小方差估计，以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。 现设线性时变系统的离散状态方程和观测方程为： ()()()()()X k+1F k X k G k u k ()w k =?++ ()()()()k+1H k+1X k+1k+1Y v =?+ 其中 ()k X 和()k Y 分别是k 时刻的状态矩阵和测量矩阵 ()k F 为状态转移矩阵 ()k G 为系统控制项矩阵 ()k u 为k 时刻对系统的控制量 ()k w 为k 时刻动态噪声，其协方差()Q k ()k H 为k 时刻观测矩阵 ()k v 为k 时刻测量噪声, 其协方差()R k 则卡尔曼滤波的算法流程为： 状态的一步预估计()()()()()??X k+1k F k X k k G k u k |=?|+ 一步预估计协方差矩阵 ()()()()()C k+1k F k C k k F k Q k '|=?|+' 计算卡尔曼增益矩阵
华北电力大学 毕业设计（论文）文献综述 所在院系电力工程系 专业班号电自0804 学生姓名崔海荣 指导教师签名黄家栋 审批人签字 毕业设计(论文)题目基于卡尔曼滤波原理的电网频率综合检测和预测方法的研究
基于卡尔曼滤波原理的电网频率综合检测和预测方法的研究 一、前言 “频率”概念源于针对周期性变化的事物的经典物理学定义，由于电力系统中许多物理变量具有（准）周期性特征，故这一概念得到广泛应用【1】。 电网频率是电力系统运行的主要指标之一，也是检测电力系统工作状态的重要依据，频率质量直接影响着电力系统安全、优质、稳定运行。因此，频率检测和预测在电网建设中起着至关重要的作用。 随着大容量、超高压、分布式电力网网络的形成以及现代电力电子设备的应用，基于传统概念的电力系统频率和测量技术在解决现代电网频率问题上遇到了诸多挑战。 目前，用于频率检测和预测的方法很多，主要有傅里叶变换法、卡尔曼滤波法、最小均方误差法、正交滤波器法、小波变换法、自适应陷波滤波器以及它们和一些算法相结合来解决电网频率检测和预测问题。 本文着重讲述卡尔曼滤波原理、分类以及它在电力系统频率检测中的应用历程进行系统性分析，并对今后的研究方向做出展望。 二、主题 1 常规卡尔曼滤波 常规卡尔曼滤波是卡尔曼等人为了克服维纳滤波的不足，于60年代初提出的一种递推算法。卡尔曼滤波不要求保留用过的观测数据，当测得新的数据后，可按照一套递推公式算出新的估计量，不必重新计算【2】。 下面对其进行简单介绍： 假设线性离散方程为 1k k k k x A x ω+=+（1） k k k k z H x ν=+ （2） 式子中：k x n R ∈为状态向量；m k z R ∈为测量向量；k ωp R ∈为系统噪声或过程噪 声向量；k νm R ∈为量测噪声向量；k A 为状态转移矩阵；k H 为量测转移转移矩阵。假设系统噪声和量测噪声是互不相关的高斯白噪声，方差阵为k Q 、k R ，定义/1k k x ∧ -=1(|)k k E x y - 其他递推，则卡尔曼滤波递推方程如下： 状态1步预测为 /1k k x ∧ -=k A 1k x ∧ -（3） 1步预测误差方差阵为 /1k k P -=1k A -1k P -1T k A -+1k Q -（4） 状态估计为 k x ∧=/1k k x ∧-+k K （k z -k H /1k k x ∧ -）（5）