www.pudn.com > UKF.rar > UKF.m, change:2014-12-06,size:3372b


for f=1:2 
clear; 
% tic; 
x_estimate=[0.1;0.6]; 
 Q=[0.003^2 0; 0 0.005^2];% 过程状态协方差  
R=0.001^2;%input('请输入测量噪声方差R的值: '); % 测量噪声协方差  
P=eye(2);%初始估计方差 
tf=10000; % 模拟长度  
n=2; % 对称的点的个数 
tall=0.001; 
w=0.003*randn(1,tf); 
r=0.005*randn(1,tf); 
v=0.001*randn(1,tf); 
for k=1:tf 
    x(:,1)=[0.8 0.2]'; 
    x(1,k+1)=x(1,k)+0.001*x(2,k)+w(k); 
    x(2,k+1)=x(2,k)+0.001*(-x(1,k)+(x(1,k)^2+x(2,k)^2-1)*x(2,k))+r(k); 
    y(k)=x(1,k)+x(2,k)+v(k); 
end 
KS_1=eye(n); 
KS_2=-eye(n); 
KS=sqrt(n)*[KS_1,KS_2]; 
        %UKF滤波器 
%        %采样点的选取  
%       X=sigmas(x_estimate,P,c); 
%        x_bar=X; 
for k=1:tf 
     F=sqrtm(P); 
    for i=1:2*n 
        x_bar(:,i)=x_estimate+F*KS(:,i); 
    end 
     %把这些粒子通过变形 得到同数目的样本 
    for i = 1:2*n 
     gam(1,i)=x_bar(1,i)+0.001*x_bar(2,i); 
     gam(2,i)=x_bar(2,i)+0.001*(-x_bar(1,i)+(x_bar(1,i)^2+x_bar(2,i)^2-1)*x_bar(2,i)); 
    end 
    %新均值和方差 
   x_next(:,1)=zeros(n,1); 
   for i=1:2*n 
       x_next(:,i+1)=x_next(:,i)+gam(:,i); 
   end 
   x_next=x_next(:,2*n+1)/(2*n); 
    p_x_next(:,:,1)=zeros(n); 
  for i = 1 :2*n 
    p_x_next(:,:,i+1)=p_x_next(:,:,i)+gam(:,i)*(gam(:,i))';  
    end 
    p_x_next=p_x_next(:,:,2*n+1)/(2*n)-x_next*x_next'+Q; 
 E=sqrtm(p_x_next); 
    for i=1:2*n 
        x_bar_new(:,i)=x_next+E*KS(:,i); 
    end 
%新测量样本; 
 for i=1:2*n 
    z_bar(i)=x_bar_new(1,i)+x_bar_new(2,i); 
 end 
 z_next_new=0; 
 for i=1:2*n 
     z_next_new=z_next_new+z_bar(i); 
 end 
 z_next(k)=z_next_new/(2*n); 
%测量方差 
 p_z(1)=0; 
 for i = 1:2*n 
 p_z(i+1)=p_z(i)+z_bar(i)*z_bar(i)'; 
 end 
 p_z=p_z(2*n+1)/(2*n)-z_next(k)*z_next(k)'+R; 
%状态与测量协方差 
 p_xz(:,1)=zeros(n,1); 
 for i = 1 : 2*n 
 p_xz(:,i+1)=p_xz(:,i)+x_bar_new(:,i)*z_bar(i)'; 
 end 
 p_xz=p_xz(:,2*n+1)/(2*n)-x_next*z_next(k)'; 
 %卡尔曼增益 
 K=p_xz*inv(p_z);    
 %新估计量 
 x_optimal(:,k)=x_next+K*(y(k)-z_next(k));%第一步的估计值 + 修正值; 
x_estimate=x_optimal(:,k); 
   %新方差 
p_x_next_update = p_x_next-K * p_z * K'; 
%     p_x_next_update(:,:,1)=zeros(n); 
%     for i = 1 :2*n+1 
%     p_x_next_update(:,:,i+1)=p_x_next_update(:,:,i) + Wc(i)* ((gam(:,i)-x_next)-(z_bar(i)-z_next(k))) * ((gam(:,i)-x_next)-(z_bar(i)-z_next(k)))'+Q+K*R*K'; 
%     end 
%     p_x_next_update= p_x_next_update(:,:,2*n+2); 
 P= p_x_next_update; 
   %进行画图程序  
    error1(k,:)=mse(x(1,k)-x_optimal(1,k)); 
    error2(k,:)=mse(x(2,k)-x_optimal(2,k)); 
end 
e1=mse(abs(x_optimal(1,:)-x(1,1:tf))); 
e2=mse(abs(x_optimal(2,:)-x(2,1:tf))); 
  figure 
   plot(1:tf,x(1,1:tf),'k-',1:tf,x_optimal(1,:),'r-'); 
   xlabel('Iterations'); 
   ylabel('the value of the first sate'); 
   legend('True','Estimate'); 
    figure 
   plot(1:tf,x(2,1:tf),'k-',1:tf,x_optimal(2,:),'r-'); 
   xlabel('Iterations'); 
   ylabel('the value of the second sate'); 
   legend('True','Estimate'); 
   figure 
     plot(1:tf,y(1:tf),'k-',1:tf,z_next(1:tf),'r-') 
      legend('输出真实值','输出估计值'); 
   disp(['状态1估计误差均方值=',num2str(e1)]); 
    disp(['状态2估计误差均方值=',num2str(e2)]); 
%     plot(t,error,'r-'); 
%     legend('UKF估计值误差'); 
    t = 1 : tf; 
    figure 
     plot(t,error2,'r-'); 
    set(gca,'FontSize',10); 
    set(gcf,'color','White'); 
   xlabel('Iterations');% lable --->label  
   ylabel('MSE of the second sate'); 
  legend('MSE'); 
%     toc; 
end