卡尔曼滤波两例题含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度

一个简单的Matlab_GUI编程实例

Matlab GUI编程教程（适用于初学者） 1．首先我们新建一个GUI文件：如下图所示； 选择Blank GUI(Default) 2．进入GUI开发环境以后添加两个编辑文本框，6个静态文本框，和一个按钮，布置如下

图所示； 布置好各控件以后，我们就可以来为这些控件编写程序来实现两数相加的功能了。3．我们先为数据1文本框添加代码； 点击上图所示红色方框，选择edit1_Callback，光标便立刻移到下面这段代码的位置。 1. 2. 3.function edit1_Callback(hObject, eventdata, handles) 4.% hObject handle to edit1 (see GCBO) 5.% eventdata reserved - to be defined in a future version of MATLAB

6.% handles structure with handles and user data (see GUIDATA) 7.% Hints: get(hObject,'String') returns contents of edit1 as text 8.% str2double(get(hObject,'String')) returns contents of edit1 as a double 复制代码 然后在上面这段代码的下面插入如下代码： 1. 2.%以字符串的形式来存储数据文本框1的内容. 如果字符串不是数字，则现实空白内容input = str2num(get(hObject,'String')); %检查输入是否为空. 如果为空,则默认显示为0if (isempty(input)) set(hObject,'String','0')endguidata(hObject, handles); 复制代码 这段代码使得输入被严格限制，我们不能试图输入一个非数字。 4．为edit2_Callback添加同样一段代码 5 现在我们为计算按钮添加代码来实现把数据1和数据2相加的目的。 用3中同样的方法在m文件中找到pushbutton1_Callback代码段 如下； 1.function pushbutton1_Callback(hObject, eventdata, handles) 2.% hObject handle to pushbutton1 (see GCBO) 3.% eventdata reserved - to be defined in a future version of MATLAB 4.% handles structure with handles and user data (see GUIDATA) 复制代码

matlab实现中值滤波去除脉冲噪声matlab小程序

matlab实现中值滤波去除脉冲噪声matlab小程序（图像处理）2010-04-1612:58:44阅读8评论0字号：大中小 实验原理：中值滤波器是将领域内像素灰度的中值代替该像素的值,对处理脉冲噪声(椒盐噪声)非常有效。为了对一幅图像上的某个点进行中值滤波处理，必须先将掩模内欲求的像素及其领域的像素值排序，确定出中值，主要功能是使拥有不同灰度的点看起来更接近于它的邻近值。 程序说明：函数名为mid(pic_name,s)的函数，其中参数pic_name为读入的图像，s为掩模矩阵的边长，由用户自行决定。 实验说明：随着掩模矩阵的变大，我们可以看到脉冲噪声去除得更加理想，但同时图像会变得更模糊，因为各点像素与其邻域更为接近，因此，进行中值滤波时选择一个适合的掩模矩阵十分重要。另外，我们看到图像的边界处出现了黑色的斑点，这是由于我采用了0来直译边界，这种影响可用镜像反射方式对称地沿其边界扩展来减弱。 另附：其实本实验可以完全由matlab中的函数median或medfilt2简单实现，此处写出内部处理过程，主要是为了让大家理解中值滤波的具体处理过程。 程序源代码： function mid(pic_name,s) close all; s=double(s); X=imread(pic_name); Y1=imnoise(X,'salt&pepper',0.2);%对读入的图像加脉冲噪声 figure; imshow(uint8(Y1)); Y1=double(Y1); [m,n]=size(X); s2=round(s/2); s3=round(s*s/2);%中值像素点的位置

扩展卡尔曼滤波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实现实例(推荐给学生)

数字信号处理Matlab 实现实例 第１章离散时间信号与系统 例1-1 用MATLAB计算序列{-2 0 1 –1 3}和序列{1 2 0 -1}的离散卷积。 解 MATLAB程序如下： a=[-2 0 1 -1 3]; b=[1 2 0 -1]; c=conv(a,b); M=length(c)-1; n=0:1:M; stem(n,c); xlabel('n'); ylabel('幅度'); 图1.1给出了卷积结果的图形，求得的结果存放在数组c中为：{-2 -4 1 3 1 5 1 -3}。 例1-2 用MATLAB计算差分方程 当输入序列为时的输出结果。 解 MATLAB程序如下： N=41; a=[0.8 -0.44 0.36 0.22]; b=[1 0.7 -0.45 -0.6]; x=[1 zeros(1,N-1)];

k=0:1:N-1; y=filter(a,b,x); stem(k,y) xlabel('n');ylabel('幅度') 图 1.2 给出了该差分方程的前41个样点的输出，即该系统的单位脉冲响应。 例1-3 用MATLAB 计算例1-2差分方程 所对应的系统函数的DTFT 。 解 例1-2差分方程所对应的系统函数为： 123 123 0.80.440.360.02()10.70.450.6z z z H z z z z -------++= +-- 其DTFT 为 23230.80.440.360.02()10.70.450.6j j j j j j j e e e H e e e e ωωωω ωωω--------++= +-- 用MATLAB 计算的程序如下： k=256; num=[0.8 -0.44 0.36 0.02]; den=[1 0.7 -0.45 -0.6]; w=0:pi/k:pi; h=freqz(num,den,w); subplot(2,2,1); plot(w/pi,real(h));grid title('实部') xlabel('\omega/\pi');ylabel('幅度')

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

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

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

(仅供参考)Matlab编写与调用函数

MATLAB 学习指南 第六章．编写与调用函数 在这一章中，我们讨论如何用多源代码文件来构造一个程序。首先，解释代码文件在MATLAB中如何工作。在编译语言中，例如FORTRAN，C ，或C++，代码被存储在一个或多个源文件中，在进行编译的时候，这些源文件组合在一起 形成了一个单独的可执行文件。作为一种解释型语言，MATLAB以一种更广泛的方式来处理多个源文件。MATLAB代码被放入带有扩展名.m的ASCII文件（或称m-文件）中。MATLAB 6 有一个集成字处理与调试应用程序，尽管会用到其它编辑程序如vi或emacs，集成字处理与调试应用程序仍是编译m-文件的首选程序。 有两种不同的m-文件。一种是脚本文件，它是一种最简单的文件，仅仅将MATLAB中的指令收集在一起。当在交互提示符处输入文件名执行脚本文件时，MATLAB在m-文件内读取并执行指令，就好像指令是我们输入的。而且，似乎我们能够削减m-文件的内容并将削减过的内容传到MATLAB指令窗口中。这种m-文件的用法将在6.1节中给予概述。 在6.2节中要讨论的第二种m-文件包含一个单一函数，此函数名与此m-文件名相同。这种m-文件包含一段独立的代码，这段代码具有一个明确规定的输入/输出界面；那就是说，传给这段代码一列空变量arg1，arg2，…，这段独立代码就能够被调用，然后返回输出值out1，out2，…。一个函数m-文件的第一个非注释行包含函数标头，其形式如下： 此m-文件以返回指令结束，将执行程序返回到函数被调用的位置。或者在交互指令提示符处或者在另一个m-文件内，无论何时用下列指令调用函数代码，函数代码都将被执行。 输入映射到空变量：arg1=var1，arg2=var2，等等。在函数主体内，输出值被分配给了变量out1，out2，等等。当遇到返回值时，当前值out1，out2，…在函数被调用处被映射到变量outvar1，outvar2，…。在用可变长度自变量和输出变量列表编写函数时，MATLAB允许更多的自由。例如，也可以使用下列指令来调用函数。 在此情况下，仅返回一个单一输出变量，这个变量在出口处包含函数变量out1的值。输入和输出自变量可能是字符串，数值，向量，矩阵，或者更高级的数据结构。 为什么使用函数呢？因为从每门计算机科学课程中可知，把一个大的程序分割 成多个可以单独执行一个被明确规定的和被注释过的任务的小程序会使大程序 易读，易于修改，不易于出错。在MATLAB中，先为程序编写一个主文件，或者是一个脚本文件或者更好的话，是一个能够返回一个单一整数的函数m-文件（返回1表示程序执行成功，0表示不完全程序执行，负值表示出现运行误差），这个主文件是程序的进入点。通过把m-文件当作函数来调用，此程序文件可以

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

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

matlab源代码实例

1.硬币模拟试验 源代码： clear; clc; head_count=0; p1_hist= [0]; p2_hist= [0]; n = 1000; p1 = 0.3; p2=0.03; head = figure(1); rand('seed',sum(100*clock)); fori = 1:n tmp = rand(1); if(tmp<= p1) head_count = head_count + 1; end p1_hist (i) = head_count /i; end figure(head); subplot(2,1,1); plot(p1_hist); grid on; hold on; xlabel('重复试验次数'); ylabel('正面向上的比率'); title('p=0.3试验次数N与正面向上比率的函数图'); head_count=0; fori = 1:n tmp = rand(1); if(tmp<= p2) head_count = head_count + 1; end p2_hist (i) = head_count /i; end figure(head); subplot(2,1,2); plot(p2_hist); grid on; hold on; xlabel('重复试验次数'); ylabel('正面向上的比率'); title('p=0.03试验次数N与正面向上比率的函数图'); 实验结果：

2.不同次数的随机试验均值方差比较 源代码： clear ; clc; close; rand('seed',sum(100*clock)); Titles = ['n=5时' 'n=20时' 'n=25时' 'n=50时' 'n=100时']; Titlestr = cellstr(Titles); X_n_bar=[0]; %the samples of the X_n_bar X_n=[0]; %the samples of X_n N=[5,10,25,50,100]; j=1; num_X_n = 100; num_X_n_bar = 100; h_X_n_bar = figure(1);

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

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

MATLAB小程序：将TXT中十六进制数转为十进制输出

matlab小程序：将txt中十六进制数转为十进制输出function htod(filename) clc [n]=textread(filename,'%2c'); [a b]=size(n) m=zeros(a,b); mm=zeros(a,1); for i=1:a for j=1:b switch n(i,j) case{'0'}m(i,j)=0; case{'1'}m(i,j)=1; case{'2'}m(i,j)=2; case{'3'}m(i,j)=3; case{'4'}m(i,j)=4; case{'5'}m(i,j)=5; case{'6'}m(i,j)=6; case{'7'}m(i,j)=7; case{'8'}m(i,j)=8; case{'9'}m(i,j)=9; case{'A'}m(i,j)=10; case{'B'}m(i,j)=11; case{'C'}m(i,j)=12; case{'D'}m(i,j)=13; case{'E'}m(i,j)=14; case{'F'}m(i,j)=15; otherwise m(i,j)=nan; end end end %m for i=1:a for j=1:b mm(i)=mm(i)+m(i,j)*16^(j-1); end end %mm [a b]=size(mm); size_mm=a mmm=mm'; savefile='C:\Documents and Settings\Administrator\桌面\test.txt'; fid=fopen(savefile,'w');

fprintf(fid,'%4d',mmm) fclose(fid); matlab如何读取二进制、十六进制txt文档 发现matlab如何读取十六进制的和二进制的txt文章不多。今天刚想了一种方法，所以在这里小结一下，所以matlab中文论坛共享一下，没有参考其他的文章哦，觉得好用就帮顶，不好用提意见。 原帖地址https://www.wendangku.net/doc/1e7966845.html,/thread-23226-1-1.html 本方法同样适合读取十六进制和二进制以外的其他进制文件， txt使用一个最简单的命令就可以读取textread这是一个十分有用，简便的函数（对于fopen fscanf而言） 读取二进制txt文件： 假如txt文档中内容为00010010001101001000，保存在pin.txt文档中 使用a=textread('pin.txt','%s')' a= '0001''0010''0011''0100''1000' 可以看到数据保存为了char格式。 使用bin2dec b=bin2dec(a)' b= 12348 可以看到成功地转换成了十进制文件。 十六进制文件： 00010010001101001000A B C AA a=textread('pin.txt','%s')' a= '0001''0010''0011''0100''1000''A''B''C''AA' 可以看到成功读取了文件。 b=hex2dec(a)' b= 11617256409610 1112170 读取完毕。 小结：本方法以简单使用方便的方法读取二进制、十六进制的txt文档，欢迎大家提出意见

(完整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;

数学建模基础入门小程序文件

自己整理MATLAB知识 1入门 例1-1 绘制正弦曲线和余弦曲线。 x=[0:0.5:360]*pi/180; plot(x,sin(x),x,cos(x)); 例1-2 求方程3x4+7x3+9x2-23=0的全部根。 p=[3,7,9,0,-23]; %建立多项式系数向量 x=roots(p) %求根 例1-3 求积分 quad('x.*log(1+x)',0,1) %‘里是被积函数’0,1分 别是积分上下限 例1-4 求解线性方程组。 a=[2,-3,1;8,3,2;45,1,-9]; %方程左面系数 b=[4;2;17]; %方程右面系数 x=inv(a)*b %也可是x=a\b的形式 例1-5 水仙花 for m=100:999 m1=fix(m/100); %求m的百位数字 m2=rem(fix(m/10),10); %求m的十位数字 m3=rem(m,10); %求m的个位数字 if m==m1*m1*m1+m2*m2*m2+m3*m3*m3 disp(m)

end end 例1-6 已知，当n=100时，求y的值。程序如下： y=0; n=100; for i=1:n y=y+1/(2*i-1); end y 例1-7 求[100，200]之间第一个能被21整除的整数 for n=100:200 if rem(n,21)~=0 continue end break end n 例1-8 若一个数等于它的各个真因子之和，则称该数为完数，如6=1+2+3，所以6是完数。求[1,500]之间的全部完数。for m=1:500 s=0; for k=1:m/2

如何培养自己成为Matlab编程高手

如何培养自己成为Matlab编程高手？[原创2010-08-15 20:09:59] 字号：大中小对理论的掌握并不代表对知识的真正理解。对于一些所谓高深的理论都可以 自己编写程序来检验对其理解的程度。我的经验是：只有你把程序流畅地写出来，才是真正意义上对知识理解通透了。比如，我在大三学电力系统分析的时候，就自己用Matlab语言编写了牛—拉法求潮流的程序，计算暂态稳定的简单程序，计算发电机短路电流的程序等。自然地这些专业课程都学得不错。 Matlab是一门优秀的编程语言，在欧美非常普及。选择一门顺手的编程语言可以让你在学习和工作中事倍功半。Matlab是一种语言因为它可以用作编程，也是一种软件因为它自带的工具箱具有类似软件前台的GUI界面以及能够轻松实现人机通信功能。在学习Matlab编程之前，需要对其有一个基本的了解： (1) 数据处理 能对数据进行计算、分析和挖掘，数据处理函数功能强大，命令简洁；(2) 软件工具箱 各式各样的工具箱，包括神经网络工具箱、Simulink工具箱（虽然Simulink 从底层开发出来的，但是我们认为也是工具箱的一种）、模糊工具箱、数字图像处理工具箱和金融工具箱等； (3) 精致绘图 Matlab通过“set”命令重设图形的句柄属性，可绘制精准而美观的图形；(4) 动画实现 Matlab可以进行实时动画、电影动画和AVI视频制作，并能在动画中添加*.WAVE格式的音频； (5) 与软硬件通信 Matlab接口函数可以实现与软件（比如C）和硬件（比如电子示波器）通信； (6) 平面设计 与全球最顶尖的平面设计软件之一Adobe Photoshop联袂使用，传达震撼的视觉设计效果； (7) 游戏开发

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

卡尔曼滤波简介及其算法实现代码 卡尔曼滤波算法实现代码（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/1e7966845.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 程序设计方法及若干程序实例 樊双喜 (河南大学数学与 信息科学学院开封475004) 摘要本文通过对 MATLAB 程序设计中的若干典型问题做简要的分析和总结,并在此基础上着重讨论了有关算法设计、程序的调试与测试、算法与程序的优化以及循环控制等方面的问题.还通过对一些程序实例做具体解析,来方便读者进行编程训练并掌握一些有关MATLAB 程序设计方面的基本概念、基本方法以及某些问题的处理技巧等.此外,在文章的最后还给出了几个常用数学方法的算法程序, 供读者参考使用.希望能对初学者进行 MATLAB 编程训练提供一些可供参考的材料,并起到一定的指导和激励作用,进而为MATLAB 编程入门打下好的基础. 关键字算法设计；程序调试与测试；程序优化；循环控制 1 算法与程序 1.1 算法与程序的关系算法被称为程序的灵魂,因此在介绍程序之前应先了 解什么是算法.所谓算 法就是对特定问题求解步骤的一种描述.对于一个较复杂的计算或是数据处理的问题,通常是先设计出在理论上可行的算法,即程序的操作步骤,然后再按照算法逐步翻译成相应的程序语言,即计算机可识别的语言. 所谓程序设计,就是使用在计算机上可执行的程序代码来有效的描述用于解决特定问题算法的过程.简单来说,程序就是指令的集合.结构化程序设计由于采用了模块分化与功能分解,自顶向下,即分而治之的方法,因而可将一个较复杂的问题分解为若干子问题,逐步求精.算法是操作的过程,而程序结构和程序流程则是算法的具体体现. 1.2MATLAB 语言的特点 MATLAB 语言简洁紧凑,使用方便灵活,库函数极其丰富,其语法规则与科技人员的思维和书写习惯相近,便于操作.MATLAB 程序书写形式自由,利用其丰富

有趣的MATLAB 1.游戏程序

MATLAB游戏程序 目录 1.空格游戏 (2) 2.华容道 (3) 3.凑五子棋 (14) 4.2048 (19) 5.俄罗斯方块 (24)

1.空格游戏 function pintu1() A = gen(); G = [1 2 3;4 5 6;7 8 0]; drawmap(A); while 1 [xpos,ypos] = ginput(1); col = ceil(xpos); row = 3-ceil(ypos)+1; num = A(row,col); if row>1&A(row-1,col)==0 A(row-1,col) = num; A(row,col) = 0; end if row<3&A(row+1,col)==0 A(row+1,col) = num; A(row,col) = 0; end if col>1&A(row,col-1)==0 A(row,col-1) = num; A(row,col) = 0; end if col<3&A(row,col+1)==0 A(row,col+1) = num; A(row,col) = 0; end drawmap(A) zt = abs(A-G); if sum(zt(:))==0 msgbox('恭喜您成功完成！') break end end function drawmap(A) clf; hold on

line([0 3],[0 0],'linewidth',4); line([3 3],[0 3],'linewidth',4); line([0 3],[3 3],'linewidth',4); line([0 0],[0 3],'linewidth',4); for i = 1:3 for j = 1:3 drawrect([j-1 3-i],[j 3-i],[j 3-i+1],[j-1 3-i+1],'y',A(i,j)); end end axis equal axis off function drawrect(x1,x2,x3,x4,color,num) x = [x1(1) x2(1) x3(1) x4(1)]; y = [x1(2) x2(2) x3(2) x4(2)]; fill(x,y,color) if num==0 text(0.5*(x1(1)+x2(1)),0.5*(x1(2)+x4(2)),' ','fontsize',24) else text(0.5*(x1(1)+x2(1))-0.05,0.5*(x1(2)+x4(2)),num2str(num),'fontsize',24) end function y = gen() y = inf*ones(1,9); for i = 1:9 while 1 a = randint(1,1,9); if isempty(find(y==a)) y(i) = a; break end end end y = reshape(y,3,3); 2.华容道 function huarongdao() A = [2 1 1 3; 2 1 1 3; 4 6 6 5;