基于matlab实现的粒子滤波pf-3.rar

% =================================================================== %H为观测转移矩阵，Q为过程噪声协方差（表示噪声引起的加速度扰动），R为观测噪声协方差 %% clear all; clc; close all; warning off; tic; %% Initialization N=1000; % 粒子个数 T_MEA=80; % 测量时间 % T=1; % 观测时间间隔 H=[1 1 1]; % 观测矩阵 delta1=1; delta2=1; Q=0.0001; % 过程噪声协方差 R=0.0001; % 量测噪声协方差 V=0.0001.*diag([1 1 1]); % 粒子群初始化时的噪声协方差 x=zeros(3,T_MEA); % 存放真实状态 z=zeros(T_MEA); % 存放观测值 x(:,1)=[1 1 1]'; % 状态初始化 z(1,1)=x(1,1); % 观测初始化 w= zeros(1,N); % 存放粒子权值 x_out_est=[0;0;0]; % 存放PF输出估计状态 p.x=zeros(3,N,T_MEA); %存放PF采样值 T=0.25; Qf=1; Q0=1; VR=100; Cf = [0.5;0.05;0]; k1 = 0.5; k2 = 0.05; k3 = 0.2; k4 = 0.01; v=[-1,1,1;0,-2,1]'; for i=1:N p.x(:,i,1)=x(:,1)+sqrt(V)*randn(3,1); % 粒子群初始化，k=1时刻粒子群，N个粒子 end %% Update for k=2:T_MEA %% 更新：真实状态、量测值 r = [k1*x(1,k-1)-k2*x(2,k-1)*x(3,k-1);k3*x(2,k-1)^2-k4*x(3,k-1)]; x(1,k) = x(1,k-1) + T*(Qf/VR*Cf(1) - Q0/VR*x(1,k-1) + v(1,:)*r)+sqrt(Q)*randn(1,1) ; x(2,k) = x(2,k-1) + T*(Qf/VR*Cf(2) - Q0/VR*x(2,k-1) + v(2,:)*r)+sqrt(Q)*randn(1,1); x(3,k) = x(3,k-1) + T*(Qf/VR*Cf(3) - Q0/VR*x(3,k-1) + v(3,:)*r)+sqrt(Q)*randn(1,1); z(:,k)=H*x(:,k)+sqrt(R)*randn(1,1); end %% PF Algorithm for k=2:T_MEA x_out=[0;0;0]; for i=1:N r1 = [k1*p.x(1,i,k-1)-k2*p.x(2,i,k-1)*p.x(3,i,k-1);k3*p.x(2,i,k-1)^2-k4*p.x(3,i,k-1)]; p.x(1,i,k) = p.x(1,i,k-1) + T*(Qf/VR*Cf(1) - Q0/VR*p.x(1,i,k-1) + v(1,:)*r1)+sqrt(Q)*randn(1,1) ; p.x(2,i,k) = p.x(2,i,k-1) + T*(Qf/VR*Cf(2) - Q0/VR*p.x(2,i,k-1) + v(2,:)*r1)+sqrt(Q)*randn(1,1); p.x(3,i,k) = p.x(3,i,k-1) + T*(Qf/VR*Cf(3) - Q0/VR*p.x(3,i,k-1) + v(3,:)*r1)+sqrt(Q)*randn(1,1); p.z(:,i)=H*p.x(:,i,k); w(i)=1/(sqrt(2*pi*det(R)))*exp(-1/2*(z(:,k)-p.z(:,i))'*inv(R)*(z(:,k)-p.z(:,i)))+1e-99; % 权值加1e-99，是防止权值将为0 end wsum=sum(w,2); % 权值归一化 w=w/wsum; Neff=1/sum(w.^2); % 有效粒子个数 % Output for i=1:N x_out=x_out+p.x(:,i,k).*w(i); % 输出估计状态 end x_out_est=[x_out_est x_out]; % Resampling if Neff<(2/3*N) for i=1:N tmp=p.x; p.x(:,i,k)=tmp(:,find(rand<cumsum(w),1),k); end end end %% Error for i=1:T_MEA error_X1_pf(i)=x(1,i)-x_out_est(1,i); error_X2_pf(i)=x(2,i)-x_out_est(2,i); % 计算PF估计和真实状态的误差 error_X3_pf(i)=x(3,i)-x_out_est(3,i); end RMSE_error_1=RMSE(x(1,:),x_out_est(1,:),T_MEA); RMSE_error_2=RMSE(x(2,:),x_out_est(2,:),T_MEA) ; RMSE_error_3=RMSE(x(3,:),x_out_est(3,:),T_MEA); % 计算测量和真实状态的RMSE误差 fprintf('状态1均方根误差=%d\n', RMSE_error_1); fprintf('状态2均方根误差=%d\n', RMSE_error_2); fprintf('状态3均方根误差=%d\n', RMSE_error_3); t=1:1:T_MEA; %% Plot fid1=figure(1); plot(t,x(1,:),'go-',t,x_out_est(1,:),'k*-','MarkerFace','g'); xlabel('Time'); ylabel('x-1'); legend('Real-1','estamation-1'); title('state-1'); saveas(fid1,'.\state-1','fig'); fid2=figure(2); plot(t,x(2,:),'go-',t,x_out_est(2,:),'k*-','MarkerFace','g'); xlabel('Time'); ylabel('x-2'); legend('Real-2','estamation-2'); title('state-2'); saveas(fid1,'.\state-2','fig'); fid3=figure(3); plot(t,x(3,:),'go-',t,x_out_est(3,:),'k*-','MarkerFace','g'); xlabel('Time'); ylabel('x-3'); legend('Real-3','estamation-3'); title('state-3'); saveas(fid1,'.\state-3','fig'); fid4=figure(4); subplot(3,1,1); % plot(error_X_mea,'ko-','MarkerFace','g'); hold on; plot(error_X1_pf,'k*-','MarkerFace','r'); hold on; legend('Error of X1-axis PF'); xlabel('Time'); ylabel('Error'); title('Error of X1-axis'); subplot(3,1,2); % plot(error_Y_mea,'ko-','MarkerFace','g'); hold on; plot(error_X2_pf,'k*-','MarkerFace','r'); hold on; legend('Error of X2-axis PF'); xlabel('Time'); ylabel('Error'); title('Error of X2-axis'); subplot(3,1,3); plot(error_X3_pf,'k*-','MarkerFace','r'); hold on; legend('Error of X3-axis PF'); xlabel('Time'); ylabel('Error'); title('Error of X3-axis'); saveas(fid3,'.\Error Analysis','fig'); toc;