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
??X(
1|1)(1|)(1)[(1)?(1)(1|)]k k X k k K k z k H k X k k ++=++++-++)
|1())1()1|1(()1|1(P k k P k H k k X I k k ++++-=++
通过EKF 算法线性化状态转移矩阵和观测矩阵后，剩下的滤波过程与普通的卡尔曼滤波无异，滤波过程简单且容易进行。正因EKF 简单易于实现的特性，使得该算法一直以来都应用广泛，但是它的局限性也是非常明显的。在这种滤波方法中非线性因子的存在对滤波稳定性和状态估计精度都用很大的影响，其滤波结果的好坏与量测噪声和状态噪声的统计特性也有很大的关系，对于高斯噪声，该算法有很好的适用性，但是对于非高斯噪声，该算法的滤波精度会受到很大的影响。在滤波过程中由于需要预先估计过程噪声协方差矩阵()Q k 和量测噪声协方差矩阵()R k ，如果这两个矩阵的值的估计出现较大的误差，将使得滤波结果出现很大的偏差，容易产生误差积累从而导致滤波结果发散。在用该算法滤波进的初始时刻要预先假设状态的初始值和初始协方差，如果这两个值的估计出现较大偏差，滤波结果也会出现发散的现象。总的来说只有当系统的动态模型和观测模型都接近线性时，利用扩展卡尔曼滤波算法跟踪目标会取得较好的效果，滤波结果较接近真实值。
B. 不敏卡尔曼滤波算法
不敏滤波也叫无损卡尔曼滤波，它的核心是不敏变
换，摒弃了对非线性函数进行线性化的传统做法，采用卡尔曼线性滤波框架，依据不敏变换计算非线性变换的随机变量的统计特性，对于一步预测方程，使用不敏变换来处理均值和协方差的非线性传递。不敏变化不需要像EKF 那样对非线性状态方程和量测方程线性化，它是对状态向量的概率密度函数进行近似化，用一系列确定样本来逼近状态的后验概率密度，表现为一些列选取好的采样点，不需要求导计算雅克比矩阵。这些采样点完全体现了高斯密度的真实均值和协方差。近似化后的概率密度函数仍然是高斯的。当这些点经过任何非线性系统的传递后，得到的后验均值和协方差都能够精确到二阶[3]。由于不需要对非线性系统进行线性化，可以很容易的应用于非线性系统的状态估。
对于不敏变换可以做如下阐述：
假设X 为一个x n 维随机向量，g ：y
x
n n R R →为一非线性函数并且()y g x =。X 的均值和协方差分别为
x P X 和。计算UT 变化的步骤如下：
①首先计算
()i i x W n 和相对应的权值采样点个ξδ12+
()(
)
()(
)
????
???=+-==++===+x
i x x n i x i x x i n i P n X n i P n X i X x , (1)
,...,1,0,0κξκξξ
()()()????
??
???=+=
==+-++=
=+=x x c
i m i x c x m n i n W W i n W i n W 2,...,1,]2[10,10,200
κβαλλ
λλ 式中，λ是一个尺度参数，可以为任何数值，只要
()0≠+λx n 。()()i x x P n λ+是()x x P n λ+均方根矩阵
的第i 行或第i 列，x n 为状态向量的维数，其中)1(2-=αλx n 。
②每个δ采样点通过非线性函数传播，得到
()x i i n i g y 2,...,2,1,0,
==ξ
③计算y 的估计均值和协方差
∑==x
n i i i y W y 20，∑='--=
x
n i i i
i
y y y y y
W P 20
))((
滤波模型如下：
由状态方程可以计算得到δ点的一步预测：
())]|(,[|1k k k f k k i i ξξ=+
状态预测估计和状态预测协方差：
∑=+=+x
n i i
i k k W k k X 20)|1()|1(?ξ )()|1()|1()|1(20
k Q k k X k k X W k k P i n i i i x
++'?+?=+∑=
式中)|1(?)|1()|1(k k X
k k k k X i i +-+=+?ξ 由量测方程可得δ点量测的预测值：
())]|1(,1[|1k k k h k k i i ++=+ξ?
量测的预测值和协方差为：
20?(1|)(1|)x
n i i
i Z k k W k k ?=+=+∑
20
(1|)(1|)(1)
x
n zz i i i i P W Z k k Z k k R k ='=?+?+++∑
式中)|1(?)|1()|1(k k Z
k k k k Z i i +-+=+?? 量测和状态向量的互协方差为：
∑='?+?=x
n i i i i xz Z k k X W P 20
)|1(
状态更新和协方差更新表示为：
??(1|1)(1|)(1)?[(1)(1|)]X
k k X k k K k Z k Z
k k ++=+++?+-+
(1|1)(1|)(1)(1)(1)
P k k P k k K k S k K k ++=+-+?
'++
1
)1(-=+zz xz P
P k K
不敏卡尔曼滤波不必计算雅克比矩阵，不必对非线性系统函数()f x 进行任何形式的逼近；在预测阶段只是标准的线性代数运算；对于系统函数来说可以不连续。不敏卡尔曼滤波算法的计算量一般扩展卡尔曼滤波算法，这是由于扩展卡尔曼滤波算法通过线性化处理来实现非线性滤波估计，而不敏卡尔曼滤波是利用样本来逼近状态的概率密度函数，计算量主要发生在选取δ点时的方根分解运算1-k P 。在计算速度上扩展卡尔曼滤波算法拥有明显优势，但他的性能随着非线性强度变大而明显下降。不敏卡尔曼滤波算法因不用线性化处理而很好的解决了这一问题。但是不敏卡尔曼滤波算法是用高斯分布来逼近系统状态的后验概率密度，如果系统状态的后验概率密度函数时非高斯的，那么滤波结果将产生极大的误差。
C. 粒子滤波算法
粒子滤波是一种非线性滤波算法，是一种基于Monte Carlo 仿真的最优回归贝叶斯滤波算法[4]。这种滤波方法将所关心的状态矢量表示为一组带有相关权值的随机样本，并基于这些样本和权值可以计算出状态估值。这种方法不受线性化误差或高斯噪声假定的限制，适用于任何环境下的任何状态转化或量测模型。粒子滤波算法的核心思想便是利用一系列随机样本的加权和表示后验概率密度，通过求和来近似积分操作。滤波模型表述如下：
假定k 时刻，一组随机样本{}0:1,s
N i
i k k i X q =是根据后验
概率密度()0:1:|k k p X Z 所获得的采样。其中0:i
k X 表示为0
到k 时刻的第i 个样本集合，即粒子集合；i k
q 为相关
权值，并且权值满足11s
N r
k r q ==∑，N s 为样本采样数，即粒
子数；1:k Z 代表传感器k 时刻的量测集合；
{}0:=,0,...,k j X X j k =表示为0到k 时刻的所有状态向量
集合。则在k 时刻，后验概率密度可近似表示为：
0:1:0:0:1
(|)()
s
N
i i
k k k k k i p X Z q X X δ==-∑ 由于很难直接从()0:1:|k k p X Z 抽取样本，通常可利用一个重要性概率密度(|)X Z π来获得样本值。从而，权值i
k q 可以按序贯重点抽样的方法获得。如果0:i
k X 是从(|)X Z π获得的样本，未归一化的权值i k q 可以定义为：
1:0:0:0:1:(|)()=(|)
i i
i k k k k i
k k p Z X P X q X Z π
如果所选择的重要性概率密度满足：
()
0:1:0:11:0:11:1(|)(|,)|i i i i
k k k k k k k X Z X X Z X Z πππ---=?
由以上两式可得：
11
0:11:(|)(|)=(|,)i i i i
i k k k k k k i i
k k k
p Z X P X X q q X X Z π---?
为了能够方便的采用回归贝叶斯滤波算法，我们希望重
要性概率密度只与前一时刻的测量和状态有关，即：
0:11:1(|,)(|,)i i i i
k k k k k k X X Z X X Z ππ--= 根据以上两式有：
11
1(|)(|)=(|,)
i i i i
i k k k k k k i i k k k p Z X P X X q q X X Z π---?
在粒子滤波算法中，经过几个迭代周期后，大多数的粒子权值会趋近于零，即粒子衰减现象。由于粒子权值的协方差随着时间的增长而不断变大，这种现象是无法避免的。为了减弱这种影响，一种最直接的方法就是使用大量的粒子数目。当然，这经常是不实际的，因此，目前采用的两种方法：（1）选择最优的重要性概率密度；（2）进行重抽样。
最优的重要性概率密度为：
11(|,)(|,)
i i i i k k k k k k X X Z p X X Z π--=
最优重要性概率密度可以使采样点权值的协方差最小。目前有两种情况经常采用最优重要性概率密度。第一种情况是X k 为有限集合，例如用于跟踪机动目标的跳变马尔科夫线性系统；第二种情况是状态方程为非线性的，量测方程为线性的系统。
重抽样的基本思想是削减小权值的粒子，并集中较大权值的粒子。目前经常采用的集中重抽样方法有：分层抽样和残差抽样、系统重抽样。为了得到正确的状态估计，通常希望粒子权值的方差尽可能趋近于零。然
而，蒙特卡洛模拟方法一般都存在权值退化问题。在实际计算中，经过数次迭代，只有少数粒子的权值较大，其余粒子的权值可忽略不计。粒子权值的方差随着时间增大，状态空间中的有效粒子数较少。对于大多数系统来说最优重要性概率密度往往是无法实现的因此经常利用线性化的技术对最优重要性概率密度进行次优近似。 粒子滤波算本质上是一种状态搜索算法[5]。我们估计得到一系列状态，然后将这些状态与我们的观测进行比较，依据似然函数，我们判断哪一些状态是最有可能生成当前观测的状态，然后保留他们，并且给予他们较大的权重，其他的状态责备削弱或者抛弃。但是如果似然函数有若干个窄的峰，那么粒子将有可能聚集到其中的某些峰处，对其他的峰视而不见，也就是说粒子滤波不能保证对任意复杂形状的概率分布都实现准确的描述和跟踪，尤其当空间维数较高时，这一缺陷会更加突出，这归根结底是重采样破坏了粒子的多样性。一个好的重采样算法应该在增加粒子多样性和减少权值较小的粒子数目之间进行有效折衷。
III. 仿真分析
在本文中我们分别利用扩展卡尔曼滤波算法，不敏卡尔曼滤波算法，粒子滤波算法对一个非线性例子进行仿真分析。系统的状态方程和量测方程都是非线性的。模型如下：
()()()2
1111
2
0.525/18cos 1.21/20k k k k k k k k
X X X X k V Z X W ----?=+++-+??=+?? 实验参数设定[6]：系统噪声(10,1)k V N ，量测噪
声(1,1)k W N ，初始状态01X =，初始方差05P =，时间序列长度T=50,。不敏卡尔曼滤波中的参数为：1=0αβ=，，=2κ。粒子滤波中的粒子数N=300.本文对各种滤波方法进行了M=50次的Monte Carlo 仿真，三种算法的滤波结果如图1所示。
图1. 三种算法的状态估计曲线
图1给出了三种非线性滤波算法的经过50次Monte Carlo 仿真后的状态估计。从图中可以看出扩展卡尔曼滤波算法的滤波效果很差，滤波结果大部分时间都偏离真实状态较大。不敏卡尔曼滤波效果相对来说较好，大部分时间滤波结果和真实状态之间的偏差不大。粒子滤波的效果最好，滤波结果和真实状态之间的差距很小，二者具有很好的一致性。
三种滤波算法的均方根误差图像如图2所示。
图2. 三种算法的均方根误差曲线
由图二可以看出扩展卡尔曼滤波算法的均方根误差
最大，而且误差数值相对真实值偏离较大，这主要是由于线性化的过程中忽略了高阶项，引入了线性化误差。不敏卡尔曼滤波算法的均方根误差相对较小，但是粒子滤波的均方根误差最小，跟随真实值的效果最好。因此可以看出在非线性条件下，粒子滤波的效果好于不敏卡尔曼滤波好于扩展卡尔曼滤波。
表1 各种滤波算法的实验结果比较 滤波 算法 运算复 杂度 运行时间(s) (归一化) 存储量 非线性
强度 EKF O(n 3) 0.142 低 弱非线性 UKF O(n 4) 0.220 中 无限制 PF O(Nn 3)
0.855 高 无限制
表1给出了各种滤波算法的运算复杂度，运算时间，运算所需存储量和滤波对非线性条件的要求。从运算速度上看，扩展卡尔曼滤波算法具有绝对优势，这是由于他将系统方程和量测方程线性化以后的结果。但是根据图1和图2可以看出在非线性较强的条件下这种滤波算法的效果很差，滤波精度较低，甚至出现发散。从滤波精度上看不敏卡尔曼滤波算法和粒子滤波算法差距不大，但是粒子滤波算法的计算量要比不敏卡尔曼滤波算法的大的多，而且算法复杂度是随之粒子数目的增长而线性增加的。对于这两种算法，在一般的高斯白噪声
环境下宜使用不敏卡尔曼滤波算法，但是在更复杂的非高斯情况下粒子滤波具有更好的适用性。
IV.结论
本文详细的介绍了三种非线性滤波算法：扩展卡尔曼滤波算法，不敏卡尔曼滤波算法，粒子滤波算法。在给出三种滤波算法的详细的理论基础上对每一种算法的优缺点都进行了一定的分析和总结。在文章的最后通过一个仿真实验分析比较了这三种算法在具体滤波过程中的性能。扩展卡尔曼滤波算法在线性化状态方程和量测方程的过程中引入了线性化误差，滤波的精度出现严重下降，滤波结果甚至出现发散；但是线性化后的模型让它的计算量大大地减小了。不敏卡尔曼滤波算法和粒子滤波算法在非线性问题的目标跟踪中表现的都很好，不敏卡尔曼滤波算法的计算量相对来说是比较小的，不过这种算法只适用于高斯白噪声的环境中，如果系统状态的后验概率密度函数时非高斯的，那么滤波结果将产生极大的误差。粒子滤波最复杂的环境有极高的适应性，而且也不要求噪声服从高斯分布，但是该算法的计算量较大，计算量是随着粒子数目线性增加的，而粒子数目往往较大，这就对计算机数据处理性能提出了较高的要求。
致谢
在本论文的撰写过程中袁子寅师兄给予了很多的建设性意见，宋河晏同学在编程方面提供了一定的帮助，特此表示感谢。
参考文献
[1]秦勤. 雷达目标跟踪的卡尔曼滤波方法的研究[D].
大连海事大学,2006.
[2]殷俊丽. 天波超视距雷达机动目标检测与跟踪[D].
西安电子科技大学,2009.
[3]夏建涛,任震,陈立,景占荣.极坐标下卡尔曼滤波算
法的研究[J].西北工业大学学报,2000(03):396-399.
[4]龚亚信. 基于粒子滤波的弱目标检测前跟踪算法研
究[D].国防科学技术大学,2009.
[5]吴兆平. 雷达微弱目标检测和跟踪方法研究[D].西
安电子科技大学,2012.
[6]刘丽丽.几种非线性滤波算法的性能分析[J].价值
工程,2010,29(34):190-191
%%% EKF UKF PF三种算法对比
clc
close all
clear;
% tic;
x = 0.1; % 初始状态
x_estimate = 1;%状态的估计
e_x_estimate = x_estimate; %EKF的初始估计
u_x_estimate = x_estimate; %UKF的初始估计
p_x_estimate = x_estimate; %PF 的初始估计
Q = 10; % 过程状态协方差
R = 1; % 测量噪声协方差
P =5;%初始估计方差
e_P = P; %UKF方差
u_P = P;%UKF方差
pf_P = P;%PF方差
tf = 50; % 模拟长度
x_array = [x];%真实值数组
e_x_estimate_array = [e_x_estimate];%EKF 最优估计值数组
u_x_estimate_array = [u_x_estimate];%UKF 最优估计值数组
p_x_estimate_array = [p_x_estimate];%PF 最优估计值数组
u_k = 1; %微调参数
u_symmetry_number = 4; % 对称的点的个数
u_total_number = 2 * u_symmetry_number + 1; % 总的采样点的个数linear = 0.5;
N = 500; %粒子滤波的粒子数
close all;
%粒子滤波初始N 个粒子
for i = 1 : N
p_xpart(i) = p_x_estimate + sqrt(pf_P) * randn;
end
for k = 1 : tf
% 模拟系统
x = linear * x + (25 * x / (1 + x^2)) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; % 状态值
y = (x^2 / 20) + sqrt(R) * randn; % 观测值
%扩展卡尔曼滤波器
%进行估计第一阶段的估计
e_x_estimate_1 = linear * e_x_estimate + 25 * e_x_estimate /(1+e_x_estimate^2) + 8 * cos(1.2*(k-1));
e_y_estimate = (e_x_estimate_1)^2/20; % 这是根据k=1 时估计值为1 得到的观测值；
%相关矩阵
e_A = linear + 25 * (1-e_x_estimate^2)/((1+e_x_estimate^2)^2);% 传递矩阵
e_H = e_x_estimate_1/10; % 观测矩阵
%估计的误差
e_p_estimate = e_A * e_P * e_A' + Q;
%扩展卡尔曼增益
e_K = e_p_estimate * e_H'/(e_H * e_p_estimate * e_H' + R);
%进行估计值的更新第二阶段
e_x_estimate_2 = e_x_estimate_1 + e_K * (y - e_y_estimate);
%更新后的估计值的误差
e_p_estimate_update = e_p_estimate - e_K * e_H * e_p_estimate;
%进入下一次迭代的参数变化
e_P = e_p_estimate_update;
e_x_estimate = e_x_estimate_2;
% 粒子滤波器
% 粒子滤波器
for i = 1 : N
p_xpartminus(i) = 0.5 * p_xpart(i) + 25 * p_xpart(i) / (1 + p_xpart(i)^2) + 8 * cos(1.2*(k-1)) +sqrt(Q) * randn; p_ypart = p_xpartminus(i)^2 / 20; % 预测值
p_vhat = y - p_ypart;% 观测和预测的差
p_q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-p_vhat^2 / 2 / R); % 各个粒子的权值
end
% 平均每一个估计的可能性
p_qsum = sum(p_q);
for i = 1 : N
p_q(i) = p_q(i) / p_qsum;% 各个粒子进行权值归一化
end
% 重采样权重大的粒子多采点，权重小的粒子少采点, 相当于每一次都进行重采样；
for i = 1 : N
p_u = rand;
p_qtempsum = 0;
for j = 1 : N
p_qtempsum = p_qtempsum + p_q(j);
if p_qtempsum >= p_u
p_xpart(i) = p_xpartminus(j); % 在这里xpart(i) 实现循环赋值；
break;
end
end
end
p_x_estimate = mean(p_xpart);
u_x_par = u_x_estimate;
for i = 2 : (u_symmetry_number+1)
u_x_par(i,:) = u_x_estimate + sqrt((u_symmetry_number+u_k) * u_P);
end
for i = (u_symmetry_number+2) : u_total_number
u_x_par(i,:) = u_x_estimate - sqrt((u_symmetry_number+u_k) * u_P);
end
%计算权值
u_w_1 = u_k/(u_symmetry_number+u_k);
u_w_N1 = 1/(2 * (u_symmetry_number+u_k));
%把这些粒子通过传递方程得到下一个状态
for i = 1: u_total_number
u_x_par(i) = 0.5 * u_x_par(i) + 25 * u_x_par(i)/(1+u_x_par(i)^2) + 8 * cos(1.2*(k-1)); end
%传递后的均值和方差
u_x_next = u_w_1 * u_x_par(1);
for i = 2 : u_total_number
u_x_next = u_x_next + u_w_N1 * u_x_par(i);
end
u_p_next = Q + u_w_1 * (u_x_par(1)-u_x_next) * (u_x_par(1)-u_x_next)';
for i = 2 : u_total_number
u_p_next = u_p_next + u_w_N1 * (u_x_par(i)-u_x_next) * (u_x_par(i)-u_x_next)'; end
for i = 1 :u_total_number
u_y_2obser(i,:) = u_x_par(i);
end
%通过观测方程得到一系列的粒子
for i = 1: u_total_number
u_y_2obser(i) = u_y_2obser(i)^2/20;
end
%通过观测方程后的均值y_obse
u_y_obse = u_w_1 * u_y_2obser(1);
for i = 2 : u_total_number
u_y_obse = u_y_obse + u_w_N1 * u_y_2obser(i);
end
%Pzz测量方差矩阵
u_pzz = R + u_w_1 * (u_y_2obser(1)-u_y_obse)*(u_y_2obser(1)-u_y_obse)';
for i = 2 : u_total_number
u_pzz = u_pzz + u_w_N1 * (u_y_2obser(i) - u_y_obse)*(u_y_2obser(i) - u_y_obse)'; end
%Pxz状态向量与测量值的协方差矩阵
u_pxz = u_w_1 * (u_x_par(1) - u_x_next)* (u_y_2obser(1)-u_y_obse)';
for i = 2 : u_total_number
u_pxz = u_pxz + u_w_N1 * (u_x_par(i) - u_x_next) * (u_y_2obser(i)- u_y_obse)'; end
%卡尔曼增益
u_K = u_pxz/u_pzz;
%估计量的更新
u_x_next_optimal = u_x_next + u_K * (y - u_y_obse);% 第一步的估计值+ 修正值；u_x_estimate = u_x_next_optimal;
%方差的更新
u_p_next_update = u_p_next - u_K * u_pzz * u_K';
u_P = u_p_next_update;
%进行画图程序
x_array = [x_array,x];
e_x_estimate_array = [e_x_estimate_array,e_x_estimate];
p_x_estimate_array = [p_x_estimate_array,p_x_estimate];
u_x_estimate_array = [u_x_estimate_array,u_x_estimate];
e_error(k,:) = abs(x_array(k)-e_x_estimate_array(k));
p_error(k,:) = abs(x_array(k)-p_x_estimate_array(k));
u_error(k,:) = abs(x_array(k)-u_x_estimate_array(k));
end
t = 0 : tf;
figure;
plot(t,x_array,'k^-',t,e_x_estimate_array,'r-',t,p_x_estimate_array,'b--',t,u_x_estimate_array,'g-.'); grid on;
set(gca,'FontSize',10);
set(gcf,'color','White');
xlabel(' 时间步长');% lable
ylabel(' 滤波值');
legend(' 真实值','EKF 估计值','PF 估计值','UKF 估计值');
%root mean square 平均值的平方根
e_xrms = sqrt((norm(x_array-e_x_estimate_array)^2)/tf);
disp(['EKF 估计误差均方值=',num2str(e_xrms)]);
p_xrms = sqrt((norm(x_array-p_x_estimate_array)^2)/tf);
disp(['PF 估计误差均方值=',num2str(p_xrms)]);
u_xrms = sqrt((norm(x_array-u_x_estimate_array)^2)/tf);
disp(['UKF 估计误差均方值=',num2str(u_xrms)]);
% plot(t,e_error,'r-',t,p_error,'g--',t,u_error,'b:');
% legend('EKF 估计值误差','PF 估计值误差','UKF 估计值误差');
t = 1 : tf;
figure;
plot(t,e_error,'r-',t,p_error,'b--',t,u_error,'g-.');grid on;
set(gca,'FontSize',10);
set(gcf,'color','White');
xlabel(' 时间步长');
ylabel(' 误差');
legend('EKF 估计值误差','PF 估计值误差','UKF 估计值误差');
% toc;
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
常用的8种数字滤波算法 摘要：分析了采用数字滤波消除随机干扰的优点，详细论述了微机控制系统中常用的8种数字滤波算法，并讨论了各种数字滤波算法的适用范围。 关键词：数字滤波；控制系统；随机干扰；数字滤波算法 1 引言 在微机控制系统的模拟输入信号中，一般均含有各种噪声和干扰，他们来自被测信号源本身、传感器、外界干扰等。为了进行准确测量和控制，必须消除被测信号中的噪声和干扰。噪声有2大类：一类为周期性的，其典型代表为50 Hz 的工频干扰，对于这类信号，采用积分时间等于20 ms整倍数的双积分A/D转换器，可有效地消除其影响；另一类为非周期的不规则随机信号，对于随机干扰，可以用数字滤波方法予以削弱或滤除。所谓数字滤波，就是通过一定的计算或判断程序减少干扰信号在有用信号中的比重，因此他实际上是一个程序滤波。 数字滤波器克服了模拟滤波器的许多不足，他与模拟滤波器相比有以下优点： (1)数字滤波器是用软件实现的，不需要增加硬设备，因而可靠性高、稳定性好，不存在阻抗匹配问题。 (2)模拟滤波器通常是各通道专用，而数字滤波器则可多通道共享，从而降低了成本。 (3)数字滤波器可以对频率很低(如0.01 Hz)的信号进行滤波，而模拟滤波器由于受电容容量的限制，频率不可能太低。 (4)数字滤波器可以根据信号的不同，采用不同的滤波方法或滤波参数，具有灵活、方便、功能强的特点。 2 常用数字滤波算法 数字滤波器是将一组输入数字序列进行一定的运算而转换成另一组输出数字序列的装置。设数字滤波器的输入为X(n)，输出为Y(n)，则输入序列和输出序列之间的关系可用差分方程式表示为： 其中：输入信号X(n)可以是模拟信号经采样和A/D变换后得到的数字序列，也
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
1、限幅滤波法(又称程序判断滤波法) A、方法: 根据经验判断,确定两次采样允许的最大偏差值(设为A) 每次检测到新值时判断: 如果本次值与上次值之差<=A,则本次值有效 如果本次值与上次值之差>A,则本次值无效,放弃本次值,用上次值代替本次值B、优点: 能有效克服因偶然因素引起的脉冲干扰 C、缺点 无法抑制那种周期性的干扰 平滑度差 2、中位值滤波法 A、方法: 连续采样N次(N取奇数) 把N次采样值按大小排列 取中间值为本次有效值 B、优点: 能有效克服因偶然因素引起的波动干扰 对温度、液位的变化缓慢的被测参数有良好的滤波效果 C、缺点: 对流量、速度等快速变化的参数不宜 3、算术平均滤波法 A、方法: 连续取N个采样值进行算术平均运算 N值较大时:信号平滑度较高,但灵敏度较低 N值较小时:信号平滑度较低,但灵敏度较高 N值的选取:一般流量,N=12;压力:N=4 B、优点: 适用于对一般具有随机干扰的信号进行滤波 这样信号的特点是有一个平均值,信号在某一数值范围附近上下波动 C、缺点: 对于测量速度较慢或要求数据计算速度较快的实时控制不适用 比较浪费RAM 4、递推平均滤波法(又称滑动平均滤波法) A、方法: 把连续取N个采样值看成一个队列 队列的长度固定为N 每次采样到一个新数据放入队尾,并扔掉原来队首的一次数据.(先进先出原则) 把队列中的N个数据进行算术平均运算,就可获得新的滤波结果 N值的选取:流量,N=12;压力:N=4;液面,N=4~12;温度,N=1~4 B、优点:
对周期性干扰有良好的抑制作用,平滑度高 适用于高频振荡的系统 C、缺点: 灵敏度低 对偶然出现的脉冲性干扰的抑制作用较差 不易消除由于脉冲干扰所引起的采样值偏差 不适用于脉冲干扰比较严重的场合 比较浪费RAM 5、中位值平均滤波法(又称防脉冲干扰平均滤波法) A、方法: 相当于“中位值滤波法”+“算术平均滤波法” 连续采样N个数据,去掉一个最大值和一个最小值 然后计算N-2个数据的算术平均值 N值的选取:3~14 B、优点: 融合了两种滤波法的优点 对于偶然出现的脉冲性干扰,可消除由于脉冲干扰所引起的采样值偏差C、缺点: 测量速度较慢,和算术平均滤波法一样 比较浪费RAM 6、限幅平均滤波法 A、方法: 相当于“限幅滤波法”+“递推平均滤波法” 每次采样到的新数据先进行限幅处理, 再送入队列进行递推平均滤波处理 B、优点: 融合了两种滤波法的优点 对于偶然出现的脉冲性干扰,可消除由于脉冲干扰所引起的采样值偏差C、缺点: 比较浪费RAM 7、一阶滞后滤波法 A、方法: 取a=0~1 本次滤波结果=(1-a)*本次采样值+a*上次滤波结果 B、优点: 对周期性干扰具有良好的抑制作用 适用于波动频率较高的场合 C、缺点: 相位滞后,灵敏度低 滞后程度取决于a值大小
自适应卡尔曼滤波 卡尔曼滤波发散的原因 如果卡尔曼滤波是稳定的，随着滤波的推进，卡尔曼滤波估计的精度应该越来越高，滤波误差方差阵也应趋于稳定值或有界值。但在实际应用中，随着量测值数目的增加，由于估计误差的均值和估计误差协方差可能越来越大，使滤波逐渐失去准确估计的作用，这种现象称为卡尔曼滤波发散。 引起滤波器发散的主要原因有两点： （1）描述系统动力学特性的数学模型和噪声估计模型不准确，不能直接真实地反映物理过程，使得模型与获得的量测值不匹配而导致滤波发散。这种由于模型建立过于粗糙或失真所引起的发散称为滤波发散。 （2）由于卡尔曼滤波是递推过程，随着滤波步数的增加，舍入误差将逐渐积累。如果计算机字长不够长，这种积累误差很有可能使估计误差方差阵失去非负定性甚至失去对称性，使滤波增益矩阵逐渐失去合适的加权作用而导致发散。这种由于计算舍入误差所引起的发散称为计算发散。 针对上述卡尔曼滤波发散的原因，目前已经出现了几种有效抑制滤波发散的方法，常用的有衰减记忆滤波、限定记忆滤波、扩充状态滤波、有限下界滤波、平方根滤波、和自适应滤波等。这些方法本质上都是以牺牲滤波器的最优性为代价来抑制滤波发散，也就是说，多数都是次优滤波方法。 自适应滤波 在很多实际系统中，系统过程噪声方差矩阵Q和量测误差方差阵R事先是不知道的，有时甚至连状态转移矩阵 或量测矩阵H也不能确切建立。如果所建立的模型与实际模型不符可能回引起滤波发散。自适应滤波就是这样一种具有抑制滤波发散作用的滤波方法。在滤波过程中，自适应滤波一方面利用量测值修正预测值，同时也对未知的或不确切的系统模型参数和噪声统计参数进行估计修正。自适应滤波的方法很多，包括贝叶斯法、极大似然法、相关法与协方差匹配法，其中最基本也是最重要的是相关法，而相关法可分为输出相关法和新息相关法。
SINS/CNS组合导航技术 众所周知，SINS和CNS具有很强的互补性。将CNS与SINS组合，构成SINS／CNS自主组合导航系统，既能有效弥补SINS误差随时间积累的缺陷，又能弥补CNS平台结构设计难度大、结构复杂、成本高的缺陷。显然，SINS/CNS 自主组合系统兼备了SINS、CNS两者的优点，相互取长补短，不但抗干扰能力强、而且自主性能好，定位精度高，非常适合飞机对导航系统性能的要求。SINS/CNS组合导航的技术难点 1. 需要设计一套具有实时性和可行性的SINS/CNS自主组合导航系统方案，具体化各子传感器技术指标，使得各子传感器指标可考核；各传感器信息既互相兼容、互补和辅助，又能有效地进行信息交换。 2. 在某些特定情况下，系统的线性化数学模型的确能够反映出实际系统或过程的实际性能和特点。但是，任何实际系统总是存在不同程度的非线性，其中有些系统可以近似看成线性系统，而大多系统则不能仅用线性数学模型来描述，存在于这些系统中的非线性因素不能忽略。 3.SINS/CNS组合导航系统利用CNS输出的位置信息对SINS进行修正，能够克服SINS导航误差随时间积累的缺点，提高导航系统的定位精度。然而，由于CNS导航系统星图匹配及定位时需要耗用的不等的匹配计算时间，导航数据输出存在时延现象，导致其输出的位置及航向信息具有滞后效应，这将严重影响组合导航的解算精度。 本项目为了贴近实际工程系统，建立的自主组合导航系统模型为非线性数学模型。显然，卡尔曼滤波不能满足项目需求，必须建立与之相适应的非线性滤波系统。 扩展卡尔曼滤波(Extended KalmanFilter，EKF)在组合导航系统非线性滤波中得到了广泛应用，但它仍然具有理论局限性，具体表现在：(1)当系统非线性度较严重时，忽略Taylor展开式的高阶项将引起线性化误差增大，导致EKF的滤波误差增大甚至发散；(2)雅可比矩阵的求取复杂、计算量大，在实际应用中很难实施，有时甚至很难得到非线性函数的雅可比矩阵；(3)EKF将状态方程中的模型误差作为过程噪声来处理，且假设为高斯白噪声，这与组合导航系统的实际噪声情况并不相符；同时，EKF是以KF为基础推导得到的，其对系统初始状态的统计特性要求严格。因此EKF关于系统模型不确定性的鲁棒性很差。 模型预测滤波器(Models Predictive Filter，MPF)是基于最小模型误差(Minimum Model Error，MME)准则对系统状态进行估计，模型误差在估计过程中被确定并用于修正系统的动态模型。这种滤波器能够有效地解决存在显著动态模型误差情况下的非线性系统状态估计问题。EKF将模型误差作为过程白噪声
毕业设计(论文) UNDERGRADUATE PROJECT (THESIS) 题目: 冲击测试常用滤波算法研究 学院 专业 学号 学生姓名 指导教师 起讫日期
目录 摘要 (2) ABSTRACT (3) 第一章绪论 (4) 1.1课题背景 (4) 1.2国内外相关领域的研究 (4) 1.3主要研究内容与创新 (5) 1.3.1研究内容与意义 (5) 1.3.2课题的创新点 (5) 1.3.3 研究目的与技术指标 (6) 第二章数字滤波基础 (7) 2.1数字滤波算法概念 (7) 2.2数据采样与频谱分析原理 (8) 2.2.1 时域抽样定理 (8) 2.2.2 离散傅立叶变换(DFT) (8) 2.2.3 快速傅立叶变换(FFT) (9) 2.2.4 频谱分析原理 (9) 2.3常用数字滤波算法基础 (10) 2.3.1常用数字滤波算法分类 (10) 2.3.2常用数字滤波算法特点 (11) 2.3.3常用滤波算法相关原理 (13) 2.4 冲击测试采样数据 (16) 2.4.1噪声的特点与分类 (16) 2.4.2冲击测试采样数据特点 (17) 2.5 MATLAB简介 (17) 2.5.1 MATLAB功能简介 (18) 2.5.2 MATLAB的发展 (18) 第三章、冲击测试滤波算法设计及滤波效果分析 (20) 3.1 冲击测试采样数据的分析 (20) 3.2 滤波算法设计及效果分析 (21) 3.2.1 中位值平均法的设计 (21) 3.2.2限幅法和限速法的设计 (23) 3.2.3一阶滞后法的设计 (25) 3.2.4低通法的设计 (26) 第四章结论与展望 (34) 4.1冲击测试的滤波算法总结 (34) 4.2冲击测试的滤波算法展望 (34) 致谢 (36) 参考文献 (37) 附录：程序代码清单 (38)
软件滤波算法（转载） 这几天做一个流量检测的东西，其中用到了对数据的处理部分，试了很多种方法，从网上找到这些个滤波算法，贴出来记下 需要注意的是如果用到求平均值的话，注意总和变量是否有溢出，程序没必要照搬，主要学习这些方法，相信做东西的时候都能用得上 1、限幅滤波法（又称程序判断滤波法） A、方法： 根据经验判断，确定两次采样允许的最大偏差值（设为A） 每次检测到新值时判断： 如果本次值与上次值之差<=A,则本次值有效 如果本次值与上次值之差>A,则本次值无效,放弃本次值,用上次值代替本次值 B、优点： 能有效克服因偶然因素引起的脉冲干扰 C、缺点 无法抑制那种周期性的干扰 平滑度差 2、中位值滤波法 A、方法： 连续采样N次（N取奇数） 把N次采样值按大小排列 取中间值为本次有效值 B、优点： 能有效克服因偶然因素引起的波动干扰 对温度、液位的变化缓慢的被测参数有良好的滤波效果 C、缺点： 对流量、速度等快速变化的参数不宜 3、算术平均滤波法 A、方法： 连续取N个采样值进行算术平均运算 N值较大时：信号平滑度较高，但灵敏度较低 N值较小时：信号平滑度较低，但灵敏度较高 N值的选取：一般流量，N=12；压力：N=4 B、优点：
适用于对一般具有随机干扰的信号进行滤波 这样信号的特点是有一个平均值，信号在某一数值范围附近上下波动 C、缺点： 对于测量速度较慢或要求数据计算速度较快的实时控制不适用 比较浪费RAM 4、递推平均滤波法（又称滑动平均滤波法） A、方法： 把连续取N个采样值看成一个队列 队列的长度固定为N 每次采样到一个新数据放入队尾,并扔掉原来队首的一次数据.(先进先出原则) 把队列中的N个数据进行算术平均运算,就可获得新的滤波结果 N值的选取：流量，N=12；压力：N=4；液面，N=4~12；温度，N=1~4 B、优点： 对周期性干扰有良好的抑制作用，平滑度高 适用于高频振荡的系统 C、缺点： 灵敏度低 对偶然出现的脉冲性干扰的抑制作用较差 不易消除由于脉冲干扰所引起的采样值偏差 不适用于脉冲干扰比较严重的场合 比较浪费RAM 5、中位值平均滤波法（又称防脉冲干扰平均滤波法） A、方法： 相当于“中位值滤波法”+“算术平均滤波法” 连续采样N个数据，去掉一个最大值和一个最小值 然后计算N-2个数据的算术平均值 N值的选取：3~14 B、优点： 融合了两种滤波法的优点 对于偶然出现的脉冲性干扰，可消除由于脉冲干扰所引起的采样值偏差 C、缺点： 测量速度较慢，和算术平均滤波法一样 比较浪费RAM
卡尔曼滤波算法总结-标准化文件发布号：（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）更新协方差阵
非线性滤波除噪技术综述 马义德张祥光 兰州大学信息科学与工程学院，兰州 730000 (Email: ydma@https://www.doczj.com/doc/061594926.html, ) 【摘要】本文阐述了以中值滤波为代表的传统非线性滤波方法以及以形态滤波为代表的新型非线性滤波方法的发展现状，指明自然图像的多样性和噪声本身的复杂性是实现图像滤除噪声的难点，只有将自适应机制、自组织能力、自学习能力与传统的成熟滤波算法相结合，才能使非线性滤波算法彻底摆脱图像多样性和噪声复杂性的困扰。 【关键词】图像复原中值滤波形态滤波遗传算法模糊数学神经网络 1、引言 在不同的应用场合中，存在着不同类型的噪声影响。按噪声对信号的影响可分为加性噪声和乘性噪声两大类[1]。在计算机视觉和数字图像处理中，噪声的消除一直是人们关注的重点。在一些应用领域，例如基于计算图像导数的算子中，图像中的任何一点噪声都会导致严重的错误。噪声与要研究的对象不相关，它以无用的信息形式出现，扰乱图像的可观测信息。噪声可被译成或多或少的极值，这些极值通过加减作用于一些象素的真实灰度级上，在图像上造成黑白亮暗点干扰，极大降低了图像质量，影响图像复原、分割、特征提取、图像识别等后继工作的进行。因而对其抑制处理是图像处理中非常重要的一项工作。 在数字信号处理和数字图像处理的早期研究中，线性滤波器是噪声抑制处理的主要手段。线性滤波器简单的数学表达形式以及某些理想特性使其很容易设计和实现。然而，当信号频谱与噪声频谱混叠时或者当信号中含有非叠加性噪声时(例如由系统非线性引起的噪声或存在非高斯噪声等)，线性滤波器的处理结果就很难令人满意。在处理图像时，传统的线性滤波器在滤除噪声的同时，往往会严重模糊图像细节（如边缘等），而且不能有效滤除椒盐噪声。就是说，线性滤波器在信号与噪声彼此相关情况下不能很好工作。虽然人类视觉的确切特性目前还未完全揭示出来，但许多实验表明，人类视觉系统的第一处理级是非线性的。基于上述原因，早在1958年维纳（Wiener）就提出了非线性滤波理论。非线性滤波器在一定程度上克服了线性滤波器的这一缺点。由于它能够在滤除噪声的同时，最大限度地保持了图像信号的高频细节，使图像清晰、逼真，从而得到广泛应用和研究。目前已有很多比较经典的非线性滤波算法，如：中值滤波[2]、形态滤波[3]、层叠滤波[4]以及基于中值滤波的一些改进滤波算法等。 一般图像处理过程如图1-1图像处理链状图所示，包含以下五项不同的工作： ①图像预处理：具体又分为噪声去除、图像增强、边缘检测以及去模糊等。 ②数据简化：具体又分为图像压缩和特征提取等。 ③分割：具体包括纹理分割、颜色识别和分类等。 ④目标识别：具体包括模板匹配以及基于特征的识别等。 1
一.十一种通用滤波算法(转) 1、限幅滤波法（又称程序判断滤波法） A、方法： 根据经验判断，确定两次采样允许的最大偏差值（设为A） 每次检测到新值时判断： 如果本次值与上次值之差<=A,则本次值有效 如果本次值与上次值之差>A,则本次值无效,放弃本次值,用上次值代替本次值B、优点： 能有效克服因偶然因素引起的脉冲干扰 C、缺点 无法抑制那种周期性的干扰 平滑度差 2、中位值滤波法 A、方法： 连续采样N次（N取奇数） 把N次采样值按大小排列 取中间值为本次有效值 B、优点： 能有效克服因偶然因素引起的波动干扰 对温度、液位的变化缓慢的被测参数有良好的滤波效果 C、缺点： 对流量、速度等快速变化的参数不宜 3、算术平均滤波法 A、方法： 连续取N个采样值进行算术平均运算 N值较大时：信号平滑度较高，但灵敏度较低 N值较小时：信号平滑度较低，但灵敏度较高 N值的选取：一般流量，N=12；压力：N=4
适用于对一般具有随机干扰的信号进行滤波 这样信号的特点是有一个平均值，信号在某一数值范围附近上下波动 C、缺点： 对于测量速度较慢或要求数据计算速度较快的实时控制不适用 比较浪费RAM 4、递推平均滤波法（又称滑动平均滤波法） A、方法： 把连续取N个采样值看成一个队列 队列的长度固定为N 每次采样到一个新数据放入队尾,并扔掉原来队首的一次数据.(先进先出原则) 把队列中的N个数据进行算术平均运算,就可获得新的滤波结果 N值的选取：流量，N=12；压力：N=4；液面，N=4~12；温度，N=1~4 B、优点： 对周期性干扰有良好的抑制作用，平滑度高 适用于高频振荡的系统 C、缺点： 灵敏度低 对偶然出现的脉冲性干扰的抑制作用较差 不易消除由于脉冲干扰所引起的采样值偏差 不适用于脉冲干扰比较严重的场合 比较浪费RAM 5、中位值平均滤波法（又称防脉冲干扰平均滤波法） A、方法： 相当于“中位值滤波法”+“算术平均滤波法” 连续采样N个数据，去掉一个最大值和一个最小值 然后计算N-2个数据的算术平均值 N值的选取：3~14
卡尔曼滤波简介及其算法实现代码 卡尔曼滤波算法实现代码（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/061594926.html,/~welch/media/pdf/Kalman1960.pdf。 简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2．卡尔曼滤波器的介绍 （Introduction to the Kalman Filter） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就
数据处理中的几种常用数字滤波算法 王庆河王庆山 (济钢集团计量管理处，济南250101) (济钢集团中厚板厂，济南250101) 摘要随着数字化技术的发展，数字滤波技术成为数字化仪表和计算机在数据采集中的关键性技术，本文对常用的几种数字滤波算法的原理进行描述，并给出必要的数学模型。 关键词：数据采样噪声滤波移动滤波 一、引言 在仪表自动化工作中，经常需要对大量的数据进行处理，这些数据往往是一个时间序列或空间序列，这时常会用到数字滤波技术对数据进行预处理。数字滤波是指利用数学的方法对原始数据进行处理，去掉原始数据中掺杂的噪声数据，获得最具有代表性的数据集合。 数据采样是一种通过间接方法取得事物状态的技术如将事物的温度、压力、流量等属性通过一定的转换技术将其转换为电信号，然后再将电信号转换为数字化的数据。在多次转换中由于转换技术客观原因或主观原因造成采样数据中掺杂少量的噪声数据，影响了最终数据的准确性。 为了防止噪声对数据结果的影响，除了采用更加科学的采样技术外，我们还要采用一些必要的技术手段对原始数据进行整理、统计，数字滤波技术是最基本的处理方法，它可以剔除数据中的噪声，提高数据的代表性。 二、几种常用的数据处理方法 在实际应用中我们所用的数据滤波方法很多，在计算机应用高度普及的今天更有许多新的方法出现，如逻辑判断滤波、中值滤波、均值滤波、加权平均 2中值滤波 中值滤波是对采样序列按大小排滤波、众数滤波、一阶滞后滤波、移动滤波、复合滤波 等。 假设我们采用前端仪表采集了一组采样周期为1s的温度数据的时间序列 T0为第0s 采集的温度值，Ti为第is采集的温度值。下面介绍如何应用几种不同滤波算法来计算结果温度T。 1.程序判断滤波 当采样信号由于随机干扰、误检测或变送器不稳定引起严重失真时，可采用程序判断滤波算法，该算法的基本原理是根据生产经验，确定出相邻采样输入信号可能的最大偏差△T,若超过此偏差值，则表明该输入信号是干扰信号，应该去掉，若小于偏差值则作为此次采样值。 (1)限幅滤波 限幅滤波是把两次相邻的采集值进行相减，取其差值的绝对值△T作为比较依据，如果小于或等于△T,则取此次采样值,如果大于△T,则取前次采样值，如式(1)所示：
随机误差是有随机干搅引起的，其特点是在相同条件下测量同一个量时，其大小和符号做无规则变化而无法预测，但多次测量结果符合统计规律。为克服随机干搅引入的误差，硬件上可采用滤波技术，软件上可以采用软件算法实现数字滤波，其算法往往是系统测控算法的一个重要组成部分，实时性很强，采用汇编语言来编写。 采用数字滤波算法克服随机干搅引入的误差具有以下几个优点： （1）数字滤波无须硬件，只用一个计算过程，可靠性高，不存在阻抗匹配问题，尤其是数字滤波可以对 频率很高或很低的信号进行滤波，这是模拟滤波器做不到的。 （2）数字滤波是用软件算法实现的，多输入通道可用一个软件“滤波器”从而降低系统开支。 （3）只要适当改变软件滤波器的滤波程序或运行参数，就能方便地改变其滤波特性，这个对于低频、脉冲 干搅、随机噪声等特别有效。 常用的数字滤波器算法有程序判断法、中值判断法、算术平均值法、加权滤波法、滑动滤波法、低通滤波法和复合滤波法。 1．程序判断法： 程序判断法又称限副滤波法，其方法是把两次相邻的采样值相减，求出其增量（以绝对值表示）。然后与两次采样允许的最大差值△Y进行比较，△Y的大小由被测对象的具体情况而定，若小于或等于△Y，则取本次采样的值；若大于△Y，则取上次采样值作为本次采样值，即 yn - yn-1|≤△Y，则yn有效， yn -yn-1|＞△Y，则yn-1有效。 式中yn ——第n次采样的值； Yn-1——第（n-1）次采样的值； △Y——相邻两次采样值允许的最大偏差。 设R1和R2为内部RAM单元，分别存放yn-1和yn，滤波值也存放在R2单元，采用MCS-51单片机指令编写的程序判断法子程序如下：付表 2．中值滤波法即对某一参数连续采样N次（一般N为奇数），然后把N次采样值按从小到大排队，再取中间值作为本次采样值。
经典的滤波算法(转) 1、限幅滤波法（又称程序判断滤波法） A、方法： 根据经验判断，确定两次采样允许的最大偏差值（设为A） 每次检测到新值时判断： 如果本次值与上次值之差<=A,则本次值有效 如果本次值与上次值之差>A,则本次值无效,放弃本次值,用上次值代替本次值 B、优点： 能有效克服因偶然因素引起的脉冲干扰 C、缺点 无法抑制那种周期性的干扰 平滑度差 2、中位值滤波法 A、方法： 连续采样N次（N取奇数） 把N次采样值按大小排列 取中间值为本次有效值 B、优点： 能有效克服因偶然因素引起的波动干扰 对温度、液位的变化缓慢的被测参数有良好的滤波效果 C、缺点： 对流量、速度等快速变化的参数不宜 3、算术平均滤波法 A、方法： 连续取N个采样值进行算术平均运算 N值较大时：信号平滑度较高，但灵敏度较低 N值较小时：信号平滑度较低，但灵敏度较高 N值的选取：一般流量，N=12；压力：N=4 B、优点： 适用于对一般具有随机干扰的信号进行滤波 这样信号的特点是有一个平均值，信号在某一数值范围附近上下波动 C、缺点： 对于测量速度较慢或要求数据计算速度较快的实时控制不适用 比较浪费RAM
递推平均滤波法对偶然出现的脉冲性干扰的抑制作用较差 4、递推平均滤波法（又称滑动平均滤波法） A、方法： 把连续取N个采样值看成一个队列 队列的长度固定为N 每次采样到一个新数据放入队尾,并扔掉原来队首的一次数据.(先进先出原则) 把队列中的N个数据进行算术平均运算,就可获得新的滤波结果 N值的选取：流量，N=12；压力：N=4；液面，N=4~12；温度，N=1~4 B、优点： 对周期性干扰有良好的抑制作用，平滑度高 适用于高频振荡的系统 C、缺点： 灵敏度低 对偶然出现的脉冲性干扰的抑制作用较差 不易消除由于脉冲干扰所引起的采样值偏差 不适用于脉冲干扰比较严重的场合 比较浪费RAM 5、中位值平均滤波法（又称防脉冲干扰平均滤波法） A、方法： 相当于“中位值滤波法”+“算术平均滤波法” 连续采样N个数据，去掉一个最大值和一个最小值 然后计算N-2个数据的算术平均值 N值的选取：3~14 B、优点： 融合了两种滤波法的优点 对于偶然出现的脉冲性干扰，可消除由于脉冲干扰所引起的采样值偏差 C、缺点： 测量速度较慢，和算术平均滤波法一样 比较浪费RAM 6、限幅平均滤波法 A、方法： 相当于“限幅滤波法”+“递推平均滤波法” 每次采样到的新数据先进行限幅处理， 再送入队列进行递推平均滤波处理 B、优点： 融合了两种滤波法的优点 对于偶然出现的脉冲性干扰，可消除由于脉冲干扰所引起的采样值偏差 C、缺点： 比较浪费RAM
第20卷第5期 V ol.20N o.5 控 制 与 决 策 Contr ol and Decision 2005年5月 M a y 2005 收稿日期:2004-05-29;修回日期:2004-09-15. 基金项目:国家自然科学基金项目(60172037);陕西省科学技术研究发展计划项目(2003k 06-G 15);西北工业大学 引进高层次人才科研启动费项目. 作者简介:潘泉(1961—),男,重庆人,教授,博士生导师,从事信息融合理论与应用、自适应滤波、估计与控制等研 究;杨峰(1977—),男,陕西西安人,博士生,从事多传感信息融合理论、机动目标跟踪等研究. 文章编号:1001-0920(2005)05-0481-09 一类非线性滤波器——UKF 综述 潘 泉,杨 峰,叶 亮,梁 彦,程咏梅 (西北工业大学自动化学院,陕西西安710072) 摘 要:回顾了U K F 算法的发展,从一般意义讨论了U T 变换算法和采样策略的选择依据,并给出了U K F 算法描述.从条件函数和代价函数入手,在给出多种采样策略的基础上对U K F 采样策略进行了分析和比较.最后对U K F 算法未来可能的研究方向进行了探讨. 关键词:非线性滤波器;unscented 卡尔曼波滤器;U T 变换;采样策略中图分类号:T P 273 文献标识码:A Survey of a kind of nonlinear filters —UKF PA N Quan ,YAN G Feng ,YE L iang ,LI A N G Yan ,CH EN G Yong -m ei (Co llege of Auto matio n,No rt hw ester n Po ly technical U niver sity ,Xi ′an 710072,China.Cor respondent :YA N G Feng ,E -mail :fly ing _y ang feng @ho tmail .co m ) Abstract :T he adv ances of unscented K alman filter (U KF )ar e fir st ly r eviewed.T he combinat ion of the K alma n linear filter ing w it h t he unscented tr ansfo rm atio n (U T )is discussed in g eneral sense.U KF and its t ypical sampling st rateg ies ar e given and analyzed .Finally ,the po ssible future dir ectio ns o f the U K F ar e also discussed .Key words :no nlinear filtering;unscent ed K alman filt er;unscent ed tr ansfor matio n;sampling str ategy 1 引 言 在许多实际应用问题中,状态方程或量测方程为非线性而噪声为非高斯情况时,滤波问题也表现为非线性.解决非线性滤波问题的最优方案需要得到其条件后验概率的完整描述,然而这种精确的描述需要无尽的参数而无法实际应用[1],为此人们提出了大量次优的近似方法[2,3].对于非线性滤波问题的次优近似,有两大途径: 1)将非线性环节线性化,对高阶项采用忽略或逼近措施; 2)用采样方法近似非线性分布. 对非线性函数进行线性化近似,对高阶项采用忽略或逼近是解决非线性问题的传统途径.其中最广泛使用的是扩展卡尔曼滤波器(EKF)[4,5].EKF 通过对非线性函数的T aylor 展开式进行一阶线性 化截断,从而将非线性问题转化为线性.尽管EKF 得到了广泛的使用,但它存在如下不足: 1)当非线性函数T aylor 展开式的高阶项无法 忽略时,线性化会使系统产生较大的误差,甚至于滤波器难以稳定[6]; 2)在许多实际问题中很难得到非线性函数的雅克比矩阵求导; 3)EKF 需要求导,所以必须清楚了解非线性函数的具体形式,无法作到黑盒封装,从而难以模块 化应用. 目前,虽然对EKF 有众多的改进方法[2,3,7],如高阶截断EKF [2,7],迭代EKF [3]等,但这些缺陷仍然难以克服. 由于近似非线性函数的概率密度分布比近似非线性函数更容易,使用采样方法近似非线性分布来 解决非线性问题的途径在最近得到了人们的广泛关 DOI ：10.13195/j.cd.2005.05.2.panq.001
五种非线性滤波 转载 今天主要实现了五种常见的非线性滤波算子，这五种滤波算子对不同的图像都会有不同的作用，最常用的是中值滤波，因为它的效果最好且信息损失的最少。 1.极大值滤波 极大值滤波就是选取像素点领域的最大值作为改点的像素值，有效率去了灰度值比较低的噪声，也可作为形态学里面的膨胀操作。 极大值滤波可以表示为： Maximum(A)=max[A(x+i,y+j)] (x,y)属于M 注：（x+i,y+j)是定义在图像上的坐标，(i,j)是定义在模板M上的坐标。M即为运算的模板。 2.极小值滤波（与极大值滤波相反） 3.中点滤波 中点滤波常用于去除图像中的短尾噪声，例如高斯噪声和均匀分布噪声。终点滤波器的输出时给定窗口内灰度的极大值和极小值的平均值； Midpoint(A)=(max[A(x+i,y+j)]+min[A(x+i,y+j)])/2 (x,y)属于M 注：（x+i,y+j)是定义在图像上的坐标，(i,j)是定义在模板M上的坐标。M即为运算的模板。 4.中值滤波 中值滤波可以消除图像中的长尾噪声，例如负指数噪声和椒盐噪声。在消除噪声时，中值滤波对图像噪声的模糊极小（受模板大小的影响），中值滤波实质上是用模板内所包括像素灰度的中值来取代模板中心像素的灰度。中值滤波在消除图像内椒盐噪声和保持图像的空域细节方面，其性能优于均值滤波。 Median(A)=Median[A(x+i,y+j)] (x,y)属于M 注：（x+i,y+j)是定义在图像上的坐标，(i,j)是定义在模板M上的坐标。M即为运算的模板。 5.加权中值滤波（中值滤波的改进） 加权中值滤波是在中值滤波的基础上加以改进，其性能在一定程度上优于中值滤波。 下面是自己在算法上的改进：以例子说明