研发埠

标题: 无迹卡尔曼滤波器的MATLAB实现(有源程序代码) [打印本页]

作者: 王欢    时间: 2015-1-9 15:10
标题: 无迹卡尔曼滤波器的MATLAB实现(有源程序代码)
通信方向的研究生在学习现代信号处理这门课程的时候,必会遇到卡尔曼滤波器的学习章节。卡尔曼滤波器是在维纳滤波器的最小均方误差准则下,无需系统参数,却能求求解出最佳输出参数的滤波器。其功能特别强大,能应用与多个领域。下面是卡尔曼滤波器最新算法---无迹卡尔曼滤波器的MATLAB实现源程序,供大家学习,参考,期待大侠们的积极探讨!源程序已经过多次仿真,没有问题!该程序用于描述UKF无迹卡尔曼滤波器,其中systemfun为系统函数,measurefun为测量函数。用于非线性系统的滤波有较好的效果。%---------------------------------------%该文件用于编写无迹卡尔曼滤波算法及其测试%注解:主要子程序包括:轨迹发生器、系统方程%     测量方程、UKF滤波器%作者:Jiangfeng%日期:2012.4.16%---------------------------------------function UKFmain%------------------清屏----------------close all;clear all;clc; tic;global Qf n;             %定义全局变量%------------------初始化--------------stater0=[220; 1;55;-0.5];   %标准系统初值state0=[200;1.3;50;-0.3];   %测量状态初值%--------系统滤波初始化 p=[0.005 0 0 0;0 0.005 0 0;   0 0 0.005 0;0 0 0 0.005]; %状态误差协方差初值                   n=4; T=3;Qf=[T^2/2 0;0 T;T^2/2 0;0 T];%--------------------------------------stater=stater0;state=state0; xc=state;staterout=[]; stateout=[];xcout=[];errorout=[];tout=[];t0=1; h=1; tf=1000;        %仿真时间设置%---------------滤波算法----------------for t=t0:h:tf    [state,stater,yc]=track(state,stater); %轨迹发生器:标准轨迹和输出    [xc,p]=UKFfiter(@systemfun,@measurefun,xc,yc,p);    error=xc-stater;         %滤波处理后的误差    staterout=[staterout,stater];    stateout=[stateout,state];    errorout=[errorout,error];    xcout=[xcout,xc];     tout=[tout,t];end %---------------状态信息图像---------------figure;plot(tout,xcout(1,,'r',tout,staterout(1,,'g',...   tout,stateout(1,,'black');legend('滤波后','真实值','无滤波');grid on;xlabel('时间 t(s)');ylabel('系统状态A');title('无迹卡尔曼滤波');figure;plot(tout,xcout(2,,'r',tout,staterout(2,,'g',...   tout,stateout(2,,'black');grid on;legend('滤波后','真实值','无滤波');xlabel('时间 t(s)');ylabel('系统状态B');title('无迹卡尔曼滤波');figure;plot(tout,xcout(3,,'r',tout,staterout(3,,'g',...   tout,stateout(3,,'black');grid on;legend('滤波后','真实值','无滤波');xlabel('时间 t(s)');ylabel('系统状态C');title('无迹卡尔曼滤波');figure;plot(tout,xcout(4,,'r',tout,staterout(4,:),'g',...   tout,stateout(4,:),'black');grid on;legend('滤波后','真实值','无滤波');xlabel('时间 t(s)');ylabel('系统状态D');title('无迹卡尔曼滤波');figure;plot(tout,errorout(1,:),'r',tout,errorout(2,:),'g',...   tout,errorout(3,:),'black',tout,errorout(4,:),'b');grid on;legend('A','B','C','D');xlabel('时间 t(s)');ylabel('滤波后的状态误差');title('无迹卡尔曼滤波误差');%---------------------------------------------toc; %计算仿真程序运行时间endfunction [state,stater,yout]=track(state0,stater0)%-----------------------------------%轨迹发生函数%-----------------------------------T=3;F=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];G=[T^2/2 0;0 T;T^2/2 0;0 T];V=0.005*randn(2,1);W=0.008*randn(1,1);state=F*state0+G*V;stater=F*stater0;yout=atan(stater0(3)/stater0(1))+W;%用真实值得到测量值,在滤波时结果才会与真实值重合。endfunction state=systemfun(state0)%-------------------------%系统方程%-------------------------    T=3;F=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];state=F*state0;endfunction yout=measurefun(state0)%----------------------------%测量方程%----------------------------yout=atan(state0(3)/state0(1));endfunction [xc,p]=UKFfiter(systemfun,measurefun,xc0,yc,p0)%------------------------------------------%此程序用于描述UKF(无迹kalman滤波)算法%作者: Jiangfeng%日期:2012.3.17%------------------------------------------global Qf n;%----------------参数注解-------------------% xc0---状态初值(列向量) yc---系统测量值% p0----状态误差协方差    n----系统状态量数%systemfun---系统方程 measurefun--测量方程%---------------滤波初始化------------------alp=1;                       %default, tunablekap=-1;                       %default, tunablebeta=2;                       %default, tunablelamda=alp^2*(n+kap)-n;             %scaling factornc=n+lamda;                      %scaling factorWm=[lamda/nc 0.5/nc+zeros(1,2*n)];     %weights for meansWc=Wm;Wc(1)=Wc(1)+(1-alp^2+beta);           %weights for covariancens=sqrt(nc);%------------------------------------------- sxk=0;spk=0;syk=0;pyy=0;pxy=0; p=p0;       %--------------构造sigma点-----------------pk=ns*chol(p); % B=chol(A);meant:A'*A=B;sigma=xc0;for k=1:2*n    if(k
作者: 上一站,守护    时间: 2015-1-13 17:44
niucha很实用,下载学习下,谢谢楼主!




欢迎光临 研发埠 (http://bbs.yanfabu.com/) Powered by Discuz! X3.2