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;
xyerror/t 2*xyerror/(t^2) yerror/t 2*yerror/(t^2)];
xestimate=[Zmeasure(2,i)*cos(Zmeasure(1,i)) 0 Zmeasure(2,i)*sin(Zmeasure(1,i)) 0 ]'; end
cho=(chol(P*(L+ramda)))';%
for j=1:L
xgamaP1(:,j)=xestimate+cho(:,j);
xgamaP2(:,j)=xestimate-cho(:,j);
end
Xsigma=[xestimate xgamaP1 xgamaP2];
F=A;
Xsigmapre=F*Xsigma;
xpred=zeros(Anoise,1);
for j=1:2*L+1
xpred=xpred+Wm(j)*Xsigmapre(:,j);
end
Noise1=Anoise;
ppred=zeros(Noise1,Noise1);
for j=1:2*L+1
ppred=ppred+Wc(j)*(Xsigmapre(:,j)-xpred)*(Xsigmapre(:,j)-xpred)';
end
ppred=ppred+processerror;
chor=(chol((L+ramda)*ppred))';
for j=1:L
XaugsigmaP1(:,j)=xpred+chor(:,j);
XaugsigmaP2(:,j)=xpred-chor(:,j);
end
Xaugsigma=[xpred XaugsigmaP1 XaugsigmaP2 ];
for j=1:2*L+1
Ysigmapre(1,j)=atan(Xaugsigma(3,j)/Xaugsigma(1,j)) ;
Ysigmapre(2,j)=sqrt((Xaugsigma(1,j))^2+(Xaugsigma(3,j))^2);
end
ypred=zeros(2,1);
for j=1:2*L+1
ypred=ypred+Wm(j)*Ysigmapre(:,j);
end
Pzz=zeros(2,2);
for j=1:2*L+1
Pzz=Pzz+Wc(j)*(Ysigmapre(:,j)-ypred)*(Ysigmapre(:,j)-ypred)';
end
Pzz=Pzz+measureerror;
Pxy=zeros(Anoise,2);
for j=1:2*L+1
Pxy=Pxy+Wc(j)*(Xaugsigma(:,j)-xpred)*(Ysigmapre(:,j)-ypred)';
end
K=Pxy*inv(Pzz);
xestimate=xpred+K*(Zmeasure(:,i)-ypred);
P=ppred-K*Pzz*K';
xukf(i)=xestimate(1,1);
yukf(i)=xestimate(3,1); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%% EKF PRO%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if i==1
ekf_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;
xyerror/t 2*xyerror/(t^2) yerror/t 2*yerror/(t^2)];
ekf_xestimate=[Zmeasure(2,i)*cos(Zmeasure(1,i)) 0 Zmeasure(2,i)*sin(Zmeasure(1,i)) 0 ]';
ekf_xpred=ekf_xestimate;
end;
F=A;
ekf_xpred=F*ekf_xestimate;
ekf_ppred=F*ekf_p*F'+processerror;
H=[-ekf_xpred(3)/(ekf_xpred(3)^2+ekf_xpred(1)^2) 0 ekf_xpred(1)/(ekf_xpred(3)^2+ekf_xpred(1)^2) 0;
ekf_xpred(1)/sqrt(ekf_xpred(3)^2+ekf_xpred(1)^2) 0 ekf_xpred(3)/sqrt(ekf_xpred(3)^2+ekf_xpred(1)^2) 0];
ekf_z(1,1)=atan(ekf_xpred(3)/ekf_xpred(1)) ;
ekf_z(2,1)=sqrt((ekf_xpred(1))^2+(ekf_xpred(3))^2);
PHHP=H*ekf_ppred*H'+measureerror;
ekf_K=ekf_ppred*H'*inv(PHHP);
ekf_p=(eye(L)-ekf_K*H)*ekf_ppred;
ekf_xestimate=ekf_xpred+ekf_K*(Zmeasure(:,i)-ekf_z);
traceekf(i)=trace(ekf_p);
xekf(i)=ekf_xestimate(1,1);
yekf(i)=ekf_xestimate(3,1);
errorx(i)=xx(i)+xradarpositon-x(i);
errory(i)=yy(i)+yradarpositon-y(i);
ukferrorx(i)=xestimate(1)+xradarpositon-x(i);
ukferrory(i)=xestimate(3)+yradarpositon-y(i);
ekferrorx(i)=ekf_xestimate(1)+xradarpositon-x(i); ekferrory(i)=ekf_xestimate(3)+yradarpositon-y(i);
aa(i)=xx(i)+xradarpositon-x(i);;
bb(i)=yy(i)+yradarpositon-y(i);
sumx=sumx+(errorx(i)^2);
sumy=sumy+(errory(i)^2);
sumxukf=sumxukf+(ukferrorx(i)^2);
sumyukf=sumyukf+(ukferrory(i)^2);
sumxekf=sumxekf+(ekferrorx(i)^2);
sumyekf=sumyekf+(ekferrory(i)^2);
mseerrorx(i)=sqrt(sumx/(i-1));%噪声的统计均方误差mseerrory(i)=sqrt(sumy/(i-1));
mseerrorxukf(i)=sqrt(sumxukf/(i-1));%UKF的统计均方误差mseerroryukf(i)=sqrt(sumyukf/(i-1));
mseerrorxekf(i)=sqrt(sumxekf/(i-1));%EKF的统计均方误差mseerroryekf(i)=sqrt(sumyekf/(i-1));
end
figure(1);
plot(mseerrorxukf,'r');
hold on;
plot(mseerrorxekf,'g');
hold on;
plot(mseerrorx,'.');
hold on;
ylabel('MSE of X axis','fontsize',15);
xlabel('sample number','fontsize',15);
legend('UKF','EKF','measurement error');
figure(2)
plot(mseerroryukf,'r');
hold on;
plot(mseerroryekf,'g');
hold on;
plot(mseerrory,'.');
hold on;
ylabel('MSE of Y axis','fontsize',15); xlabel('sample number','fontsize',15); legend('UKF','EKF','measurement error');
figure(3)
plot(x,y);
hold on;
plot(xekf,yekf,'g');
hold on;
plot(xukf,yukf,'r');
hold on;
plot(xx,yy,'m');
ylabel(' X ','fontsize',15);
xlabel('Y','fontsize',15);
legend('TRUE','UKF','EKF','measurements');
一个应用实例详解卡尔曼滤波及其算法实现 标签：算法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度
文件一 % 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;
function [R_best,L_best,L_ave,Shortest_Route,Shortest_Length]=ACATSP(C,NC_max,m,Alpha,Beta ,Rho,Q) %%===================================================== ==================== %% ACATSP.m %% Ant Colony Algorithm for Traveling Salesman Problem %% ChengAihua,PLA Information Engineering University,ZhengZhou,China %% Email:aihuacheng@https://www.doczj.com/doc/134526158.html, %% All rights reserved %%------------------------------------------------------------------------- %% 主要符号说明 %% C n个城市的坐标，n×4的矩阵 %% NC_max 最大迭代次数 %% m 蚂蚁个数 %% Alpha 表征信息素重要程度的参数 %% Beta 表征启发式因子重要程度的参数 %% Rho 信息素蒸发系数 %% Q 信息素增加强度系数 %% R_best 各代最佳路线 %% L_best 各代最佳路线的长度 %%===================================================== ==================== %%第一步：变量初始化 n=size(C,1);%n表示问题的规模（城市个数） D=zeros(n,n);%D表示完全图的赋权邻接矩阵 for i=1:n for j=1:n if i~=j D(i,j)=max( ((C(i,1)-C(j,1))^2+(C(i,2)-C(j,2))^2)^0.5,min(abs(C(i,3)-C(j,3)),144- abs(C(i,3)-C(j,3))) );%计算城市间距离 else D(i,j)=eps; end D(j,i)=D(i,j); end end Eta=1./D;%Eta为启发因子，这里设为距离的倒数 Tau=ones(n,n);%Tau为信息素矩阵 Tabu=zeros(m,n);%存储并记录路径的生成 NC=1;%迭代计数器 R_best=zeros(NC_max,n);%各代最佳路线
卡尔曼滤波入门： 卡尔曼滤波是用来进行数据滤波用的，就是把含噪声的数据进行处理之后得出相对真值。卡尔曼滤波也可进行系统辨识。 卡尔曼滤波是一种基于统计学理论的算法，可以用来对含噪声数据进行在线处理，对噪声有特殊要求，也可以通过状态变量的增广形式实现系统辨识。 用上一个状态和当前状态的测量值来估计当前状态，这是因为上一个状态估计此时状态时会有误差，而测量的当前状态时也有一个测量误差，所以要根据这两个误差重新估计一个最接近真实状态的值。 信号处理的实际问题，常常是要解决在噪声中提取信号的问题，因此，我们需要寻找一种所谓有最佳线性过滤特性的滤波器。这种滤波器当信号与噪声同时输入时，在输出端能将信号尽可能精确地重现出来，而噪声却受到最大抑制。 维纳(Wiener)滤波与卡尔曼(Kalman)滤波就是用来解决这样一类从噪声中提取信号问题的一种过滤(或滤波)方法。 (1)过滤或滤波 - 从当前的和过去的观察值x(n)，x(n-1)，x(n-2)，…估计当前的信号值称为过滤或滤波; (2)预测或外推 - 从过去的观察值，估计当前的或将来的信号值称为预测或外推; (3)平滑或内插 - 从过去的观察值，估计过去的信号值称为平滑或内插; 因此，维纳过滤与卡尔曼过滤又常常被称为最佳线性过滤与预测或线性最优估计。这里所谓“最佳”与“最优”是以最小均方误差为准则的。 维纳过滤与卡尔曼过滤都是解决最佳线性过滤和预测问题，并且都是以均方误差最小为准则的。因此在平稳条件下，它们所得到的稳态结果是一致的。然而，它们解决的方法有很大区别。 维纳过滤是根据全部过去的和当前的观察数据来估计信号的当前值，它的解是以均方误差最小条件下所得到的系统的传递函数H(z)或单位样本响应h(n)的形式给出的，因此更常称这种系统为最佳线性过滤器或滤波器。 而卡尔曼过滤是用前一个估计值和最近一个观察数据(它不需要全部过去的观察数据)来估计信号的当前值，它是用状态方程和递推的方法进行估计的，它的解是以估计值(常常是状态变量值)形式给出的。因此更常称这种系统为线性最优估计器或滤波器。 维纳滤波器只适用于平稳随机过程，而卡尔曼滤波器却没有这个限制。维纳过滤中信号和噪声是用相关函数表示的，因此设计维纳滤波器要求已知信号和噪声的相关函数。 卡尔曼过滤中信号和噪声是状态方程和量测方程表示的，因此设计卡尔曼滤波器要求已知状态方程和量测方程(当然，相关函数与状态方程和量测方程之间会存在一定的关系。卡尔曼过滤方法看来似乎比维纳过滤方法优越，它用递推法计算，不需要知道全部过去的数据，从而运用计算机计算方便，而且它可用于平稳和不平稳的随机过程(信号)，非时变和时变的系统。 但从发展历史上来看维纳过滤的思想是40年代初提出来的，1949年正式以书的形式出版。卡尔曼过滤到60年代初才提出来，它是在维纳过滤的基础上发展起来的，虽然如上所述它比维纳过滤方法有不少优越的地方，但是最佳线性过滤问题是由维纳过滤首先解决的，维纳过滤的物理概念比较清楚，也可以认为卡尔曼滤波仅仅是对最佳线性过滤问题提出的一种新的算法。 卡尔曼滤波在数学上是一种统计估算方法，通过处理一系列带有误差的实际量测数据而得到的物理参数的最佳估算。例如在气象应用上，根据滤波的基本思想，利用前一时刻预报误差的反馈信息及时修正预报方程，以提高下一时刻预报精度。作温度预报一般只需要连续两个月的资料即可建立方程和递推关系。
卡尔曼滤波器及其简matlab仿真
卡尔曼滤波器及其简matlab仿真 一、卡尔曼滤波的起源 谈到信号的分析与处理，就离不开滤波两个字。通常，信号的频谱处于有限的频率范围内，而噪声的频谱则散布在很广的频率范围内，为了消除噪声，可以进行频域滤波。但在许多应用场合，需要直接进行时域滤波，从带噪声的信号中提取有用信号。虽然这样的过程其实也算是对信号的滤波，但其所依据的理论，即针对随机信号的估计理论，是自成体系的。人们对于随机信号干扰下的有用信号不能“确知”，只能“估计”。为了“估计”，要事先确定某种准则以评定估计的好坏程度。 1960年卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文A New Approach to Linear Filtering and Prediction Problems （线性滤波与预测问题的新方法），在这篇文章里一种克服了维纳滤波缺点的新方法被提出来，这就是我们今天称之为卡尔曼滤波的方法。卡尔曼滤波应用广泛且功能强大，它可以估计信号的过去和当前状态甚至能估计将来的状态即使并不知道模型的确切性质。 其基本思想是以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值。算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 对于解决很大部分的问题，它是最优，效率最高甚至是最有用的。它的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 卡尔曼滤波不要求保存过去的测量数据，当新的数据到来时，根据新的数据和前一时刻的储值的估计，借助于系统本身的状态转移方程，按照一套递推公式，即可算出新的估值。卡尔曼递推算法大大减少了滤波装置的存储量和计算量，并且突破了平稳随机过程的限制，使卡尔曼滤波器适用于对时变信号的实时处理。
卡尔曼滤波器及其简matlab仿真 一、卡尔曼滤波的起源 谈到信号的分析与处理，就离不开滤波两个字。通常，信号的频谱处于有限的频率范围内，而噪声的频谱则散布在很广的频率范围内，为了消除噪声，可以进行频域滤波。但在许多应用场合，需要直接进行时域滤波，从带噪声的信号中提取有用信号。虽然这样的过程其实也算是对信号的滤波，但其所依据的理论，即针对随机信号的估计理论，是自成体系的。人们对于随机信号干扰下的有用信号不能“确知”，只能“估计”。为了“估计”，要事先确定某种准则以评定估计的好坏程度。 1960年卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文A New Approach to Linear Filtering and Prediction Problems（线性滤波与预测问题的新方法），在这篇文章里一种克服了维纳滤波缺点的新方法被提出来，这就是我们今天称之为卡尔曼滤波的方法。卡尔曼滤波应用广泛且功能强大，它可以估计信号的过去和当前状态甚至能估计将来的状态即使并不知道模型的确切性质。 其基本思想是以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值。算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 对于解决很大部分的问题，它是最优，效率最高甚至是最有用的。它的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 卡尔曼滤波不要求保存过去的测量数据，当新的数据到来时，根据新的数据和前一时刻的储值的估计，借助于系统本身的状态转移方程，按照一套递推公式，即可算出新的估值。卡尔曼递推算法大大减少了滤波装置的存储量和计算量，并且突破了平稳随机过程的限制，使卡尔曼滤波器适用于对时变信号的实时处理。 二、卡尔曼滤波的原理
先新建一个主程序M文件ACATSP.m 代码如下: function [R_best,L_best,L_ave,Shortest_Route,Shortest_Length]=ACATSP(C,NC_max,m,Alpha,Beta,Rho,Q) %%================================================== ======================= %% 主要符号说明 %% C n个城市的坐标，n×2的矩阵 %% NC_max 蚁群算法MATLAB程序最大迭代次数 %% m 蚂蚁个数 %% Alpha 表征信息素重要程度的参数 %% Beta 表征启发式因子重要程度的参数 %% Rho 信息素蒸发系数 %% Q 表示蚁群算法MATLAB程序信息素增加强度系数 %% R_best 各代最佳路线 %% L_best 各代最佳路线的长度 %%================================================== =======================
%% 蚁群算法MATLAB程序第一步：变量初始化 n=size(C,1);%n表示问题的规模（城市个数） D=zeros(n,n);%D表示完全图的赋权邻接矩阵 for i=1:n for j=1:n if i~=j D(i,j)=((C(i,1)-C(j,1))^2+(C(i,2)-C(j,2))^2)^0.5; else D(i,j)=eps; % i = j 时不计算，应该为0，但后面的启发因子要取倒数，用eps（浮点相对精度）表示 end D(j,i)=D(i,j); %对称矩阵 end end Eta=1./D; %Eta为启发因子，这里设为距离的倒数 Tau=ones(n,n); %Tau为信息素矩阵 Tabu=zeros(m,n); %存储并记录路径的生成
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源码，同时请指出为何不能优化到已知的最好解 % % % the procedure of ant colony algorithm for VRP % % % % % % % % % % % % %initialize the parameters of ant colony algorithms load data.txt; d=data(:,2:3); g=data(:,4); m=31; % 蚂蚁数 alpha=1; belta=4;% 决定tao和miu重要性的参数 lmda=0; rou=0.9; %衰减系数 q0=0.95; % 概率 tao0=1/(31*841.04);%初始信息素 Q=1;% 蚂蚁循环一周所释放的信息素 defined_phrm=15.0; % initial pheromone level value QV=100; % 车辆容量 vehicle_best=round(sum(g)/QV)+1; %所完成任务所需的最少车数V=40; % 计算两点的距离 for i=1:32; for j=1:32;
dist(i,j)=sqrt((d(i,1)-d(j,1))^2+(d(i,2)-d(j,2))^2); end; end; %给tao miu赋初值 for i=1:32; for j=1:32; if i~=j; %s(i,j)=dist(i,1)+dist(1,j)-dist(i,j); tao(i,j)=defined_phrm; miu(i,j)=1/dist(i,j); end; end; end; for k=1:32; for k=1:32; deltao(i,j)=0; end; end; best_cost=10000; for n_gen=1:50; print_head(n_gen); for i=1:m; %best_solution=[]; print_head2(i);
卡尔曼滤波简介及其算法实现代码 卡尔曼滤波算法实现代码（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/134526158.html,/~welch/media/pdf/Kalman1960.pdf。 简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2．卡尔曼滤波器的介绍 （Introduction to the Kalman Filter） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就
function [y,val]=QACStic load att48 att48; MAXIT=300; % 最大循环次数 NC=48; % 城市个数 tao=ones(48,48);% 初始时刻各边上的信息最为1 rho=0.2; % 挥发系数 alpha=1; beta=2; Q=100; mant=20; % 蚂蚁数量 iter=0; % 记录迭代次数 for i=1:NC % 计算各城市间的距离 for j=1:NC distance(i,j)=sqrt((att48(i,2)-att48(j,2))^2+(att48(i,3)-att48(j,3))^2); end end bestroute=zeros(1,48); % 用来记录最优路径 routelength=inf; % 用来记录当前找到的最优路径长度 % for i=1:mant % 确定各蚂蚁初始的位置 % end for ite=1:MAXIT for ka=1:mant %考查第K只蚂蚁 deltatao=zeros(48,48); % 第K只蚂蚁移动前各边上的信息增量为零 [routek,lengthk]=travel(distance,tao,alpha,beta); if lengthk 设高度的测量误差是均值为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精讲及仿真 4.1基本蚁群算法 4.1.1基本蚁群算法的原理 蚁群算法是上世纪90年代意大利学者M.Dorigo,v.Maneizz。等人提出来的,在越来越多的领域里得到广泛应用。蚁群算法，是一种模拟生物活动的智能算法，蚁群算法的运作机理来源于现实世界中蚂蚁的真实行为，该算法是由Marco Dorigo 首先提出并进行相关研究的，蚂蚁这种小生物，个体能力非常有限，但实际的活动中却可以搬动自己大几十倍的物体，其有序的合作能力可以与人类的集体完成浩大的工程非常相似，它们之前可以进行信息的交流，各自负责自己的任务，整个运作过程统一有序，在一只蚂蚁找食物的过程中，在自己走过的足迹上洒下某种物质，以传达信息给伙伴，吸引同伴向自己走过的路径上靠拢，当有一只蚂蚁找到食物后，它还可以沿着自己走过的路径返回，这样一来找到食物的蚂蚁走过的路径上信息传递物质的量就比较大，更多的蚂蚁就可能以更大的机率来选择这条路径，越来越多的蚂蚁都集中在这条路径上，蚂蚁就会成群结队在蚁窝与食物间的路径上工作。当然，信息传递物质会随着时间的推移而消失掉一部分，留下一部分，其含量是处于动态变化之中，起初，在没有蚂蚁找到食物的时候，其实所有从蚁窝出发的蚂蚁是保持一种随机的运动状态而进行食物搜索的，因此，这时，各蚂蚁间信息传递物质的参考其实是没有价值的，当有一只蚂蚁找到食物后，该蚂蚁一般就会向着出发地返回，这样，该蚂蚁来回一趟在自己的路径上留下的信息传递物质就相对较多，蚂蚁向着信息传递物质比较高的路径上运动，更多的蚂蚁就会选择找到食物的路径，而蚂蚁有时不一定向着信 息传递物质量高的路径走，可能搜索其它的路径。这样如果搜索到更短的路径后，蚂蚁又会往更短的路径上靠拢，最终多数蚂蚁在最短路径上工作。【基于蚁群算法和遗传算法的机器人路径规划研究】 该算法的特点： (1)自我组织能力，蚂蚁不需要知道整体环境信息，只需要得到自己周围的信息，并且通过信息传递物质来作用于周围的环境，根据其他蚂蚁的信息素来判断自己的路径。 (2)正反馈机制，蚂蚁在运动的过程中，收到其他蚂蚁的信息素影响，对于某路径上信息素越强的路径，其转向该路径的概率就越大，从而更容易使得蚁群寻找到最短的避障路径。 (3)易于与其他算法结合，现实中蚂蚁的工作过程简单，单位蚂蚁的任务也比较单一，因而蚁群算法的规则也比较简单，稳定性好，易于和其他算法结合使得避障路径规划效果更好。 (4)具有并行搜索能力探索过程彼此独立又相互影响，具备并行搜索能力，这样既可以保持解的多样性，又能够加速最优解的发现。 4.1.2 基本蚁群算法的生物仿真模型 a为蚂蚁所在洞穴,food为食物所在区,假设abde为一条路径,eadf为另外一条路径,蚂蚁走过后会留下信息素,5分钟后蚂蚁在两条路径上留下的信息素的量都为3,概率可以认为相同,而30分钟后baed路径上的信息素的量为60,明显大于eadf路径上的信息素的量。最终蚂蚁会完全选择abed这条最短路径,由此可见, MATLAB 对卡尔曼滤波器的仿真实现 刘丹，朱毅，刘冰 武汉理工大学信息工程学院，武汉（430070） E-mail ：liudan_ina@https://www.doczj.com/doc/134526158.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主要程序如下： 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仿真 1.滤波问题浅谈 估计器或滤波器这一术语通常用来称呼一个系统，设计这样的系统是为了从含有噪声的数据中提取人们感兴趣的，接近规定质量的信息。由于这样一个宽目标，估计理论应用于诸如通信、雷达、声纳、导航、地震学、生物医学工程、 金融工程等众多不同的领域。例如，考虑一个数字通信系统，其基本形式由发 射机、信道和接收机连接组成。发射机的作用是把数字源(例如计算机)产生的0、1符号序列组成的消息信号变换成为适合于信道上传送的波形。而由于符号间干扰和噪声的存在，信道输出端收到的信号是含有噪声的或失真的发送信号。接收机的作用是，操作接收信号并把原消息信号的一个可靠估值传递给系统输出端的某个用户。随着通信系统复杂度的提高，对原消息信号的还原成为通信系统中最为重要的环节，而噪声是接收端需要排除的最主要的干扰，人们也设计出了针对各种不同条件应用的滤波器，其中最速下降算法是一种古老的最优化技术，而卡尔曼滤波器随着应用条件的精简成为了普适性的高效滤波器。2．维纳最速下降算法滤波器 2.1 最速下降算法的基本思想 考虑一个代价函数，它是某个未知向量的连续可微分函数。函数 将的元素映射为实数。这里，我们要寻找一个最优解。使它满足如下条件 (2.1) 这也是无约束最优化的数学表示。 特别适合于自适应滤波的一类无约束最优化算法基于局部迭代下降的算法： 从某一初始猜想出发，产生一系列权向量，使得代价函数在算法的每一次迭代都是下降的，即 其中是权向量的过去值，而是其更新值。 我们希望算法最终收敛到最优值。迭代下降的一种简单形式是最速下降法，该方法是沿最速下降方向连续调整权向量。为方便起见，我们将梯度向量表示为 function [y,val]=QACS tic load att48 att48; MAXIT=300; % 最大循环次数 NC=48; % 城市个数 tao=ones(48,48);% 初始时刻各边上的信息最为1 rho=0.2; % 挥发系数 alpha=1; beta=2; Q=100; mant=20; % 蚂蚁数量 iter=0; % 记录迭代次数 for i=1:NC % 计算各城市间的距离 for j=1:NC distance(i,j)=sqrt((att48(i,2)-att48(j,2))^2+(att48(i,3)-att48(j,3))^2); end end bestroute=zeros(1,48); % 用来记录最优路径 routelength=inf; % 用来记录当前找到的最优路径长度 % for i=1:mant % 确定各蚂蚁初始的位置 % end for ite=1:MAXIT for ka=1:mant %考查第K只蚂蚁 deltatao=zeros(48,48); % 第K只蚂蚁移动前各边上的信息增量为零 [routek,lengthk]=travel(distance,tao,alpha,beta); if lengthk卡尔曼滤波两例题含matlab程序汇总
matlab蚁群算法精讲及仿真图
matlab对卡尔曼滤波的仿真实现
基于扩展卡尔曼滤波算法的matlab程序
维纳最速下降法滤波器卡尔曼滤波器设计及Matlab仿真
(完整版)蚁群算法matlab程序实例整理
相关主题
文本预览