二维Kalman滤波

%% 本程序仿真2维运动的Kalman滤波 close all clear clc %% dt=1; N=100; t=(0:N-1)*dt; x0=80; y0=0; Vx=5; Vy=4; x_real=x0+Vx*t; y_real=y0+Vy*t; vx_real=Vx*ones(1,N); vy_real=Vy*ones(1,N); X_real=[x_real;vx_real;y_real;vy_real]; sigma_x=0.1; sigma_vx=0.0; sigma_y=0.1; sigma_vy=0.0; Q=diag([sigma_x,sigma_vx,sigma_y,sigma_vy]); % 状态转移矩阵 A=[1 dt 0 0; 0 1 0 0; 0 0 1 dt; 0 0 0 1]; H=[1 0 0 0; 0 0 1 0]; %% 测量结果 sigma_x_m=3; sigma_y_m=3; R=diag([sigma_x_m,sigma_y_m]); x_mea=x_real+sigma_x_m*randn(1,N); y_mea=y_real+sigma_y_m*randn(1,N); X_mea=[x_mea;y_mea]; X_est=zeros(4,N); X_pre=zeros(4,N); % X_est0=[x0,Vx,y0,Vy]'; X_est0=[x0,0,y0,0]'; X_est(:,1)=X_est0; % 跟踪的初始状态 % P=zeros(4); P=diag([2 1 2 3]); I=eye(4); for k=2:N X_pre(:,k)=A*X_est(:,k-1); % 状态转移过程应该加入噪声??? P_pre=A*P*A'+Q; % 测量更新过程 K=P_pre*H'*inv(H*P_pre*H'+R); X_est(:,k) = X_pre(:,k)+K*(X_mea(:,k)-H*X_pre(:,k)); P=(I-K*H)*P_pre; end figure hold on plot(X_real(1,:),'b') plot(X_mea(1,:),'k') plot(X_est(1,:),'r') legend('Real','Measure','Estimate') title('X coordinate') figure hold on plot(X_real(3,:),'b') plot(X_mea(2,:),'k') plot(X_est(3,:),'r') legend('Real','Measure','Estimate') title('Y coordinate') figure hold on plot(X_real(1,:),X_real(3,:),'b') plot(X_mea(1,:),X_mea(2,:),'k') plot(X_est(1,:),X_est(3,:),'r') legend('Real','Measure','Estimate') title('Kalman tracking in 2D plane') xlabel('x') ylabel('y') % % % for i=2:L % x_mea=Z(:,i); % [x_f,P_f]=fun_kalman_1D(x_pre,P_pre,x_mea,A,Q,H,R,E); % X(:,i)=x_f; % P(:,:,i)=P_f; % % x_pre=x_f; % P_pre=P_f; % end % % % figure % plot(x_real,'k') % hold on % plot(Z(1,:),'bo') % hold on % plot(X(1,:),'r+') % legend('Expected','Measure','Filter') % % % figure % plot(vx*ones(1,L),'k') % hold on % plot(vx_mea,'bo') % hold on % plot(X(2,:),'r+') % legend('expectation value','measurement value','filter output','location','northwest') % % % % function [x_f,P]=fun_kalman_1D(x_pre,P_pre,x_mea,A,Q,H,R,E) % % % % x_t=A*x_pre; % % P_t=A*P_pre*A'+Q; % % G=P_t*H'*inv(R+H*P_t*H'); % % x_f=x_t+G*(x_mea-H*x_t); % % P=(E-G*H)*P_t; % % end