ukf_6_div_system.rar_6维无迹卡尔曼滤波_UKF

function ukf_6_div_system n=6;%状态维数 t=0.5;%采样时间 Q=diag([1 1 0.01 0.01 0.0001 0.0001]);%过程噪声协方差 R=diag([100 0.001^2]);%观测噪声协方差 f=@(x)[x(1)+t*x(3)+0.5*t^2*x(5);x(2)+t*x(4)+0.5*t^2*x(6);x(3)+t*x(5);x(4)+t*x(6);x(5);x(6)];%状态方程 %x(1)为x轴的位置，x(2)是y轴的位置；x(3),x(4)分别是xy轴的速度，x(5),x(6)分别是xy轴的加速度 h=@(x)[sqrt(x(1)^2+x(2)^2);atan(x(2)/x(1))]; s=[1000;5000;10;50;2;-4];%初始赋值 x0=s+sqrtm(Q)*randn(n,1); P0=diag([100 100 1 1 0.1 0.1]); N=50;%仿真步数 Xukf=zeros(n,N);%UKF滤波状态初始化 X=zeros(n,N);%%真是状态 Z=zeros(2,N);%%测量值 for i=1:N; X(:,i)=f(s)+sqrtm(Q)*randn(6,1); s=X(:,i); end ux=x0;%ux是中间变量 for k=1:N; Z(:,k)=h(X(:,k))+sqrtm(R)*randn(2,1); [Xukf(:,k),P0]=ukf(f,ux,P0,h,Z(:,k),Q,R); ux=Xukf(:,k); end %%%以下为误差分析 for k=1:N; RMS(k)=sqrt((X(1,k)-Xukf(1,k))^2+(X(2,k)-Xukf(2,k))^2); end figure %轨迹图 t=1:N; hold on;box on; plot(X(1,t),X(2,t),'k-') plot(Z(1,t).*cos(Z(2,t)),Z(1,t).*sin(Z(2,t)),'b-.') plot(Xukf(1,t),Xukf(2,t),'r-') legend('实际值','测量值','ukf估计值') xlabel('x方向位置/m') ylabel('y方向位置/m') figure %误差分析图 box on plot(RMS,'-ko','MarkerFace','r') xlabel('t/s'); ylabel('偏差/m') title('跟踪位置偏差') %%%%%%%%%%%%%%%%%以下为UKF子程序%%%% function [X,P]=ukf(ffun,X,P,hfun,Z,Q,R) L=numel(X);%%状态维数 m=numel(Z);%%%%观测维数 alpha=1e-2; ki=0; beta=2; lambda=alpha^2*(L+ki)-L; c=L+lambda; Wm=[lambda/c 0.5/c+zeros(1,2*L)]; Wc=Wm; Wc(1)=Wc(1)+(1-alpha^2+beta); c=sqrt(c); Xsigmaset=sigma(X,P,c);%%sigma点集在状态X附近的点集，X是6*13的矩阵，每列样本为1 [X1means,X1,P1,X2]=ut(ffun,Xsigmaset,Wm,Wc,L,Q);%%对状态进行UT变换 [Zpre,Z1,Pzz,Z2]=ut(hfun,X1,Wm,Wc,m,R); %%5对观测进行ut变换 Pxz=X2*diag(Wc)*Z2'; %%计算协方差Pxz K=Pxz*inv(Pzz);%%计算卡尔曼增益 X=X1means+K*(Z-Zpre);%%%状态更新 P=P1-K*Pxz';%%协方差更新 %%%%UT变换子函数，输入fun函数句柄，Xsigma为样本集，Wm和Wc为权值，n=6为状态维数，COV为方差，输出Xmeans为均值 function [Xmeans,Xsigma_pre,P,Xdiv]=ut(fun,Xsigma,Wm,Wc,n,COV); LL=size(Xsigma,2);%得到样本的个数 Xmeans=zeros(n,1); Xsigma_pre=zeros(n,LL); for k=1:LL Xsigma_pre(:,k)=fun(Xsigma(:,k)); Xmeans=Xmeans+Wm(k)*Xsigma_pre(:,k); end Xdiv=Xsigma_pre-Xmeans(:,ones(1,LL));%%%%预测减去均值，Xmeans(:,ones(1,LL)将Xmeans扩展成n*LL矩阵，每一列都相等 P=Xdiv*diag(Wc)*Xdiv'+COV %%%协方差 %%%%%%%%%%%%%产生sigma点集函数 function Xset=sigma(X,P,c) A=c*chol(P)';%choleskey分解 Y=X(:,ones(1,numel(X))); Xset=[X Y+A Y-A];