文档库 最新最全的文档下载
当前位置:文档库 › 卡尔曼滤波算法与matlab实现

卡尔曼滤波算法与matlab实现

卡尔曼滤波算法与matlab实现
卡尔曼滤波算法与matlab实现

一个应用实例详解卡尔曼滤波及其算法实现

标签:算法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度温度值的偏差,得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差(对应于上面的3)。

就是这样,卡尔曼滤波器就不断的把covariance递归,从而估算出最优的温度值。他运行的很快,而且它只保留了上一时刻的covariance。上面的Kg,就是卡尔曼增益(Kalman Gain)。他可以随不同的时刻而改变他自己的值,是不是很神奇!

下面就要言归正传,讨论真正工程系统上的卡尔曼。

3.卡尔曼滤波器算法

(The Kalman Filter Algorithm)

在这一部分,我们就来描述源于Dr Kalman 的卡尔曼滤波器。下面的描述,会涉及一些基本的概念知识,包括概率(Probability),随即变量(Random Variable),高斯或正态分配(Gaussian Distribution)还有State-space Model 等等。但对于卡尔曼滤波器的详细证明,这里不能一一描述。

首先,我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程(Linear Stochastic Difference equation)来描述:

X(k)=A X(k-1)+B U(k)+W(k)

再加上系统的测量值:

Z(k)=H X(k)+V(k)

上两式子中,X(k)是k时刻的系统状态,U(k)是k时刻对系统的控制量。A和B 是系统参数,对于多模型系统,他们为矩阵。Z(k)是k时刻的测量值,H是测量

系统的参数,对于多测量系统,H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise),他们的covariance 分别是Q,R(这里我们假设他们不随系统状态变化而变化)。

对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出(类似上一节那个温度的例子)。

首先我们要利用系统的过程模型,来预测下一状态的系统。假设现在的系统状态是k,根据系统的模型,可以基于系统的上一状态而预测出现在状态:

X(k|k-1)=A X(k-1|k-1)+B U(k) (1)

式(1)中,X(k|k-1)是利用上一状态预测的结果,X(k-1|k-1)是上一状态最优的结果,U(k)为现在状态的控制量,如果没有控制量,它可以为0。

到现在为止,我们的系统结果已经更新了,可是,对应于X(k|k-1)的covariance(协方差)还没更新。我们用P表示covariance:

P(k|k-1)=A P(k-1|k-1) A’+Q (2)

式(2)中,P(k|k-1)是X(k|k-1)对应的covariance,P(k-1|k-1)是X(k-1|k-1)对应的covariance,A’表示A的转置矩阵,Q是系统过程的covariance。式子1,2就是卡尔曼滤波器5个公式当中的前两个,也就是对系统的预测。

现在我们有了现在状态的预测结果,然后我们再收集现在状态的测量值。结合预

测值和测量值,我们可以得到现在状态(k)的最优化估算值X(k|k):

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) (3)

其中Kg为卡尔曼增益(Kalman Gain):

Kg(k)= P(k|k-1) H’/ (H P(k|k-1) H’+ R) (4)

到现在为止,我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束,我们还要更新k状态下X(k|k)的covariance:

P(k|k)=(I-Kg(k) H)P(k|k-1) (5)

其中I 为1的矩阵,对于单模型单测量,I=1。当系统进入k+1状态时,P(k|k)就是式子(2)的P(k-1|k-1)。这样,算法就可以自回归的运算下去。

卡尔曼滤波器的原理基本描述了,式子1,2,3,4和5就是他的5 个基本公式。根据这5个公式,可以很容易的实现计算机的程序。

下面,用Matlab程序举一个实际运行的例子。

4.简单例子

(A Simple Example)

这里我们结合第二第三节,举一个非常简单的例子来说明卡尔曼滤波器的工作过程。所举的例子是进一步描述第二节的例子,而且还会配以程序模拟结果。

根据第二节的描述,把房间看成一个系统,然后对这个系统建模。当然,我们见的模型不需要非常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的,所以A=1。没有控制量,所以U(k)=0。因此得出:

X(k|k-1)=X(k-1|k-1) (6)

式子(2)可以改成:

P(k|k-1)=P(k-1|k-1) +Q (7)

因为测量的值是温度计的,跟温度直接对应,所以H=1。式子3,4,5可以改成以下:

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) (8)

Kg(k)= P(k|k-1) / (P(k|k-1) + R) (9)

P(k|k)=(1-Kg(k))P(k|k-1) (10)

现在我们模拟一组测量值作为输入。假设房间的真实温度为25 度,我模拟了200个测量值,这些测量值的平均值为25度,但是加入了标准偏差为几度的高斯白噪声(在图中为蓝线)。

为了令卡尔曼滤波器开始工作,我们需要告诉卡尔曼两个零时刻的初始值,是X(0|0)和P(0|0)。他们的值不用太在意,随便给一个就可以了,因为随着卡尔曼的工作,X会逐渐的收敛。但是对于P,一般不要取0,因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的,从而使算法不能收敛。我选了

X(0|0)=1 度,P(0|0)=10。

该系统的真实温度为25度,图中用黑线表示。图中红线是卡尔曼滤波器输出的最优化结果(该结果在算法中设置了Q=1e-6,R=1e-1)。

clear

N=200;

w(1)=0;

w=randn(1,N)

x(1)=0;

a=1;

for k=2:N;

x(k)=a*x(k-1)+w(k-1);

end

V=randn(1,N);

q1=std(V);

Rvv=q1.^2;

q2=std(x);

Rxx=q2.^2;

q3=std(w);

Rww=q3.^2;

c=0.2;

Y=c*x+V;

p(1)=0;

s(1)=0;

for t=2:N;

p1(t)=a.^2*p(t-1)+Rww;

b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);

s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));

p(t)=p1(t)-c*b(t)*p1(t);

end

t=1:N;

plot(t,s,'r',t,Y,'g',t,x,'b');

用matlab做的kalman滤波程序,已通过测试

--------------------------还有下面一个Matlab源程序,显示效果更好。

clear

clc;

N=300;

CON = 25;%房间温度,假定温度是恒定的%%%%%%%%%%%%%%%kalman filter%%%%%%%%%%%%%%%%%%%%%%

x = zeros(1,N);

y = 2^0.5 * randn(1,N) + CON;%加过程噪声的状态输出

x(1) = 1;

p = 10;

Q = cov(randn(1,N));%过程噪声协方差

R = cov(randn(1,N));%观测噪声协方差

for k = 2 : N

x(k) = x(k - 1);%预估计k时刻状态变量的值

p = p + Q;%对应于预估值的协方差

kg = p / (p + R);%kalman gain

x(k) = x(k) + kg * (y(k) - x(k));

p = (1 - kg) * p;

end

%%%%%%%%%%%Smoothness Filter%%%%%%%%%%%%%%%%%%%%%%%%

Filter_Wid = 10;

smooth_res = zeros(1,N);

for i = Filter_Wid + 1 : N

tempsum = 0;

for j = i - Filter_Wid : i - 1

tempsum = tempsum + y(j);

end

smooth_res(i) = tempsum / Filter_Wid;

end

% figure(1);

% hist(y);

t=1:N;

figure(1);

expValue = zeros(1,N);

for i = 1: N

expValue(i) = CON;

end

plot(t,expValue,'r',t,x,'g',t,y,'b',t,smooth_res,'k'); legend('expected','estimate','measure','smooth result');

axis([0 N 20 30])

xlabel('Sample time');

ylabel('Room Temperature');

title('Smooth filter VS kalman filter');

卡尔曼滤波算法--核心公式推导导论

再造红旗

写在最前面:这是我第一篇专栏文章,感谢知乎提供这么一个平台,让自己能和大家分享知识。本人会不定期的开始更新文章,文章的内容应该集中在汽车动力学控制,整车软件架构,控制器等方面。作为一名在校硕士,很多理解都可能不全面,不正确,大家有不同意见欢迎讨论。谢谢!

---------------------------------------------

卡尔曼滤波算法很牛逼,因为有一堆公式,有一堆符号,看起来就很牛逼啊,乍一看不懂的都很牛逼啊!

本文针对卡尔曼滤波算法的核心公式进行推导,不让大家被它华丽的外表吓到。(之后计划写关于针对非线性情况的EKF和UKF,对卡尔曼滤波算法做一个全面一点的应用介绍。感兴趣的可以关注专栏。)

-------------------------------------------------------------------------------------------------------

Okay,进入正题。这篇文章假设读者已经对卡尔曼滤波算法有初步的了解,知道它能做什么,知道它的优点,知道它很牛逼,并且你已经对它产生兴趣,但不知道如何下手。

首先给出一个控制理论中公式,别急着翻控制理论的书,没那么复杂:

两个基本问题:

1.卡尔曼滤波算法要做什么?

对状态进行估计。

2. 卡尔曼滤波算法怎么对状态进行估计?

利用状态过程噪声和测量噪声对状态进行估计。

一个状态在一个时刻点k的状态进入下一个时刻点k+1状态,会有很多外界因素的干扰,我们把干扰就叫做过程噪声,(这个词一看就是硬翻译过来的,别在意为什么叫噪声)用w表示。任何一个测量仪器,都会有误差,我们把这个误差叫做量测噪声,用v表示。

回到上面那个公式,状态方程表示状态在不断的更新,从一个时刻点进入下一个时刻点,这个很好理解。关键是量测方程,它表示,我们不断更新的状态有几个能用测量仪器测出来,比如,汽车运动状态参数有很多,比如速度,轮速,滑移率等,但是我们只能测量出轮速,因此量测方程要做的就是把状态参数中能量测的状态拿出来。

我们始终要记得我们要做的事:我们要得到的是优化的状态量Xk。

理解了上面之后就可以开始推导公式了。

1.首先不考虑过程噪声对状态进行更新,很简单:

举个例子,v(k)=v(k-1)+at,匀加速运动咯。

2.不考虑测量噪声取出能测量的状态,也很简单:

3.用测量仪器测量出来的状态值(大家可以考虑到:测量的值就是被各种噪声干扰后的真实值)减去上面不考虑噪声得到的测量值:

这个值在数学上是一个定义值,叫做新息,有很多有趣的性质,感兴趣的可以自己谷歌。

我们对步骤暂且停一停。这个叫新息的值有什么用?由上面的过程我们可以明显看到,它反映了过程噪声和测量噪声综合对测量状态值的影响,也就是它包含了w和v的情况。

回到数学层面,(不要害怕,很简单的数学应用和思考啦!)一个数值c由两部分内容a和b组成,那么怎样用数学表达式来表达?

一般有两种做法:

I.直接相加:c=a+b;

II. 用比例的方法:a=n*c,b=(1-n)*c

卡尔曼采用了方法II,用比例的方法来做(其实这也是为什么叫做滤波的原因,因为滤波就是给权值之类的操作)。也就是说,过程噪声w=新息*一个比例。这样得到的过程噪声加上原来(第一步)不考虑过程噪声的状态值不就是优化值了吗?也就是:

Okay,都写到这里了,有必要做一下前提假设:

a. 什么高斯噪声,均值为零一堆;

b.Ak,Ck,wk的协方差Q,vk的协方差R,系统协方差初始值P0,状态初始值X0,都已知。为什么已知,你实际做项目就知道了。不过不懂的可以留言或者私信。

那么到目前为止我们的思路就是清楚了,找到一个合适的Hk值(卡尔曼增益),那么我们就能得到状态的最优值。(卡尔曼说的,不是我说的,所以你问为什么,你要问他,这么深层次的理论留给博士和学者们去做就好,我们就现学现用就行,哈哈哈,站在巨人的肩膀!)

问题来了:怎么得到合适的Hk?似乎不是随便一个参数。

这是误差协方差矩阵。

思路:使得误差协方差矩阵Pk最小的Hk。

为什么?这里我从感观的角度说明自己的理解,欢迎讨论。

协方差表示什么,协方差表示两者之间的联系或者关系,关系越大,协方差越大。误差协方差越小说明过程噪声和量测噪声的关系越小。关系越小能做什么,这要回到我们第3步讨论的我们用比例的方法分开了w和v。用比例分开,到底多少属于w,多少是v,如果关系越小,分开的越精确,比如一堆白砂糖和盐,如果两种混合的很均匀,我们说它关系很大,也就越难用比例的方法将其分开。

4.求的误差协方差矩阵Pk

自然是把里面的Xk先得到,然后公式运算,通过上面的步骤我们也容易得到:

然后复杂的数学计算,和之前假设的高斯噪声,新息的性质之类(至于过程,个人觉得你如果只做应用,不研究算法,就没必要深入去看了),就能得到下面的卡尔曼滤波递推公式:

通过上面的解释,我们也就不难知道这些公式都在干嘛,知道干嘛就可以了。在知道A,C,P0,Q,R的情况下,整个公式的运算流程也都很清晰了。

过程方程:

X(k+1) = A X(k) + B U(k) + W(k) >>>>式1

量测方程:

Z(k+1) = H X(k+1)+ V(k+1) >>>>式2

A和B是系统参数,对于多模型系统,他们为矩阵;H是测量系统的参数,对于多测量系统,H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声,他们的协方差分别是Q,R。为了不失一般性,下面的讨论中将X,Z都视为矩阵,其中X是m 行的单列矩阵,Z是n行的单列矩阵。

说明:下面的表达式中,不带前缀的量都代表实际量,其小括号里面的“k”或“k+1”代表该量是第k或第k+1时刻的实际量,如“Z(k+1)”就代表第k+1时刻的实际测量值;带前缀“^”的量都代表预测量,如果小括号里面是“k+1|k”,就代表k+1时刻的先验预测值,如果小括号里面是“k+1|k+1”,就代表k+1时刻的后验预测值;(测量值可以通过测量得到,所以只有先验预测,没有后验预测。而实际状态值无法得知,既有先验预测,又有后验预测)

带前缀“~”的量都代表与预测值对应的偏差值。

实际状态值与先验预测状态值的偏差= 实际状态值–先验预测状态值

~X(k+1|k) = X(k+1) - ^X(k+1|k) >>>> 式3

实际测量值与先验预测测量值的偏差= 当前测量值- 先验预测测量值

~Z(k+1|k) = Z(k+1) - ^Z(k+1|k) >>>>式4

并且

先验预测测量值= 转换矩阵H * 先验预测状态值

^Z(k+1|k) = H ^X(k+1|k) >>>> 式5

得到测量值后,再对当前状态值X(k+1) 进行后验预测(设后验预测值为^Z(k+1|k+1) ),则后验预测值(同时也是最终预测值)的偏差为

~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1) >>>>式6

为了得到当前状态值X(k+1),根据式3,需要:

X(k+1) = ^X(k+1|k) + ~X(k+1|k) >>>> 式7

上式中,我们可以通过卡尔曼公式1(见附注2)计算出^X(k+1|k),但我们无法得知实际状态值X(k+1),因而~X(k+1|k) 也无法得知。我们最终的目的是得出一个比较接近实际状态值X(k+1)的滤波值^X(k+1|k+1),根据式7,只要能准确的估计出~X(k+1|k)即可。

~X(k+1|k)本身虽无法得知,但~Z(k+1|k) 却可以通过测量得到,而且它们二者存在一定的相关性。不妨再设存在一个矩阵K(m行n列矩阵),能使得

~X(k+1|k) = K * ~Z(k+1|k) >>>>式8

那么最终的预测任务其实就是找到K。由于~X(k+1|k)和~Z(k+1|k)都是单列矩阵,因此不难看出,满足式8的矩阵K应有无穷多个。矩阵K中第i行第j列反映了量测变量偏差矩阵~Z(k+1|k)的第j个元素对状态变量偏差矩阵~X(k+1|k)的第i个元素的贡献。因此矩阵K 的物理意义很明显,K的第i行第j列的元素表示:对于第i个待测的状态量来说,第j个测量仪器测到的偏差的可信度。某个测量值对应的可信度越高,滤波器越“相信”该测量值。

既然满足条件的K有无穷多个,那应该使用哪个K呢?实际上,我们并不知道~X(k+1|k)的值,所以也就无法直接计算出K,而只能通过某种方法找到一个Kg,使得将Kg带入式8后,等号两边的差(的平方)的期望尽可能小。

我们最终的预测值或滤波值是后验预测值^X(k+1|k+1),因此最后的预测也应使

~X(k+1|k+1) 的期望为0且方差最小(这与让8式两端的差最小是一致的,下面的式9体现了这一点),这样预测值才最可靠。下面详细说明。

^X(k+1|k+1) = ^X(k+1|k) + Kg * ~Z(k+1|k) (后验预测的状态值)

~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1) (后验预测的偏差)

~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1)

= ( ^X(k+1|k) + ~X(k+1|k) ) - ( ^X(k+1|k) + Kg *

~Z(k+1|k) )

= ~X(k+1|k) - Kg * ~Z(k+1|k)

>>>>式9

~Z(k+1|k) = Z(k+1) - ^Z(k+1|k)

= ( H X(k+1)+ V(k+1) ) - ( H ^X(k+1|k) )

= H ( X(k+1)-^X(k+1|k) ) + V(k+1)

= H ~ X(k+1|k) + V(k+1) >>>>式10

卡尔曼滤波算法与matlab实现

一个应用实例详解卡尔曼滤波及其算法实现 标签:算法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度

扩展卡尔曼滤波matlab程序

文件一 % THIS PROGRAM IS FOR IMPLEMENTATION OF DISCRETE TIME PROCESS EXTENDED KALMAN FILTER % FOR GAUSSIAN AND LINEAR STOCHASTIC DIFFERENCE EQUATION. % By (R.C.R.C.R),SPLABS,MPL. % (17 JULY 2005). % Help by Aarthi Nadarajan is acknowledged. % (drawback of EKF is when nonlinearity is high, we can extend the % approximation taking additional terms in Taylor's series). clc; close all; clear all; Xint_v = [1; 0; 0; 0; 0]; wk = [1 0 0 0 0]; vk = [1 0 0 0 0]; for ii = 1:1:length(Xint_v) Ap(ii) = Xint_v(ii)*2; W(ii) = 0; H(ii) = ‐sin(Xint_v(ii)); V(ii) = 0; Wk(ii) = 0; end Uk = randn(1,200); Qu = cov(Uk); Vk = randn(1,200); Qv = cov(Vk); C = [1 0 0 0 0]; n = 100; [YY XX] = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V); for it = 1:1:length(XX) MSE(it) = YY(it) ‐ XX(it); end tt = 1:1:length(XX); figure(1); subplot(211); plot(XX); title('ORIGINAL SIGNAL'); subplot(212); plot(YY); title('ESTIMATED SIGNAL'); figure(2); plot(tt,XX,tt,YY); title('Combined plot'); legend('original','estimated'); figure(3); plot(MSE.^2); title('Mean square error'); 子文件::function [YY,XX] = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V); Ap(2,:) = 0; for ii = 1:1:length(Ap)‐1 Ap(ii+1,ii) = 1;

卡尔曼滤波入门简介及其算法MATLAB实现代码

卡尔曼滤波入门: 卡尔曼滤波是用来进行数据滤波用的,就是把含噪声的数据进行处理之后得出相对真值。卡尔曼滤波也可进行系统辨识。 卡尔曼滤波是一种基于统计学理论的算法,可以用来对含噪声数据进行在线处理,对噪声有特殊要求,也可以通过状态变量的增广形式实现系统辨识。 用上一个状态和当前状态的测量值来估计当前状态,这是因为上一个状态估计此时状态时会有误差,而测量的当前状态时也有一个测量误差,所以要根据这两个误差重新估计一个最接近真实状态的值。 信号处理的实际问题,常常是要解决在噪声中提取信号的问题,因此,我们需要寻找一种所谓有最佳线性过滤特性的滤波器。这种滤波器当信号与噪声同时输入时,在输出端能将信号尽可能精确地重现出来,而噪声却受到最大抑制。 维纳(Wiener)滤波与卡尔曼(Kalman)滤波就是用来解决这样一类从噪声中提取信号问题的一种过滤(或滤波)方法。 (1)过滤或滤波 - 从当前的和过去的观察值x(n),x(n-1),x(n-2),…估计当前的信号值称为过滤或滤波; (2)预测或外推 - 从过去的观察值,估计当前的或将来的信号值称为预测或外推; (3)平滑或内插 - 从过去的观察值,估计过去的信号值称为平滑或内插; 因此,维纳过滤与卡尔曼过滤又常常被称为最佳线性过滤与预测或线性最优估计。这里所谓“最佳”与“最优”是以最小均方误差为准则的。 维纳过滤与卡尔曼过滤都是解决最佳线性过滤和预测问题,并且都是以均方误差最小为准则的。因此在平稳条件下,它们所得到的稳态结果是一致的。然而,它们解决的方法有很大区别。 维纳过滤是根据全部过去的和当前的观察数据来估计信号的当前值,它的解是以均方误差最小条件下所得到的系统的传递函数H(z)或单位样本响应h(n)的形式给出的,因此更常称这种系统为最佳线性过滤器或滤波器。 而卡尔曼过滤是用前一个估计值和最近一个观察数据(它不需要全部过去的观察数据)来估计信号的当前值,它是用状态方程和递推的方法进行估计的,它的解是以估计值(常常是状态变量值)形式给出的。因此更常称这种系统为线性最优估计器或滤波器。 维纳滤波器只适用于平稳随机过程,而卡尔曼滤波器却没有这个限制。维纳过滤中信号和噪声是用相关函数表示的,因此设计维纳滤波器要求已知信号和噪声的相关函数。 卡尔曼过滤中信号和噪声是状态方程和量测方程表示的,因此设计卡尔曼滤波器要求已知状态方程和量测方程(当然,相关函数与状态方程和量测方程之间会存在一定的关系。卡尔曼过滤方法看来似乎比维纳过滤方法优越,它用递推法计算,不需要知道全部过去的数据,从而运用计算机计算方便,而且它可用于平稳和不平稳的随机过程(信号),非时变和时变的系统。 但从发展历史上来看维纳过滤的思想是40年代初提出来的,1949年正式以书的形式出版。卡尔曼过滤到60年代初才提出来,它是在维纳过滤的基础上发展起来的,虽然如上所述它比维纳过滤方法有不少优越的地方,但是最佳线性过滤问题是由维纳过滤首先解决的,维纳过滤的物理概念比较清楚,也可以认为卡尔曼滤波仅仅是对最佳线性过滤问题提出的一种新的算法。 卡尔曼滤波在数学上是一种统计估算方法,通过处理一系列带有误差的实际量测数据而得到的物理参数的最佳估算。例如在气象应用上,根据滤波的基本思想,利用前一时刻预报误差的反馈信息及时修正预报方程,以提高下一时刻预报精度。作温度预报一般只需要连续两个月的资料即可建立方程和递推关系。

卡尔曼滤波器及其简matlab仿真

卡尔曼滤波器及其简matlab仿真

卡尔曼滤波器及其简matlab仿真 一、卡尔曼滤波的起源 谈到信号的分析与处理,就离不开滤波两个字。通常,信号的频谱处于有限的频率范围内,而噪声的频谱则散布在很广的频率范围内,为了消除噪声,可以进行频域滤波。但在许多应用场合,需要直接进行时域滤波,从带噪声的信号中提取有用信号。虽然这样的过程其实也算是对信号的滤波,但其所依据的理论,即针对随机信号的估计理论,是自成体系的。人们对于随机信号干扰下的有用信号不能“确知”,只能“估计”。为了“估计”,要事先确定某种准则以评定估计的好坏程度。 1960年卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文A New Approach to Linear Filtering and Prediction Problems (线性滤波与预测问题的新方法),在这篇文章里一种克服了维纳滤波缺点的新方法被提出来,这就是我们今天称之为卡尔曼滤波的方法。卡尔曼滤波应用广泛且功能强大,它可以估计信号的过去和当前状态甚至能估计将来的状态即使并不知道模型的确切性质。 其基本思想是以最小均方误差为最佳估计准则,采用信号与噪声的状态空间模型利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计,求出当前时刻的估计值。算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 对于解决很大部分的问题,它是最优,效率最高甚至是最有用的。它的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。 卡尔曼滤波不要求保存过去的测量数据,当新的数据到来时,根据新的数据和前一时刻的储值的估计,借助于系统本身的状态转移方程,按照一套递推公式,即可算出新的估值。卡尔曼递推算法大大减少了滤波装置的存储量和计算量,并且突破了平稳随机过程的限制,使卡尔曼滤波器适用于对时变信号的实时处理。

卡尔曼滤波器及其简matlab仿真.

卡尔曼滤波器及其简matlab仿真 一、卡尔曼滤波的起源 谈到信号的分析与处理,就离不开滤波两个字。通常,信号的频谱处于有限的频率范围内,而噪声的频谱则散布在很广的频率范围内,为了消除噪声,可以进行频域滤波。但在许多应用场合,需要直接进行时域滤波,从带噪声的信号中提取有用信号。虽然这样的过程其实也算是对信号的滤波,但其所依据的理论,即针对随机信号的估计理论,是自成体系的。人们对于随机信号干扰下的有用信号不能“确知”,只能“估计”。为了“估计”,要事先确定某种准则以评定估计的好坏程度。 1960年卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文A New Approach to Linear Filtering and Prediction Problems(线性滤波与预测问题的新方法),在这篇文章里一种克服了维纳滤波缺点的新方法被提出来,这就是我们今天称之为卡尔曼滤波的方法。卡尔曼滤波应用广泛且功能强大,它可以估计信号的过去和当前状态甚至能估计将来的状态即使并不知道模型的确切性质。 其基本思想是以最小均方误差为最佳估计准则,采用信号与噪声的状态空间模型利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计,求出当前时刻的估计值。算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 对于解决很大部分的问题,它是最优,效率最高甚至是最有用的。它的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。 卡尔曼滤波不要求保存过去的测量数据,当新的数据到来时,根据新的数据和前一时刻的储值的估计,借助于系统本身的状态转移方程,按照一套递推公式,即可算出新的估值。卡尔曼递推算法大大减少了滤波装置的存储量和计算量,并且突破了平稳随机过程的限制,使卡尔曼滤波器适用于对时变信号的实时处理。 二、卡尔曼滤波的原理

(完整word版)扩展卡尔曼滤波算法的matlab程序

clear all v=150; %%目标速度 v_sensor=0;%%传感器速度 t=1; %%扫描周期 xradarpositon=0; %%传感器坐标yradarpositon=0; %% ppred=zeros(4,4); Pzz=zeros(2,2); Pxx=zeros(4,2); xpred=zeros(4,1); ypred=zeros(2,1); sumx=0; sumy=0; sumxukf=0; sumyukf=0; sumxekf=0; sumyekf=0; %%%统计的初值 L=4; alpha=1; kalpha=0; belta=2; ramda=3-L; azimutherror=0.015; %%方位均方误差rangeerror=100; %%距离均方误差processnoise=1; %%过程噪声均方差 tao=[t^3/3 t^2/2 0 0; t^2/2 t 0 0; 0 0 t^3/3 t^2/2; 0 0 t^2/2 t]; %% the input matrix of process G=[t^2/2 0 t 0 0 t^2/2 0 t ]; a=35*pi/180; a_v=5/100; a_sensor=45*pi/180; x(1)=8000; %%初始位置

y(1)=12000; for i=1:200 x(i+1)=x(i)+v*cos(a)*t; y(i+1)=y(i)+v*sin(a)*t; end for i=1:200 xradarpositon=0; yradarpositon=0; Zmeasure(1,i)=atan((y(i)-yradarpositon)/(x(i)-xradarpositon))+random('Normal',0,azimutherror,1,1); Zmeasure(2,i)=sqrt((y(i)-yradarpositon)^2+(x(i)-xradarpositon)^2)+random('Normal',0,rangeerror,1,1); xx(i)=Zmeasure(2,i)*cos(Zmeasure(1,i));%%观测值 yy(i)=Zmeasure(2,i)*sin(Zmeasure(1,i)); measureerror=[azimutherror^2 0;0 rangeerror^2]; processerror=tao*processnoise; vNoise = size(processerror,1); wNoise = size(measureerror,1); A=[1 t 0 0; 0 1 0 0; 0 0 1 t; 0 0 0 1]; Anoise=size(A,1); for j=1:2*L+1 Wm(j)=1/(2*(L+ramda)); Wc(j)=1/(2*(L+ramda)); end Wm(1)=ramda/(L+ramda); Wc(1)=ramda/(L+ramda);%+1-alpha^2+belta; %%%权值 if i==1 xerror=rangeerror^2*cos(Zmeasure(1,i))^2+Zmeasure(2,i)^2*azimutherror^2*sin(Zmeasure(1,i))^2; yerror=rangeerror^2*sin(Zmeasure(1,i))^2+Zmeasure(2,i)^2*azimutherror^2*cos(Zmeasure(1,i))^2; xyerror=(rangeerror^2-Zmeasure(2,i)^2*azimutherror^2)*sin(Zmeasure(1,i))*cos(Zmeasure(1,i)); P=[xerror xerror/t xyerror xyerror/t; xerror/t 2*xerror/(t^2) xyerror/t 2*xyerror/(t^2); xyerror xyerror/t yerror yerror/t;

卡尔曼滤波简介及其算法实现代码

卡尔曼滤波简介及其算法实现代码 卡尔曼滤波算法实现代码(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.wendangku.net/doc/ab16867798.html,/~welch/media/pdf/Kalman1960.pdf。 简单来说,卡尔曼滤波器是一个“optimal recursive data processing algorithm(最优化自回归数据处理算法)”。对于解决很大部分的问题,他是最优,效率最高甚至是最有用的。他的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。 2.卡尔曼滤波器的介绍 (Introduction to the Kalman Filter) 为了可以更加容易的理解卡尔曼滤波器,这里会应用形象的描述方法来讲解,而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是,他的5条公式是其核心内容。结合现代的计算机,其实卡尔曼的程序相当的简单,只要你理解了他的那5条公式。 在介绍他的5条公式之前,先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断,这个房间的温度是恒定的,也就

卡尔曼滤波两例题含matlab程序汇总

设高度的测量误差是均值为0、方差为1的高斯白噪声随机序列,该物体的初始高度0h 和速度0V 也是高斯分布的随机变量,且0000019001000,var 10/02Eh h m P EV m s V ???????? ===? ??????? ???? ????。试求该物体高度和速度随时间变化的最优估计。(2/80.9s m g =) 解: 1. 令()()()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. 实验结果

matlab对卡尔曼滤波的仿真实现

MATLAB 对卡尔曼滤波器的仿真实现 刘丹,朱毅,刘冰 武汉理工大学信息工程学院,武汉(430070) E-mail :liudan_ina@https://www.wendangku.net/doc/ab16867798.html, 摘 要:本文以卡尔曼滤波器原理为理论基础,用MATLAB 进行卡尔曼滤波器仿真、对比卡尔曼滤波器的预测效果,对影响滤波其效果的各方面原因进行讨论和比较,按照理论模型进行仿真编程,清晰地表述了编程过程。 关键词:数字信号处理;卡尔曼滤波器;MATLAB ;仿真过程 中图分类号: TN912.3 1. 引言 随着信息时代和数字世界的到来,数字信号处理已成为当今一门极其重要的学科和技术领域。数字信号处理已在通信、语音、图像、自动控制、雷达、军事、航空航天、医疗和家用电器等众多领域得到了广泛的应用。在数字信号处理中,数字滤波占有极其重要的地位,目前对数字滤波器的设计有多种方法,其中著名的MATLAB 软件包在多个研究领域都有着广泛的应用,它的频谱分析[1]和滤波器的分析设计功能很强,从而使数字信号处理变得十分简单、直观。本文分析了数字滤波器的设计方法,举出了基于MATLAB 软件的信号处理工具在数字滤波器设计中的应用。 2. 卡尔曼滤波基本原理 卡尔曼滤波过程实际上是获取维纳解的递推运算过程[2]。从维纳解导出的卡尔曼滤波器实际上是卡尔曼滤波过程结束后达到稳态的情况,这时Kalman Filtering 的结果与Wiener Solution 是相同的[3]。具体推导如下: )()1|1(?)|(?n Gy n n x f n n x +??= )|(?)()(n n x n x n e ?= 已知由此求c a cG a f F G n e E n ,)1(( ..min )]([)(2?=??→?==ε 由 f G f G ,0??????????=??εε ⑴ )]1|1(?)()[()1|1(?)|(????+??=n n x ac n y n G n n x a n n x 可以是时变的,非平稳的随机信号 ⑵ Q n a n P +?=)1()(2 ε均为正数。 ⑶ ) () ()(2n P C R n CP n G += ⑷ )()](1[)()(n P n CG n G C P n ??== ε )(n G 是个随时间变化的量,每次输入输出,)(n G 就调整一次,并逐渐逼近Kalman Filter 的增益G ,而)1()(?

基于扩展卡尔曼滤波算法的matlab程序

扩展卡尔曼滤波原理: 在原有卡尔曼滤波的基础上,为了解决多目标值的跟踪与估计,形成了扩展卡尔曼滤波。起matlab主要程序如下: clear all v=150; %%目标速度 v_sensor=0;%%传感器速度 t=1; %%扫描周期 xradarpositon=0; %%传感器坐标 yradarpositon=0; %% ppred=zeros(4,4); Pzz=zeros(2,2); Pxx=zeros(4,2); xpred=zeros(4,1); ypred=zeros(2,1); sumx=0; sumy=0; sumxukf=0; sumyukf=0; sumxekf=0; sumyekf=0; %%%统计的初值 %滤波算法描述: L=4; alpha=1; kalpha=0; belta=2; ramda=3-L; azimutherror=0.015; %%方位均方误差 rangeerror=100; %%距离均方误差 processnoise=1; %%过程噪声均方差 tao=[t^3/3 t^2/2 0 0; t^2/2 t 0 0; 0 0 t^3/3 t^2/2; 0 0 t^2/2 t]; %% the input matrix of process G=[t^2/2 0 t 0 0 t^2/2 0 t ]; a=35*pi/180; a_v=5/100;

a_sensor=45*pi/180; x(1)=8000; %%初始位置 y(1)=12000; for i=1:200 x(i+1)=x(i)+v*cos(a)*t; y(i+1)=y(i)+v*sin(a)*t; end for i=1:200 xradarpositon=0; yradarpositon=0; Zmeasure(1,i)=atan((y(i)-yradarpositon)/(x(i)-xradarpositon))+random('Normal',0,azimutherror,1,1); Zmeasure(2,i)=sqrt((y(i)-yradarpositon)^2+(x(i)-xradarpositon)^2)+random('Normal',0,rangeerror,1,1); xx(i)=Zmeasure(2,i)*cos(Zmeasure(1,i));%%观测值 yy(i)=Zmeasure(2,i)*sin(Zmeasure(1,i)); measureerror=[azimutherror^2 0;0 rangeerror^2]; processerror=tao*processnoise; vNoise = size(processerror,1); wNoise = size(measureerror,1); A=[1 t 0 0; 0 1 0 0; 0 0 1 t; 0 0 0 1]; Anoise=size(A,1); for j=1:2*L+1 Wm(j)=1/(2*(L+ramda)); Wc(j)=1/(2*(L+ramda)); end Wm(1)=ramda/(L+ramda); Wc(1)=ramda/(L+ramda);%+1-alpha^2+belta; %%%权值 if i==1 xerror=rangeerror^2*cos(Zmeasure(1,i))^2+Zmeasure(2,i)^2*azimutherror^2*sin(Zmeasure(1,i))^2; yerror=rangeerror^2*sin(Zmeasure(1,i))^2+Zmeasure(2,i)^2*azimutherror^2*cos(Zmeasure(1,i))^2; xyerror=(rangeerror^2-Zmeasure(2,i)^2*azimutherror^2)*sin(Zmeasure(1,i))*cos(Zmeasure(1,i));

维纳最速下降法滤波器卡尔曼滤波器设计及Matlab仿真

信息融合大作业 ——维纳最速下降法滤波器,卡尔曼滤波器设计及Matlab仿真 1.滤波问题浅谈 估计器或滤波器这一术语通常用来称呼一个系统,设计这样的系统是为了从含有噪声的数据中提取人们感兴趣的,接近规定质量的信息。由于这样一个宽目标,估计理论应用于诸如通信、雷达、声纳、导航、地震学、生物医学工程、 金融工程等众多不同的领域。例如,考虑一个数字通信系统,其基本形式由发

射机、信道和接收机连接组成。发射机的作用是把数字源(例如计算机)产生的0、1符号序列组成的消息信号变换成为适合于信道上传送的波形。而由于符号间干扰和噪声的存在,信道输出端收到的信号是含有噪声的或失真的发送信号。接收机的作用是,操作接收信号并把原消息信号的一个可靠估值传递给系统输出端的某个用户。随着通信系统复杂度的提高,对原消息信号的还原成为通信系统中最为重要的环节,而噪声是接收端需要排除的最主要的干扰,人们也设计出了针对各种不同条件应用的滤波器,其中最速下降算法是一种古老的最优化技术,而卡尔曼滤波器随着应用条件的精简成为了普适性的高效滤波器。2.维纳最速下降算法滤波器 2.1 最速下降算法的基本思想 考虑一个代价函数,它是某个未知向量的连续可微分函数。函数 将的元素映射为实数。这里,我们要寻找一个最优解。使它满足如下条件 (2.1) 这也是无约束最优化的数学表示。 特别适合于自适应滤波的一类无约束最优化算法基于局部迭代下降的算法: 从某一初始猜想出发,产生一系列权向量,使得代价函数在算法的每一次迭代都是下降的,即 其中是权向量的过去值,而是其更新值。 我们希望算法最终收敛到最优值。迭代下降的一种简单形式是最速下降法,该方法是沿最速下降方向连续调整权向量。为方便起见,我们将梯度向量表示为

卡尔曼滤波算法及MATLAB实现

基于matlab的卡尔曼信号滤波设计 卡尔曼滤波的基本思想是:以最小均方误差为最佳估计准则,采用信号与噪声的状态空间模型,利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计,求出当前时刻的估计值,算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 语音信号在较长时间内是非平稳的,但在较短的时间内的一阶统计量和二阶统计量近似为常量,因此语音信号在相对较短的时间内可以看成白噪声激励以线性时不变系统得到的稳态输出。假定语音信号可看成由一AR模型产生: 时间更新方程: 测量更新方程: K(t)为卡尔曼增益,其计算公式为: 其中 、分别为过程模型噪声协方差和测量模型噪声协方差,测量协方差可以通过观测得到, 则较难确定,在本实验中则通过与两者比较得到。 由于语音信号短时平稳,因此在进行卡尔曼滤波之前对信号进行分帧加窗操作,在滤波之后对处理得到的信号进行合帧,这里选取帧长为256,而帧重叠个数为128; 下图为原声音信号与加噪声后的信号以及声音信号与经卡尔曼滤波处理后的信号:

原声音信号与加噪声后的信号 原声音信号与经卡尔曼滤波处理后的信号 MATLAB程序实现如下: %%%%%%%%%%%%%%%%%基于LPC全极点模型的最大后验概率估计法,采用卡尔曼滤波%%%%%%%%%%%%%% clear; clc; %%%%%%%%%%%%%%%%%%%%%%%%%%%加载声音数据%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% loadvoice.mat y=m1(2,:); x=y+0.08*randn(1,length(y)); %%%%%%%%%%%%%%%原声音信号和加噪声后的信号%%%%%%%%%%%%%%% figure(1); subplot(211);plot(m1(1,:),m1(2,:));xlabel('时间');ylabel('幅度');title('原声音信号'); subplot(212);plot(m1(1,:),x);xlabel('时间');ylabel('幅度');title('加噪声后的信号');

完整word版Kalman滤波MATLAB综合实验报告

《数学实验》综合实验报告实验名称综合实验(Kalman滤波) 2016年 5月

一、【实验目的】 明白滤波计算流程 能够调用相关函数进行数据处理 使用循环函数和二维曲线画图 有效的构建仿真模型,产生模拟数据 二、【实验原理分析】 卡尔曼滤波器是一个“optimal recursive data processing algorithm(最优化自回归数据处理算法)”。对于解决很大部分的问题,它是最优,效率最高甚至是最有用的。它的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。 设系统可用一个线性随机微分方程来描述: X(k)=A X(k-1)+B U(k)+W(k) 再加上系统的测量值:Z(k)=H X(k)+V(k) 上两式子中,X(k)是k时刻的系统状态,U(k)是k时刻对系统的控制量。A和B是系统参数,对于多模型系统,他们为矩阵。Z(k)是k时刻的测量值,H是测量系统的参数,对于多测量系统,H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声,他们的协方差分别是Q,R(这里假设他们不随系统状态变化而变化)。 对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。首先要利用系统的过程模型,来预测下一状态的系统。假设现在的系统状态是k,根据系统的模型,可以基于系统的上一状态而预测出现在状态: X(k|k-1)=A X(k-1|k-1)+B U(k) (1) 式(1)中,X(k|k-1)是利用上一状态预测的结果,X(k-1|k-1)是上一状态最优的结果,U(k)为现在状态的控制量,如果没有控制量,它可以为0。到现在为止,我们的系统结果已经更新了,可是,对应于X(k|k-1)的协方差还没更新。我们用P表示协方差: P(k|k-1)=A P(k-1|k-1) A'+Q (2) 式(2)中,P(k|k-1)是X(k|k-1)对应的协方差,P(k-1|k-1)是X(k-1|k-1)对应的协方差,A'表示A的转置矩阵,Q是系统过程的协方差。式子1,2就是卡尔曼滤波器5个公式当中的前两个,也就是对系统的预测。 现在我们有了现在状态的预测结果,然后我们再收集现在状态的测量值。结合预测值和测量值,我们 2 可以得到现在状态(k)的最优化估算值X(k|k): X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) (3) 其中Kg为卡尔曼增益(Kalman Gain): Kg(k)= P(k|k-1) H' / (H P(k|k-1) H' + R) (4) 到现在为止,我们已经得到了k状态下最优的估算值X(k|k)。但是为了要令卡尔曼滤波器不断的运行下去直到系统过程结束,我们还要更新k状态下X(k|k)的协方差:

Kalman 滤波和MATLAB 实现

Kalman滤波和MATLAB实现 卡尔曼滤波器(Kalman Filter)是一个最优化自回归数据处理算法(optimal recursive data processing algorithm)。对于解决很大部分的问题,他是最优,效率最高甚至是最有用的。他的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等 简单来说,卡尔曼滤波器是一个“optimal recursive data processing algorithm(最优化自回归数据处理算法)”。对于解决很大部分的问题,他是最优,效率最高甚至是最有用的。他的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。 为了可以更加容易的理解卡尔曼滤波器,这里应用形象的描述方法讲解,不像参考书那样罗列一大堆的数学公式和数学符号。但是,他的5条公式是其核心内容。结合现代的计算机,其实卡尔曼的程序相当的简单,只要你理解了他的那5条公式。 假设我们要研究的对象是一个房间的温度。根据你的经验判断,这个房间的温度是恒定的,也就是下一分钟的温度等于现在这一分钟的温度。假设你对你的经验不是100%的相信,可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声,也就是这些偏差跟前后时间是没有关系的而且符合高斯分配。另外,我们在房间里放一个温度计,但是这个温度计也是不准确的,测量值会比实际值有偏差。我们也把这些偏差看成是高斯白噪声。 好了,现在对于某一分钟我们有两个有关该房间的温度值:你根据经验的预测值(系统的预测值)和温度计的值(测量值)。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。 假如我们要估算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)。

卡尔曼滤波简介及其算法MATLAB实现代码

卡尔曼滤波简介说明及其算法MATLAB实现代码 卡尔曼滤波算法实现代码(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.wendangku.net/doc/ab16867798.html,/~welch/media/pdf/Kalman1960.pdf。 简单来说,卡尔曼滤波器是一个“optimal recursive data processing algorithm(最优化自回归数据处理算法)”。对于解决很大部分的问题,他是最优,效率最高甚至是最有用的。他的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。 2.卡尔曼滤波器的介绍 (Introduction to the Kalman Filter) 为了可以更加容易的理解卡尔曼滤波器,这里会应用形象的描述方法来讲解,而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是,他的5条公式是其核心内容。结合现代的计算机,其实卡尔曼的程序相当的简单,只要你理解了他的那5条公式。 在介绍他的5条公式之前,先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断,这个房间的温度是恒定的,也就

卡尔曼滤波matlab 代码

kalman滤波matlab代码%kalman filter卡尔曼滤波 clear clc A = [1,1;0,1]; B = [1/2,1]'; C = [1,0]; x1(1)= 100; %初始化 x2(1)= 10; x = [x1(1),x2(1)]'; z=C*x; P = [1,0;0,1]; Q=[2,0;0,1] R = 10; g=0.98; u=-g; I=eye(2); for k=2:20 xk=A*x+B*u; %KF 方程1 xg1(k)=xk(1); xg2(k)=xk(2); z(k)=C*xk+wgn(1,1,10); P=A*P*A'+Q; %KF 方程2 Kk=P*C'/(C*P*C'+R); %KF 方程3 x=xk+Kk*(z(k)-C*xk); %KF 方程4 x1(k)=x(1); x2(k)=x(2); e1(k)=x1(k)-xg1(k); e2(k)= x2(k)-xg2(k); P=(I-Kk*C)*P; %KF 方程5 end k=1:20; figure(1) plot(k,x1,k,xg1) title('x1(k)的估计值和真实值曲线') legend('x1(k)的估计值','x1(k)的真实值') figure(2); plot(k,x2,k,xg2') title('x2(k)的估计值和真实值曲线')

legend('x2(k)的估计值','x2(k)的真实值') figure (3) subplot(2,1,1) plot(k,e1) title('x1(k)的估计值和真实值误差曲线') subplot(2,1,2) plot(k,e2) title('x2(k)的估计值和真实值误差曲线') figure (4) plot(k,z) title('输出z的观测值曲线')

相关文档
相关文档 最新文档